Python msg.MarkerArray方法代碼示例

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import MarkerArray [as 別名]
def callback(data):
    global marker_ests
    #Publish it as a marker in rviz
    marker_ests = MarkerArray()
    marker_ests.markers = []
    print len(data.poses)
    i = 0
    for pose in data.poses:
        marker_est = Marker()
        marker_est.header.frame_id = "base"
        marker_est.ns = "est_pose_"+str(i)
        marker_est.id = 42+i
        marker_est.type = Marker.CUBE
        marker_est.action = Marker.ADD
        marker_est.pose = pose
        marker_est.color.r, marker_est.color.g, marker_est.color.b = (0, 255, 0)
        marker_est.color.a = 0.5
        marker_est.scale.x, marker_est.scale.y, marker_est.scale.z = (0.06, 0.06, 0.06)
        marker_ests.markers.append(marker_est)
        i+=1 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import MarkerArray [as 別名]
def __init__(self):
        self.map = None
        self.start = None
        self.goal = None

        self.moves = [Move(0.1, 0),  # forward
                      Move(-0.1, 0),  # back
                      Move(0, 1.5708),  # turn left 90
                      Move(0, -1.5708)] # turn right 90
        self.robot = Robot(0.5, 0.5)
        self.is_working = False # Replace with mutex after all

        self.map_subscriber = rospy.Subscriber("map", OccupancyGrid, self.new_map_callback)
        self.start_subscriber = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.new_start_callback)
        self.goal_subscriber = rospy.Subscriber("goal", PoseStamped, self.new_goal_callback)

        self.path_publisher = rospy.Publisher("trajectory", MarkerArray, queue_size=1)
        self.pose_publisher = rospy.Publisher("debug_pose", PoseStamped, queue_size=1)

        # what will be there. A module goes into variable. Isn't it too much memory consumption. Maybe I should assign function replan() to this variable?
        self.planner = planners.astar.replan 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import MarkerArray [as 別名]
def _init_markers(self):
        self.marker_idx = 0
        self.marker_array_msg = MarkerArray()
        for i in range(self.max_markers):
            marker = Marker()
            marker.header.frame_id = self.global_frame
            marker.id = self.marker_idx
            marker.type = 2
            marker.action = 2
            marker.pose = Pose()
            marker.color.r = 0.0
            marker.color.g = 0.0
            marker.color.b = 0.0
            marker.color.a = 0.0
            marker.scale.x = 0.1
            marker.scale.y = 0.1
            marker.scale.z = 0.1
            marker.frame_locked = False
            marker.ns = "Goal-%u"%i
            self.marker_array_msg.markers.append(marker) 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import MarkerArray [as 別名]
def kitti_custom_data_visualise(data_dir):
    markerArray = MarkerArray()
    files = os.listdir(data_dir + "velodyne_crop/")
    print(len(files))
    for crop_num in range(len(files)):
        crop_path = data_dir + "velodyne_crop/"+ "%06d.npy" % crop_num
        points = np.load(crop_path)
        N = points.shape[0] # Num of points in PointCloud
        color = np.array([1,1,1])
        color = np.vstack([color]*N)
        print(N)
        ros_pub(points, "/all_points", color)


        labels_path = data_dir + "labels/" + "%06d.txt" % crop_num
        labels = np.loadtxt(labels_path, delimiter=' ',ndmin=2)
        for x in range(len(labels)):
            loc = labels[x]
            if(loc[8] == 1):
                marker = rosMarker(loc,x, "green")
            else:
                loc[:4] = np.array([0, 8, 1.5, 3.14/2.0])
                marker = rosMarker(loc,x, "red")

            markerArray.markers.append(marker)
            # ros_pub_marker(labels[x])
            # pdb.set_trace()
        markerArray_pub.publish(markerArray)
        pdb.set_trace() 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import MarkerArray [as 別名]
