Python Marker.header方法代码示例

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import header [as 别名]
def publish(anns, data):
    wall_list = WallList()
    marker_list = MarkerArray()    

    marker_id = 1
    for a, d in zip(anns, data):
        
        # Walls
        object = deserialize_msg(d.data, Wall)
        wall_list.obstacles.append(object)
        
        # Markers
        marker = Marker()
        marker.id = marker_id
        marker.header = a.pose.header
        marker.type = a.shape
        marker.ns = "wall_obstacles"
        marker.action = Marker.ADD
        marker.lifetime = rospy.Duration.from_sec(0)
        marker.pose = copy.deepcopy(a.pose.pose.pose)
        marker.scale = a.size
        marker.color = a.color

        marker_list.markers.append(marker)

        marker_id = marker_id + 1

    marker_pub = rospy.Publisher('wall_marker',    MarkerArray, latch=True, queue_size=1)
    wall_pub   = rospy.Publisher('wall_pose_list', WallList,    latch=True, queue_size=1)

    wall_pub.publish(wall_list)
    marker_pub.publish(marker_list)
    
    return

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import header [as 别名]
 def draw_curve(
     self,
     pose_array,
     id=None,
     rgba=(0, 1, 0, 1),
     width=.01,
     ns='default_ns',
     duration=0,
     type=Marker.LINE_STRIP,
     ):
     if id is None:
         id = self.get_unused_id()
     marker = Marker(type=type, action=Marker.ADD)
     marker.header = pose_array.header
     marker.points = [pose.position for pose in pose_array.poses]
     marker.lifetime = rospy.Duration(0)
     if isinstance(rgba, list):
         assert len(rgba) == len(pose_array.poses)
         marker.colors = [stdm.ColorRGBA(*c) for c in rgba]
     else:
         marker.color = stdm.ColorRGBA(*rgba)
     marker.scale = gm.Vector3(width, width, width)
     marker.id = id
     marker.ns = ns
     self.pub.publish(marker)
     self.ids.add(id)
     return MarkerHandle(marker, self.pub)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import header [as 别名]
    def makeMarker(self, id_num, point, rgb=(1.0,1.0,1.0)):
        ## Make a std_msgs/Marker object
        #
        # @param id_num The id number corresponding to the marker. Note that these must be unique.
        # @param point An (x,y,z) tuple where the marker should be located in space
        # @param rgb A tuple corresponding to the RGB values on a scale from 0.0 to 1.0
        # 
        # @return A std_msgs/Marker object

        marker = Marker()
        marker.header = Header()
        marker.header.stamp = rospy.Time.now()
        marker.header.frame_id = self.frame_id
        marker.id = id_num
        marker.type = marker.SPHERE
        marker.action = marker.ADD
        marker.scale.x = 0.02 
        marker.scale.y = 0.02
        marker.scale.z = 0.02
        marker.color.r = rgb[0]
        marker.color.g = rgb[1] 
        marker.color.b = rgb[2]
        marker.color.a = 1.0
        marker.pose.orientation.w = 1.0
        marker.pose.position.x = point[0]
        marker.pose.position.y = point[1]
        marker.pose.position.z = point[2]
        return marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import header [as 别名]
    def makeMarker(self, header, id_num, point):
        ## Make a std_msgs/Marker object
        #
        # @param header A ROS Header object for the marker
        # @param id_num The id number corresponding to the marker. Note that these must be unique.
        # @param point An (x,y,z) tuple where the marker should be located in space
        # 
        # @return A std_msgs/Marker object

        marker = Marker()
        marker.header = header
        marker.id = id_num
        marker.type = marker.SPHERE
        marker.action = marker.ADD
        marker.scale.x = 0.03
        marker.scale.y = 0.03
        marker.scale.z = 0.03
        marker.color.r = 1.0
        marker.color.g = 0.2
        marker.color.b = 0.3
        marker.color.a = 1.0
        marker.pose.orientation.w = 1.0
        marker.pose.position.x = point[0]
        marker.pose.position.y = point[1]
        marker.pose.position.z = point[2]
        return marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import header [as 别名]
    def create_line_marker(p1, p2, frame_id):
        h = Header()
        h.frame_id = frame_id #tie marker visualization to laser it comes from
        h.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
        
        #create marker:person_marker, modify a red cylinder, last indefinitely
        mark = Marker()
        mark.header = h
        mark.ns = "unit_vector"
        mark.id = 0
        mark.type = Marker.LINE_STRIP
        mark.action = 0
        mark.scale.x = 0.2 
        mark.color = color = ColorRGBA(0.2, 0.5, 0.7, 1.0)
        mark.text = "unit_vector"

        points = []
        pt1 = Point(p1[0], p1[1], p1[2])
        pt2 = Point(p2[0], p2[1], p2[2])
        # print "Pt 1 ", pt1
        # print "Pt 2 ", pt2
        points.append(pt1)
        points.append(pt2)
        mark.points = points

        return mark

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import header [as 别名]
    def publish(self, grasps, obj=None):
        msg = MarkerArray()

        self.marker_id = 0  # reset marker counter
        if obj and len(obj.primitives) > 0 and len(obj.primitive_poses) > 0:
            m = Marker()
            m.header = obj.header
            m.ns = "object"
            m.id = self.marker_id
            self.marker_id += 1
            m.type = m.CUBE
            m.action = m.ADD
            m.pose = obj.primitive_poses[0]
            m.scale.x = obj.primitives[0].dimensions[0]
            m.scale.y = obj.primitives[0].dimensions[1]
            m.scale.z = obj.primitives[0].dimensions[2]
            m.color.r = 0
            m.color.g = 0
            m.color.b = 1
            m.color.a = 0.8
            msg.markers.append(m)

        for grasp in grasps:
            msg.markers.append(self.get_gripper_marker(grasp.grasp_pose, grasp.grasp_quality))

        self.pub.publish(msg)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import header [as 别名]
