Python visualization_msgs.msg.Marker() Examples

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) 

def _append_waypoint_pose(self,pose,create_heading=False):
        if (self.marker_idx > self.max_markers):
            rospy.logwarn("You have exceeded the maximum number of allowed waypoints.....")
            return
        self.waypoints.append([create_heading,pose])
        marker = Marker()
        marker.header.frame_id = self.global_frame
        marker.id = self.marker_idx
        marker.type = 2
        marker.action = 0
        marker.pose = pose
        marker.color.r = 1.0
        marker.color.g = 0.0
        marker.color.b = 0.0
        marker.color.a = 1.0
        marker.scale.x = 0.1
        marker.scale.y = 0.1
        marker.scale.z = 0.1
        marker.frame_locked = False
        marker.ns = "Goal-%u"%self.marker_idx
        self.marker_array_msg.markers[self.marker_idx] = marker
        self.marker_idx+=1 

def test_marker(self):
        """
        Tests marker
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message("/carla/marker", Marker, timeout=TIMEOUT)
        self.assertEqual(msg.header.frame_id, "ego_vehicle")
        self.assertNotEqual(msg.id, 0)
        self.assertEqual(msg.type, 1)
        self.assertNotEqual(msg.pose, Pose())
        self.assertNotEqual(msg.scale, Vector3())
        self.assertEqual(msg.color.r, 0.0)
        self.assertEqual(msg.color.g, 255.0)
        self.assertEqual(msg.color.b, 0.0) 

def display_ik_goal(pos_goal, quat_goal):
    global tf_broadcaster, marker_pub
    quat_mat = T.quaternion_matrix(quat_goal)
    quat_goal = T.quaternion_from_matrix(quat_mat)
    tf_broadcaster.sendTransform(tuple(pos_goal), (quat_goal[1],quat_goal[2],quat_goal[3],quat_goal[0]), rospy.Time.now(), 'ik_goal', 'common_world')
    marker = Marker()
    marker.header.frame_id = 'common_world'
    marker.id = 100
    marker.type = marker.SPHERE
    marker.action = marker.ADD
    marker.pose.position.x = pos_goal[0]
    marker.pose.position.y = pos_goal[1]
    marker.pose.position.z = pos_goal[2]
    marker.scale.x = .05
    marker.scale.y = .05
    marker.scale.z = .05
    marker.color.a = 1
    marker.color.r = 0.0
    marker.color.g = 0.9
    marker.color.b = 0.2
    marker_pub.publish(marker) 

def getMarkerWindow(x,y,z,r,p,yaw):

	myMarker = Marker()
	myMarker.header.frame_id = "world"
	myMarker.header.seq = 1
	myMarker.header.stamp    = rospy.get_rostime()
	myMarker.ns = "window"
	myMarker.id = 1
	myMarker.type = myMarker.MESH_RESOURCE # sphere
	myMarker.action = myMarker.ADD
	myMarker.pose.position.x = x
	myMarker.pose.position.y = y
	myMarker.pose.position.z = z
	q = quaternion_from_euler(r, p, yaw)
	myMarker.pose.orientation.x=q[0]
	myMarker.pose.orientation.y=q[1]
	myMarker.pose.orientation.z=q[2]
	myMarker.pose.orientation.w=q[3]
	myMarker.mesh_resource = "package://project/models/window_buena.stl";
	myMarker.color=ColorRGBA(0, 1, 0, 1)
	myMarker.scale.x = 5;
	myMarker.scale.y = 5;
	myMarker.scale.z = 6;

	return myMarker 

def show_marker(marker_array_, pos_, ori_, scale_, color_, lifetime_):
    marker_ = Marker()
    marker_.header.frame_id = "/table_top"
    # marker_.header.stamp = rospy.Time.now()
    marker_.type = marker_.CUBE
    marker_.action = marker_.ADD

    marker_.pose.position.x = pos_[0]
    marker_.pose.position.y = pos_[1]
    marker_.pose.position.z = pos_[2]
    marker_.pose.orientation.x = ori_[1]
    marker_.pose.orientation.y = ori_[2]
    marker_.pose.orientation.z = ori_[3]
    marker_.pose.orientation.w = ori_[0]

    marker_.lifetime = rospy.Duration.from_sec(lifetime_)
    marker_.scale.x = scale_[0]
    marker_.scale.y = scale_[1]
    marker_.scale.z = scale_[2]
    marker_.color.a = 0.5
    red_, green_, blue_ = color_
    marker_.color.r = red_
    marker_.color.g = green_
    marker_.color.b = blue_
    marker_array_.markers.append(marker_) 

def make_label(text, position, id = 0 ,duration = 5.0, color=[1.0,1.0,1.0]):
    """ Helper function for generating visualization markers.

        Args:
            text (str): Text string to be displayed.
            position (list): List containing [x,y,z] positions
            id (int): Integer identifying the label
            duration (rospy.Duration): How long the label will be displayed for
            color (list): List of label color floats from 0 to 1 [r,g,b]

        Returns:
            Marker: A text view marker which can be published to RViz
    """
    marker = Marker()
    marker.header.frame_id = '/world'
    marker.id = id
    marker.type = marker.TEXT_VIEW_FACING
    marker.text = text
    marker.action = marker.ADD
    marker.scale.x = 0.05
    marker.scale.y = 0.05
    marker.scale.z = 0.05
    marker.color.a = 1.0
    marker.color.r = color[0]
    marker.color.g = color[1]
    marker.color.b = color[2]
    marker.lifetime = rospy.Duration(duration)
    marker.pose.orientation.w = 1.0
    marker.pose.position.x = position[0]
    marker.pose.position.y = position[1]
    marker.pose.position.z = position[2]
    return marker 

def make_rviz_marker_super(self):
        self.marker = Marker()
        self.marker.header.frame_id = 'common_world'
        self.marker.header.stamp = rospy.Time()
        self.marker.id = self.id
        self.marker.color.a = 0.4
        self.marker.color.g = 1.0
        self.marker.color.b = 0.7
        self.marker.text = self.name 

def __init__(self, collision_dict):
        self.name = collision_dict['name']
        self.coordinate_frame = collision_dict['coordinate_frame']
        self.rotation = collision_dict['rotation']
        rx, ry, rz = self.rotation[0], self.rotation[1], self.rotation[2]
        self.quaternion = T.quaternion_from_euler(rx, ry, rz)
        self.translation = collision_dict['translation']
        self.params = collision_dict['parameters']
        self.id = 0
        self.type = ''
        self.pub = rospy.Publisher('visualization_marker',Marker,queue_size=5)
        self.make_rviz_marker_super() 

def ros_pub_marker(loc,color = "green"):
    val = 0.1
    if (color == "green"):
        val = 0.9

    # print(loc[3])
    quat = quaternion_from_euler(0, 0, loc[3])
    marker = Marker()
    marker.header.frame_id = "/zoe/base_link"
    # marker.header.frame_id = "/zoe/base_link"
    marker.type = marker.CUBE
    marker.action = marker.ADD

    marker.scale.x = 2.5
    marker.scale.y = 5
    marker.scale.z = 2
    if (len(loc) > 4):
        marker.scale.x = loc[5]
        marker.scale.y = loc[6]
        marker.scale.z = loc[4]

    marker.color.a = 0.4
    marker.color.r = 1 - val
    marker.color.g = val
    marker.color.b = 0.2
    marker.pose.orientation.x = quat[0]
    marker.pose.orientation.y = quat[1]
    marker.pose.orientation.z = quat[2]
    marker.pose.orientation.w = quat[3]
    marker.pose.position.x = loc[0]
    marker.pose.position.y = loc[1]
    marker.pose.position.z = loc[2]
    # marker.lifetime = rospy.Duration(0.3)
    marker_pub.publish(marker)
    # rospy.sleep(0.008)
    # # while (True):
    # for i in range(2):
    #     marker_pub.publish(marker)
    # # rospy.sleep(0.1) 

def show_reward(self, reward):
        """
        Publishing reward value as marker.
        :param reward
        """
        # Publish reward as Marker
        msg = Marker()
        msg.header.frame_id = "/base_footprint"
        msg.ns = ""
        msg.id = 0
        msg.type = msg.TEXT_VIEW_FACING
        msg.action = msg.ADD

        msg.pose.position.x = 0.0
        msg.pose.position.y = 1.0
        msg.pose.position.z = 0.0
        msg.pose.orientation.x = 0.0
        msg.pose.orientation.y = 0.0
        msg.pose.orientation.z = 0.0
        msg.pose.orientation.w = 1.0

        msg.text = "%f"%reward

        msg.scale.x = 10.0
        msg.scale.y = 10.0
        msg.scale.z = 1.0

        msg.color.r = 0.3
        msg.color.g = 0.4
        msg.color.b = 1.0
        msg.color.a = 1.0
        self.__rew_pub.publish(msg)

        # Publish reward as number
        msg = Float64()
        msg.data = reward
        self. __rew_num_pub.publish(msg) 

def __init__(self, ns, stack_offset):
        self.__ns = ns
        self.__stack_offset = stack_offset
        print("stack_offset: %d"%self.__stack_offset)
        self.__input_images = deque(maxlen=4 * self.__stack_offset)

        #Input state
        self.__input_img_pub1 = rospy.Publisher('%s/state_image1' % (self.__ns), Image, queue_size=1)
        self.__input_img_pub2 = rospy.Publisher('%s/state_image2' % (self.__ns), Image, queue_size=1)
        self.__input_img_pub3 = rospy.Publisher('%s/state_image3' % (self.__ns), Image, queue_size=1)
        self.__input_img_pub4 = rospy.Publisher('%s/state_image4' % (self.__ns), Image, queue_size=1)
        self.__occ_grid_pub = rospy.Publisher('%s/rl_map' % (self.__ns), OccupancyGrid, queue_size=1)
        self.__input_scan_pub = rospy.Publisher('%s/state_scan' % (self.__ns), LaserScan, queue_size=1)

        # reward info
        self.__rew_pub = rospy.Publisher('%s/reward' % (self.__ns), Marker, queue_size=1)
        self.__rew_num_pub = rospy.Publisher('%s/reward_num' % (self.__ns), Float64, queue_size=1)

        # Waypoint info
        self.__wp_pub1 = rospy.Publisher('%s/wp_vis1' % (self.__ns), PointStamped, queue_size=1)
        self.__wp_pub2 = rospy.Publisher('%s/wp_vis2' % (self.__ns), PointStamped, queue_size=1)
        self.__wp_pub3 = rospy.Publisher('%s/wp_vis3' % (self.__ns), PointStamped, queue_size=1)
        self.__wp_pub4 = rospy.Publisher('%s/wp_vis4' % (self.__ns), PointStamped, queue_size=1) 

def ros_pub_marker(loc):
    marker_pub = rospy.Publisher("/marker_topic", Marker, queue_size=10)
    # print(loc[3])
    quat = quaternion_from_euler(0, 0, loc[3])
    marker = Marker()
    marker.header.frame_id = "zoe/base_link"
    marker.type = marker.CUBE
    marker.action = marker.ADD
    marker.scale.x = 2.5
    marker.scale.y = 5
    marker.scale.z = 2
    marker.color.a = 0.4
    marker.color.r = 0.9
    marker.color.g = 0.1
    marker.color.b = 0.2
    marker.pose.orientation.x = quat[0]
    marker.pose.orientation.y = quat[1]
    marker.pose.orientation.z = quat[2]
    marker.pose.orientation.w = quat[3]
    marker.pose.position.x = loc[0]
    marker.pose.position.y = loc[1]
    marker.pose.position.z = loc[2]
    # marker.lifetime = rospy.Duration(1)
    rospy.sleep(0.2)
    # while (True):
    for i in range(5):
        marker_pub.publish(marker)
    # rospy.sleep(0.1) 

def rosMarker(loc,idx,color = "green"):
    val = 0.1
    if (color == "green"):
        val = 0.9

    # print(loc[3])
    quat = quaternion_from_euler(0, 0, loc[3])
    marker = Marker()
    # marker.header.frame_id = "map"
    marker.header.frame_id = "/zoe/base_link"
    marker.type = marker.CUBE
    marker.action = marker.ADD
    marker.scale.x = 2.5
    marker.scale.y = 5
    marker.scale.z = 2
    marker.color.a = 0.4
    marker.color.r = 1 - val
    marker.color.g = val
    marker.color.b = 0.2
    marker.pose.orientation.x = quat[0]
    marker.pose.orientation.y = quat[1]
    marker.pose.orientation.z = quat[2]
    marker.pose.orientation.w = quat[3]
    marker.pose.position.x = loc[0]
    marker.pose.position.y = loc[1]
    marker.pose.position.z = loc[2]
    # marker.lifetime = rospy.Duration(0.3)
    marker.id = idx
    return marker




# # xmin,ymin,zmin,smin,xmax,ymax,zmax,smax
# regions =  [(0, -2, -3, 0, 10, 8, 0,0),
#             (0, -8, -3, 0, 10, 2, 0,0),
#             (10, -2, -3, 0, 20, 8, 0,0),
#             (10, -8, -3, 0, 20, 2, 0,0)] 

def ros_pub_marker(loc):
    # print(loc[3])
    quat = quaternion_from_euler(0, 0, loc[3])
    marker = Marker()
    marker.header.frame_id = "/zoe/base_link"
    marker.type = marker.CUBE
    marker.action = marker.ADD
    marker.scale.x = 2.5
    marker.scale.y = 5
    marker.scale.z = 2
    marker.color.a = 0.4
    marker.color.r = 0.9
    marker.color.g = 0.1
    marker.color.b = 0.2
    marker.pose.orientation.x = quat[0]
    marker.pose.orientation.y = quat[1]
    marker.pose.orientation.z = quat[2]
    marker.pose.orientation.w = quat[3]
    marker.pose.position.x = loc[0]
    marker.pose.position.y = loc[1]
    marker.pose.position.z = loc[2]
    # marker.lifetime = rospy.Duration(1)
    rospy.sleep(0.2)
    # while (True):
    for i in range(5):
        marker_pub.publish(marker)
    # rospy.sleep(0.1) 

def rosMarker(loc,idx,color = "green",dur=0.2):
    val = 0.1
    if (color == "green"):
        val = 0.9

    # print(loc[3])
    quat = quaternion_from_euler(0, 0, loc[3])
    marker = Marker()
    # marker.header.frame_id = "map"
    marker.header.frame_id = "/zoe/base_link"
    marker.type = marker.CUBE
    marker.action = marker.ADD

    marker.scale.x = 2.5
    marker.scale.y = 5
    marker.scale.z = 2
    if (len(loc) > 4):
        marker.scale.x = loc[5]
        marker.scale.y = loc[6]
        marker.scale.z = loc[4]

    marker.color.a = 0.4
    marker.color.r = 1 - val
    marker.color.g = val
    marker.color.b = 0.2
    marker.pose.orientation.x = quat[0]
    marker.pose.orientation.y = quat[1]
    marker.pose.orientation.z = quat[2]
    marker.pose.orientation.w = quat[3]
    marker.pose.position.x = loc[0]
    marker.pose.position.y = loc[1]
    marker.pose.position.z = loc[2]
    marker.lifetime = rospy.Duration(dur)
    marker.id = idx
    return marker
    # rospy.sleep(0.008)
    # # while (True):
    # for i in range(2):
    #     marker_pub.publish(marker)
    # # rospy.sleep(0.1) 

def __debug_loop(self):
        """
        Debug loop that will run in a separate thread.
        (tfBuffer should be threadsafe)
        """
        broadcaster = tf2_ros.TransformBroadcaster()
        rviz_marker_pub = rospy.Publisher('/visualization_marker', Marker,
                                          queue_size=1000)
        rate = rospy.Rate(5)
        while not self.__debug_stop_event.is_set() and not rospy.is_shutdown():
            if self.__debug_current_ws is None:
                print "Could not publish debug tf, no workspace set."
                rate.sleep()
                continue

            try:
                broadcaster.sendTransform(
                    self.__tf_buffer.lookup_transform(
                        "base_link", self.__debug_current_ws.name,
                        rospy.Time(0))
                )
                broadcaster.sendTransform(
                    self.__tf_buffer.lookup_transform(
                        self.__debug_current_ws.name, "object_base",
                        rospy.Time(0))
                )
                broadcaster.sendTransform(
                    self.__tf_buffer.lookup_transform(
                        "object_base", "tool_link_target", rospy.Time(0))
                )
            except tf2_ros.LookupException as e:
                print "Could not publish debug tf: ", e

            for i in range(4):  # Iterate over the 4 markers defining the workspace
                msg = Marker()
                msg.header.frame_id = "base_link"
                msg.id = i
                msg.type = 2  # It correspond to a sphere which will be drawn

                msg.pose.position.x = self.__debug_current_ws.points[i][0]
                msg.pose.position.y = self.__debug_current_ws.points[i][1]
                msg.pose.position.z = self.__debug_current_ws.points[i][2]

                msg.scale.x = 0.005
                msg.scale.y = 0.005
                msg.scale.z = 0.005

                msg.color.r = 1.0 if i == 0 or i == 3 else 0.0
                msg.color.g = 1.0 if i == 1 or i == 3 else 0.0
                msg.color.b = 1.0 if i == 2 or i == 3 else 0.0
                msg.color.a = 1.0

                rviz_marker_pub.publish(msg)

        rate.sleep() 

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)