def __init__(self, state_collector, ns, robot_radius = 0,robot_width = 0.58, robot_height = 0.89):
        self.__robot_radius = robot_radius                              # robot radius
        self.__robot_height = robot_height                              # robot width
        self.__robot_width = robot_width                                # robot heigth
        self.__odom = Odometry()                                        # most recently published odometry of the robot
        self.__path = Path()                                            # most recently published path
        self.__done = False                                             # is episode done?
        self.__new_task_started = False                                 # has a new task started?
        self.__state_collector = state_collector                        # for getting relevant state values of the robot
        self.__rl_agent_path = rospkg.RosPack().get_path('rl_agent')    # absolute path rl_agent-package
        self.__flatland_topics = []                                     # list of flatland topics
        self.__timestep = 0                                             # actual timestemp of training
        self.__NS = ns
        self.MODE = rospy.get_param("%s/rl_agent/train_mode", 1)
        self.__clock = Clock().clock
        self.__task_generator = TaskGenerator(self.__NS, self.__state_collector, 0.46)
        self.__recent_agent_states = []

        # Subscriber for getting data
        #self.__odom_sub = rospy.Subscriber("%s/odom"%self.__NS, Odometry, self.__odom_callback, queue_size=1)
        self.__global_plan_sub = rospy.Subscriber("%s/move_base/NavfnROS/plan"%self.__NS, Path, self.__path_callback, queue_size=1)
        # self.__path_sub = rospy.Subscriber("%s/move_base/GlobalPlanner/plan" % self.__NS, Path, self.__path_callback,  queue_size=1)
        self.__done_sub = rospy.Subscriber("%s/rl_agent/done"%self.__NS, Bool, self.__done_callback, queue_size=1)
        self.__new_task_sub = rospy.Subscriber('%s/rl_agent/new_task_started'%self.__NS, Bool, self.__new_task_callback, queue_size=1)
        self.__flatland_topics_sub = rospy.Subscriber("%s/flatland_server/debug/topics"%self.__NS, DebugTopicList, self.__flatland_topic_callback, queue_size=1)
        self.__agent_action_sub = rospy.Subscriber('%s/rl_agent/action'%self.__NS, Twist, self.__trigger_callback)
        self.__ped_sub = rospy.Subscriber('%s/pedsim_simulator/simulated_agents' % self.__NS, AgentStates, self.__ped_callback)
        if self.MODE == 1 or self.MODE == 0:
            self.clock_sub = rospy.Subscriber('%s/clock' % self.__NS, Clock, self.__clock_callback)

        # Publisher for generating qualitative image
        self.__driven_path_pub = rospy.Publisher('%s/rl_eval/driven_path'%self.__NS, Path, queue_size=1)
        self.__driven_path_pub2 = rospy.Publisher('%s/rl_eval/driven_path2'%self.__NS, Path, queue_size=1)
        self.__global_path_pub = rospy.Publisher('%s/rl_eval/global_path'%self.__NS, Path, queue_size=1)
        self.__agent_pub = rospy.Publisher('%s/rl_eval/viz_agents'%self.__NS, MarkerArray, queue_size=1) 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import MarkerArray [as 別名]
def __init__(self):
        
        #self.sub_state = rospy.Subscriber('vicon', ViconState, self.state_cb)


        self.srv_flip = rospy.Service('window', Trigger, self.window_cb)
        self.srv_flip = rospy.Service('line', Trigger, self.line_cb)
        self.srv_flip = rospy.Service('flip_pitch', Trigger, self.flip_pitch_cb)
        self.srv_flip = rospy.Service('flip', Trigger, self.flip_cb)
        self.srv_flip = rospy.Service('flip_trans', Trigger, self.flip_trans_cb)

        #self.srv_takeoff = rospy.Service('takeoff', Trigger, self.takeoff_cb)

        # outer loop setpoints
        #self.pub_goal = rospy.Publisher('goal', QuadGoal, queue_size=1)

        self.pub_drone_markers=rospy.Publisher('snapshots', MarkerArray, queue_size=10)
        self.pub_window=rospy.Publisher('window', Marker, queue_size=10)

        # initialize members
        #self.state_msg = ViconState()

        # desired control rate
        self.dc = 0.01

        # motor spinup time, seconds
        self.spinup_secs = 0 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import MarkerArray [as 別名]
def GetLabeledArray(demo,labels,used=None):
    
    msg = MarkerArray()

    #if not used == None:
    #    r = np.random.rand(len(used))
    #    g = np.random.rand(len(used))
    #    b = np.random.rand(len(used))

    for i in range(len(labels)):
        marker = Marker()
        marker.header.frame_id = demo.base_link
        pt = demo.GetForward(demo.joint_states[i].position) * PyKDL.Frame(PyKDL.Rotation.RotY(-1*np.pi/2))
        marker.pose = pm.toMsg(pt)

        marker.color.a = 1.0
        if used == None:
            marker.color.r = marker.color.g = marker.color.b = 1.0
        else:
            idx = used.index(labels[i])
            marker.color.r = r[idx]
            marker.color.g = g[idx]
            marker.color.b = b[idx]

        marker.id = i
        marker.scale.x = 0.015
        marker.scale.y = 0.0025
        marker.scale.z = 0.0025
        marker.type = Marker.ARROW

        msg.markers.append(marker)

    return msg 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import MarkerArray [as 別名]