def make_kin_tree_from_link(ps,linkname, ns='default_ns',valuedict=None):
    markers = []
    U = get_pr2_urdf()
    link = U.links[linkname]
    
    if link.visual and link.visual.geometry and isinstance(link.visual.geometry,urdf.Mesh):
        rospy.logdebug("%s is a mesh. drawing it.", linkname)
        marker = Marker(type = Marker.MESH_RESOURCE, action = Marker.ADD)
        marker.ns = ns
        marker.header = ps.header        
        
        linkFromGeom = conversions.trans_rot_to_hmat(link.visual.origin.position, transformations.quaternion_from_euler(*link.visual.origin.rotation))
        origFromLink = conversions.pose_to_hmat(ps.pose)
        origFromGeom = np.dot(origFromLink, linkFromGeom)
        
        marker.pose = conversions.hmat_to_pose(origFromGeom)           
        marker.scale = gm.Vector3(1,1,1)
        marker.color = stdm.ColorRGBA(1,1,0,.5)
        marker.id = 0
        marker.lifetime = rospy.Duration()
        marker.mesh_resource = str(link.visual.geometry.filename)
        marker.mesh_use_embedded_materials = False
        markers.append(marker)
    else:
        rospy.logdebug("%s is not a mesh", linkname)
        
    if U.child_map.has_key(linkname):
        for (cjoint,clink) in U.child_map[linkname]:
            markers.extend(make_kin_tree_from_joint(ps,cjoint,ns=ns,valuedict=valuedict))
            
    return markers    

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import header [as 别名]
    def visualize_poop(x,y,z,color,frame,ns):
      msg = Marker()
      msg.header = Header(stamp=Time.now(), frame_id=frame)
      #msg.scale = Vector3(x=0.02, y=0.02, z=0.02) # for sphere
      msg.scale = Vector3(x=0.005, y=0.04, z=0.0) # for arrow
      #msg.pose.position = Point(x=x, y=y, z=z)
      #msg.pose.position = Point(x=x, y=y, z=z+0.15) # arrow
      #msg.pose.orientation = Quaternion(x=0, y=0, z=0, w=1) # arrow
      #msg.pose.orientation = Quaternion(x=0.707, y=0, z=0, w=0.707)
      msg.points = [Point(x=x, y=y,z=z+0.15), Point(x=x,y=y,z=z)]
      msg.ns = ns
      msg.id = 0
      msg.action = 0 # add
      #msg.type = 2 # sphere
      msg.type = 0 # arrow
      msg.color = ColorRGBA(r=0, g=0, b=0, a=1)
      if color == 0:
        msg.color.g = 1;
      elif color == 1:
        msg.color.b = 1;
      elif color == 2:
        msg.color.r = 1; 
        msg.color.g = 1; 
      elif color == 3:
        msg.color.g = 1; 
        msg.color.b = 1; 

      #loginfo("Publishing %s marker at %0.3f %0.3f %0.3f",ns,x,y,z)
      viz_pub.publish(msg)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import header [as 别名]
