Python msg.Marker方法代碼示例

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import Marker [as 別名]
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) 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import Marker [as 別名]
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_) 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import Marker [as 別名]
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 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import Marker [as 別名]
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) 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import Marker [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 Marker [as 別名]
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 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import Marker [as 別名]
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) 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import Marker [as 別名]
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) 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import Marker [as 別名]
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) 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import Marker [as 別名]
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)] 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import Marker [as 別名]
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) 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import Marker [as 別名]
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) 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import Marker [as 別名]
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 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import Marker [as 別名]
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() 

# 需要導入模塊: from visualization_msgs import msg [as 別名]
# 或者: from visualization_msgs.msg import Marker [as 別名]
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