def __init__(self):
        # ArUco data -- we're using 6x6 ArUco images
        self._aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
        self._aruco_parameters = cv2.aruco.DetectorParameters_create()

        # Initialize ROS
        rospy.init_node('detect_aruco_node', anonymous=False)

        # ROS publishers
        self._image_pub = rospy.Publisher('image_marked', Image, queue_size=10)
        self._rviz_markers_pub = rospy.Publisher('rviz_markers', MarkerArray, queue_size=10)

        # ROS OpenCV bridge
        self._cv_bridge = CvBridge()

        # ROS transform managers
        self._tf_listener = tf.TransformListener()
        self._tf_broadcaster = tf.TransformBroadcaster()

        # Get base_link => camera_frame transform
        self._tf_listener.waitForTransform("base_link", "camera_frame", rospy.Time(), rospy.Duration(4))
        self._Tcb = tf_to_matrix(self._tf_listener.lookupTransform("base_link", "camera_frame", rospy.Time()))

        # Now that we're initialized, set up subscriptions and spin
        rospy.Subscriber("image_raw", Image, self.image_callback)
        rospy.spin() 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import MarkerArray [as 別名]
def restore_path(self, final_state):
        current_state = copy.copy(final_state)
        path = MarkerArray()
        pose_id = 0
        while True:
            pose_marker = current_state.to_marker(self.robot)
            pose_marker.id = pose_id
            path.markers.append(pose_marker)

            current_state = current_state.parent
            pose_id += 1

            if current_state is None:
                break
        return path 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import MarkerArray [as 別名]
def __init__(self, carla_debug_helper):
        """
        Constructor

        :param carla_debug_helper: carla debug helper
        :type carla_debug_helper: carla.DebugHelper
        """
        self.debug = carla_debug_helper
        self.marker_subscriber = rospy.Subscriber(
            "/carla/debug_marker", MarkerArray, self.on_marker) 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import MarkerArray [as 別名]
def navigate(self):
        if not hasattr(self, 'map_to_N'):
            print ('generating maps')
            kernel = np.ones((3,3),np.uint8)
            navi_map = cv2.dilate(self.map_for_LM, kernel, iterations=self.cr_pixels+1)
            if self.args.figure:
                self.ax_map.imshow(navi_map, alpha=0.3)

            self.map_to_N, self.map_to_E, self.map_to_S, self.map_to_W = generate_four_maps(navi_map, self.grid_rows, self.grid_cols)
            
        bel_cell = Cell(self.bel_grid.row, self.bel_grid.col)
        print (self.bel_grid)
        self.target_cell = Cell(self.args.navigate_to[0],self.args.navigate_to[1])
        distance_map = compute_shortest(self.map_to_N,self.map_to_E,self.map_to_S,self.map_to_W, bel_cell, self.target_cell, self.grid_rows)
        print (distance_map)
        shortest_path = give_me_path(distance_map, bel_cell, self.target_cell, self.grid_rows)
        action_list = give_me_actions(shortest_path, self.bel_grid.head)
        self.action_from_policy = action_list[0]
        print ('actions', action_list)
        if self.next_action is None:
            self.action_str = self.action_space[self.action_from_policy]
        else:
            self.action_from_policy = self.next_action
            self.action_str = self.action_space[self.next_action]
            self.next_action = None
            
        if self.action_str == 'go_fwd' and  self.collision_fnc(0, 0, 0, self.scan_2d_slide):
            self.action_from_policy = np.random.randint(2)
            self.action_str = self.action_space[self.action_from_policy]
            self.next_action = 2
        else:
            self.next_action = None
            
        if self.action_str == "hold":
            self.skip_to_end = True
            self.step_count = self.step_max -1

        markerArray = MarkerArray()
        for via_id, via in enumerate(shortest_path):
            marker = Marker()
            marker.header.frame_id = "map"
            marker.type = marker.SPHERE
            marker.action = marker.ADD
            marker.scale.x = 0.2
            marker.scale.y = 0.2
            marker.scale.z = 0.2
            marker.color.a = 1.0
            marker.color.r = 1.0
            marker.color.g = 1.0
            marker.color.b = 1.0        
            marker.pose.orientation.w = 1.0

            marker.pose.position.x = - to_real(via.y, self.ylim, self.grid_cols)
            marker.pose.position.y = to_real(via.x, self.xlim, self.grid_rows)
            
            marker.pose.position.z = 0        
            marker.id = via_id
            markerArray.markers.append(marker)
            
        self.pub_dalpath.publish(markerArray)