def publish(anns, data):
    ar_mk_list = AlvarMarkers()
    marker_list = MarkerArray()    

    marker_id = 1
    for a, d in zip(anns, data):
        
        # AR markers
        object = deserialize_msg(d.data, AlvarMarker)
        ar_mk_list.markers.append(object)
        
        # Visual markers
        marker = Marker()
        marker.id = marker_id
        marker.header = a.pose.header
        marker.type = a.shape
        marker.ns = "ar_mk_obstacles"
        marker.action = Marker.ADD
        marker.lifetime = rospy.Duration.from_sec(0)
        marker.pose = copy.deepcopy(a.pose.pose.pose)
        marker.scale = a.size
        marker.color = a.color

        marker_list.markers.append(marker)

        marker_id = marker_id + 1

    marker_pub = rospy.Publisher('ar_mk_marker',    MarkerArray, latch=True, queue_size=1)
    ar_mk_pub  = rospy.Publisher('ar_mk_pose_list', AlvarMarkers,latch=True, queue_size=1)

    ar_mk_pub.publish(ar_mk_list)
    marker_pub.publish(marker_list)
    
    return

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import header [as 别名]
 def pub_at_position(self, odom_msg):
     """
     Handles necessary information for displaying things at 
                             ACCEPTED (NOVATEL) POSITION SOLUTION:
         -vehicle mesh 
     !!!this should only be invoked when the accepted (novatel) position 
     is updated
     """
     ### G35 Mesh #############################################################
     marker = Marker()
     pub = self.curpos_publisher
     marker.header = odom_msg.header
     marker.id = 0 # enumerate subsequent markers here
     marker.action = Marker.MODIFY # can be ADD, REMOVE, or MODIFY
     marker.pose = odom_msg.pose.pose
     marker.pose.position.z = 1.55
     marker.lifetime = rospy.Duration() # will last forever unless modified
     marker.ns = "vehicle_model"
     marker.type = Marker.MESH_RESOURCE
     marker.scale.x = 0.0254 # artifact of sketchup export
     marker.scale.y = 0.0254 # artifact of sketchup export
     marker.scale.z = 0.0254 # artifact of sketchup export
     marker.color.r = .05
     marker.color.g = .05
     marker.color.b = .05
     marker.color.a = .2
     marker.mesh_resource = self.veh_mesh_resource
     marker.mesh_use_embedded_materials = False
     pub.publish(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import header [as 别名]
 def callback(self, state):
     self._id = 0
     
     m = self.create_arrow(state.base_pose)
     m.color.r = 1.0
     self._out_pub.publish(m)
     
     m = self.create_arrow(state.bridge_pose)
     m.color.g = 1.0
     m.scale.x = Constants.BRIDGE_TO_STRIKE_MIN
     self._out_pub.publish(m)
     
     m = Marker()
     m.header = state.bridge_pose.header
     m.ns = "billiards_shot_plan"
     m.id = self._id
     m.action = Marker.ADD
     m.type = Marker.CUBE
     m.pose = state.bridge_pose.pose
     m.pose.position.z += 0.055
     m.scale.x = 0.005
     m.scale.y = 0.05
     m.scale.z = 0.11
     m.color.a = 1.0
     m.color.b = 1.0
     self._out_pub.publish(m)
     
     self._id += 1

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import header [as 别名]
    def pose_cb(self, pose):
        m_pose = PointStamped()
        m_pose.header = pose.header
        m_pose.point = pose.pose.position
        m_pose = self.listener.transformPoint(self.reference_frame,m_pose)
        self.goal_x = m_pose.point.x
        self.goal_y = m_pose.point.y
        print "New goal: %.2f %.2f" % (self.goal_x, self.goal_y)

        marker = Marker()
        marker.header = pose.header
        marker.ns = "goal_marker"
        marker.id = 10
        marker.type = Marker.CYLINDER
        marker.action = Marker.ADD
        marker.pose = pose.pose
        marker.scale.x = 0.1
        marker.scale.y = 0.1
        marker.scale.z = 0.5;
        marker.color.a = 1.0;
        marker.color.r = 1.0;
        marker.color.g = 1.0;
        marker.color.b = 0.0;
        marker.lifetime.secs=-1.0
        self.marker_pub.publish(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import header [as 别名]
    def create_person_label_marker(self, person, color):
        h = Header()
        h.frame_id = person.header.frame_id #tie marker visualization to laser it comes from
        h.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
        
        #create marker:person_marker, modify a red cylinder, last indefinitely
        mark = Marker()
        mark.header = h
        mark.ns = "person_label_marker"
        
        ## simple hack to map persons name to integer value for unique marker id
        char_list = list(person.name)
        char_int_list = [str(ord(x)) for x in char_list]
        char_int_str = "".join(char_int_list)
        char_int = int(char_int_str) & 255
        print 
        print "str to int"
        print char_int_list
        print char_int
        print "Char int binary) ", bin(char_int)

        # mark.id = int(char_int / 10000)
        mark.id = int(float(person.name)) #char_int
        mark.type = Marker.TEXT_VIEW_FACING 
        mark.action = 0
        mark.scale = Vector3(self.scale_factor * 0.5, self.scale_factor * 0.5, 1) 
        mark.color = color 
        mark.lifetime = rospy.Duration(0.5,0)
        print "person name: ", person.name
        mark.text = person.name

        pose = Pose(Point(person.pose.position.x + 0.75, person.pose.position.y + 0.5, self.person_height + 0.75), Quaternion(0.0,0.0,1.0,cos(person.pose.position.z/2)))
        mark.pose = pose

        return mark

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import header [as 别名]
    def pose_cb(self, pose):
        pose = self.listener.transformPose(self.reference_frame,pose)
        q = pose.pose.orientation
        qvec = [q.x,q.y,q.z,q.w]
        euler = euler_from_quaternion(qvec)
        self.goal_x = pose.pose.position.x
        self.goal_y = pose.pose.position.y
        self.goal_theta = euler[2]
        (ex,ey,etheta) = self.compute_error()
        if ex < -self.dist_threshold:
            self.goal_theta = norm_angle(etheta + pi)
        print "New goal: %.2f %.2f %.2f" % (self.goal_x, self.goal_y, self.goal_theta)

        marker = Marker()
        marker.header = pose.header
        marker.ns = "goal_marker"
        marker.id = 10
        marker.type = Marker.ARROW
        marker.action = Marker.ADD
        marker.pose = pose.pose
        marker.scale.x = 0.5
        marker.scale.y = 0.5
        marker.scale.z = 1.0;
        marker.color.a = 1.0;
        marker.color.r = 1.0;
        marker.color.g = 1.0;
        marker.color.b = 0.0;
        marker.lifetime.secs=-1.0
        self.marker_pub.publish(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import header [as 别名]
def publish(anns, data):
    table_list = TableList()
    marker_list = MarkerArray()    

    marker_id = 1
    for a, d in zip(anns, data):
        
        # Tables
        object = pickle.loads(d.data)
        table_list.tables.append(object)
        
        # Markers
        marker = Marker()
        marker.id = marker_id
        marker.header = a.pose.header
        marker.type = a.shape
        marker.ns = "table_obstacles"
        marker.action = Marker.ADD
        marker.lifetime = rospy.Duration.from_sec(0)
        marker.pose = copy.deepcopy(a.pose.pose.pose)
        marker.scale = a.size
        marker.color = a.color

        marker_list.markers.append(marker)

        marker_id = marker_id + 1

    marker_pub = rospy.Publisher('table_marker',    MarkerArray, latch=True, queue_size=1)
    table_pub  = rospy.Publisher('table_pose_list', TableList,   latch=True, queue_size=1)

    table_pub.publish(table_list)
    marker_pub.publish(marker_list)
    
    return