Python Marker.lifetime方法代码示例

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import lifetime [as 别名]
def arrow_marker(position, direction, shaft_radius=0.02, head_radius=0.04, rgba=[1.0, 1.0, 1.0, 1.0], 
                     ns="", id=0, frame_id="", lifetime=None, stamp=None):
    marker = Marker()
    if stamp is None:
        marker.header.stamp = rospy.Time.now()
    else:
        marker.header.stamp = stamp
    marker.header.frame_id = frame_id
    marker.ns = ns
    marker.id = id
    marker.type = Marker.ARROW
    marker.action = Marker.ADD
    #marker.pose.position.x = position[0]
    #marker.pose.position.y = position[1]
    #marker.pose.position.z = position[2]
    marker.points = [Point(*tuple(position)), Point(*tuple(position+direction*1.0))]
    marker.scale.x = shaft_radius
    marker.scale.y = head_radius
    marker.scale.z = 1.0
    marker.color.r = rgba[0]
    marker.color.g = rgba[1]
    marker.color.b = rgba[2]
    marker.color.a = rgba[3]
    if lifetime is None:
        marker.lifetime = rospy.Duration()
    else:
        marker.lifetime = rospy.Duration(lifetime)
    return marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import lifetime [as 别名]
def spherelist_marker(points, size=[0.1, 0.1, 0.1], rgba=[1.0, 1.0, 1.0, 1.0], 
                     ns="", id=0, frame_id="", lifetime=None, stamp=None):
    marker = Marker()
    if stamp is None:
        marker.header.stamp = rospy.Time.now()
    else:
        marker.header.stamp = stamp
    marker.header.frame_id = frame_id
    marker.ns = ns
    marker.id = id
    marker.type = Marker.SPHERE_LIST
    marker.action = Marker.ADD
    for i in range(len(points)):
        marker.points.append(points[i])
    marker.scale.x = size[0]
    marker.scale.y = size[1]
    marker.scale.z = size[2]
    marker.color.r = rgba[0]
    marker.color.g = rgba[1]
    marker.color.b = rgba[2]
    marker.color.a = rgba[3]
    if lifetime is None:
        marker.lifetime = rospy.Duration()
    else:
        marker.lifetime = rospy.Duration(lifetime)
    return marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import lifetime [as 别名]
def mesh_marker(position, orientation=None, mesh_resource="", rgba=[1.0, 1.0, 1.0, 1.0],
                ns="", id=0, frame_id="", lifetime=None, stamp=None):
    marker = Marker()
    if stamp is None:
        marker.header.stamp = rospy.Time.now()
    else:
        marker.header.stamp = stamp
    marker.header.frame_id = frame_id
    marker.ns = ns
    marker.id = id
    marker.type = Marker.MESH_RESOURCE
    marker.mesh_resource = mesh_resource
    marker.action = Marker.ADD
    marker.pose.position.x = position[0]
    marker.pose.position.y = position[1]
    marker.pose.position.z = position[2]
    if orientation is not None:
        marker.pose.orientation.x = orientation[0] 
        marker.pose.orientation.y = orientation[1] 
        marker.pose.orientation.z = orientation[2] 
        marker.pose.orientation.w = orientation[3] 
    #marker.points = [Point(*tuple(position)), Point(*tuple(position+direction*1.0))]
    marker.scale.x = 1.0
    marker.scale.y = 1.0
    marker.scale.z = 1.0
    marker.color.r = rgba[0]
    marker.color.g = rgba[1]
    marker.color.b = rgba[2]
    marker.color.a = rgba[3]
    if lifetime is None:
        marker.lifetime = rospy.Duration()
    else:
        marker.lifetime = rospy.Duration(lifetime)
    return marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import lifetime [as 别名]
    def poseStampedToLabeledSphereMarker(cls, psdata, label, fmt="{label} %Y-%m-%d %H:%M:%S", zoffset=0.05):
        "[poseStamped, meta] -> sphereMarker"
        ps, meta = psdata
        res = []
        h = Header()
        h.stamp = rospy.Time.now()
        h.frame_id = ps.header.frame_id
        sphere = Marker(type=Marker.SPHERE,
                   action=Marker.ADD,
                   header=h,
                   id=cls.marker_id)
        sphere.scale.x = 0.07
        sphere.scale.y = 0.07
        sphere.scale.z = 0.07
        sphere.pose = ps.pose
        sphere.color = ColorRGBA(1.0,0,0,1.0)
        sphere.ns = "db_play"
        sphere.lifetime = rospy.Time(3)
        cls.marker_id += 1
        res.append(sphere)

        text = Marker(type=Marker.TEXT_VIEW_FACING,
                   action=Marker.ADD,
                   header=h,
                   id=cls.marker_id)
        text.scale.z = 0.1
        text.pose = deepcopy(ps.pose)
        text.pose.position.z += zoffset
        text.color = ColorRGBA(1.0,1.0,1.0,1.0)
        text.text = meta["inserted_at"].strftime(fmt).format(label=label)
        text.ns = "db_play_text"
        text.lifetime = rospy.Time(300)
        cls.marker_id += 1
        res.append(text)
        return res

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import lifetime [as 别名]
def ellipsoid_marker(position, size=[0.1, 0.1, 0.1], rgba=[1.0, 1.0, 1.0, 1.0], 
                     ns="", id=0, frame_id="", lifetime=None, stamp=None):
    marker = Marker()
    if stamp is None:
        marker.header.stamp = rospy.Time.now()
    else:
        marker.header.stamp = stamp
    marker.header.frame_id = frame_id
    marker.ns = ns
    marker.id = id
    marker.type = Marker.SPHERE
    marker.action = Marker.ADD
    marker.pose.position.x = position[0]
    marker.pose.position.y = position[1]
    marker.pose.position.z = position[2]
    marker.scale.x = size[0]
    marker.scale.y = size[1]
    marker.scale.z = size[2]
    marker.color.r = rgba[0]
    marker.color.g = rgba[1]
    marker.color.b = rgba[2]
    marker.color.a = rgba[3]
    if lifetime is None:
        marker.lifetime = rospy.Duration()
    else:
        marker.lifetime = rospy.Duration(lifetime)
    return marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import lifetime [as 别名]
def publish_cluster(publisher, points, frame_id, namespace, cluster_id):
    """Publishes a marker representing a cluster.
    The x and y arguments specify the center of the target.

    Args:
      publisher: A visualization_msgs/Marker publisher
      points: A list of geometry_msgs/Point
      frame_id: The coordinate frame in which to interpret the points.
      namespace: string, a unique name for a group of clusters.
      cluster_id: int, a unique number for this cluster in the given namespace.
    """
    marker = Marker()
    # TODO(jstn): Once the point clouds have the correct frame_id,
    # use them here.
    marker.header.frame_id = frame_id
    marker.header.stamp = rospy.Time().now()
    marker.ns = namespace
    marker.id = 2 * cluster_id
    marker.type = Marker.POINTS
    marker.action = Marker.ADD
    marker.color.r = random.random()
    marker.color.g = random.random()
    marker.color.b = random.random()
    marker.color.a = 0.5
    marker.points = points
    marker.scale.x = 0.002
    marker.scale.y = 0.002
    marker.lifetime = rospy.Duration()

    center = [0, 0, 0]
    for point in points:
        center[0] += point.x
        center[1] += point.y
        center[2] += point.z
    center[0] /= len(points)
    center[1] /= len(points)
    center[2] /= len(points)

    text_marker = Marker()
    text_marker.header.frame_id = frame_id
    text_marker.header.stamp = rospy.Time().now()
    text_marker.ns = namespace
    text_marker.id = 2 * cluster_id + 1
    text_marker.type = Marker.TEXT_VIEW_FACING
    text_marker.action = Marker.ADD
    text_marker.pose.position.x = center[0] - 0.1
    text_marker.pose.position.y = center[1]
    text_marker.pose.position.z = center[2]
    text_marker.color.r = 1
    text_marker.color.g = 1
    text_marker.color.b = 1
    text_marker.color.a = 1
    text_marker.scale.z = 0.05
    text_marker.text = '{}'.format(cluster_id)
    text_marker.lifetime = rospy.Duration()

    _publish(publisher, marker, "cluster")
    _publish(publisher, text_marker, "text_marker")
    return marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import lifetime [as 别名]
    def visualize_cluster(self, cluster, label=None):
        points = pc2.read_points(cluster.pointcloud, skip_nans=True)
        point_list = [Point(x=x, y=y-0.3, z=z) for x, y, z, rgb in points]
        if len(point_list) == 0:
            rospy.logwarn('Point list was of size 0, skipping.')
            return

        marker_id = len(self._current_markers)

        marker = Marker()
        marker.header.frame_id = 'map'
        marker.header.stamp = rospy.Time().now()
        marker.ns = 'clusters'
        marker.id = marker_id
        marker.type = Marker.POINTS
        marker.action = Marker.ADD
        marker.color.r = random.random()
        marker.color.g = random.random()
        marker.color.b = random.random()
        marker.color.a = 0.5 + random.random()
        marker.points = point_list
        marker.scale.x = 0.002
        marker.scale.y = 0.002
        marker.lifetime = rospy.Duration()
        self.visualize_marker(marker)
    
        if label is not None:
            center = [0, 0, 0]
            for point in point_list:
                center[0] += point.x
                center[1] += point.y
                center[2] += point.z
            center[0] /= len(point_list)
            center[1] /= len(point_list)
            center[2] /= len(point_list)
    
            text_marker = Marker()
            text_marker.header.frame_id = 'map'
            text_marker.header.stamp = rospy.Time().now()
            text_marker.ns = 'labels'
            text_marker.id = marker_id + 1
            text_marker.type = Marker.TEXT_VIEW_FACING
            text_marker.action = Marker.ADD
            text_marker.pose.position.x = center[1] - 0.05
            text_marker.pose.position.y = center[1]
            text_marker.pose.position.z = center[2]
            text_marker.color.r = 1
            text_marker.color.g = 1
            text_marker.color.b = 1
            text_marker.color.a = 1
            text_marker.scale.z = 0.05
            text_marker.text = label
            text_marker.lifetime = rospy.Duration()
    
            self.visualize_marker(text_marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import lifetime [as 别名]
    def transformStampedArrayToLabeledLineStripMarker(cls, tsdata_lst, fmt="%Y-%m-%d %H:%M:%S", zoffset=0.05, label_downsample=1, discrete=False):
        "[[transformStamped, meta],...] -> LineStrip / String"
        res = []
        # make line strip
        points = []
        colors = []
        t_first = tsdata_lst[0][0]
        prev_t = t_first.transform.translation
        for ts, _ in tsdata_lst:
            t = ts.transform.translation
            dist = distanceOfVector3(prev_t, t) * 0.65
            rgb = colorsys.hsv_to_rgb(dist, 1.0, 1.0)
            points.append(Point(x=t.x, y=t.y, z=t.z))
            colors.append(ColorRGBA(rgb[0],rgb[1],rgb[2],1.0))
            prev_t = t

        h = Header()
        h.stamp = rospy.Time(0) #rospy.Time.now()
        h.frame_id = "eng2" #t_first.child_frame_id
        if discrete:
            m_type = Marker.POINTS
        else:
            m_type = Marker.LINE_STRIP
        m = Marker(type=m_type,
                   action=Marker.ADD,
                   header=h,
                   id=cls.marker_id)
        cls.marker_id += 1
        m.scale.x = 0.1
        m.scale.y = 0.1
        m.points = points
        m.colors = colors
        m.ns = "labeled_line"
        m.lifetime = rospy.Time(3000)
        res.append(m)

        for t, t_meta in tsdata_lst[::label_downsample]:
            text = Marker(type=Marker.TEXT_VIEW_FACING,
                          action=Marker.ADD,
                          header=h,
                          id=cls.marker_id)
            cls.marker_id += 1
            text.scale.z = 0.1
            text.pose = T.poseFromTransform(t.transform)
            text.pose.position.z += zoffset
            text.color = ColorRGBA(0.0,0.0,1.0,1.0)
            text.text = t_meta["inserted_at"].strftime(fmt)
            text.ns = "labeled_line_text"
            text.lifetime = rospy.Time(3000)
            res.append(text)
        return res

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import lifetime [as 别名]
def drawlandmark(pub):
    global pt_count
    points_tag = Marker()
    points_tag.header.frame_id = "/base_link"
    points_tag.action = Marker.ADD
    points_tag.header.stamp = rospy.Time.now()
    points_tag.lifetime = rospy.Time(0)
    points_tag.scale.x = 0.1;
    points_tag.scale.y = 0.1;
    points_tag.scale.z = 0.1;
    points_tag.color.a = 1.0;
    points_tag.color.r = 1.0;
    points_tag.color.g = 0.0;
    points_tag.color.b = 0.0;
    points_tag.ns = "pts_line"
    pt_count+=1
    points_tag.id = pt_count
    points_tag.type = Marker.POINTS
    for x in range(6):
        p1 = Point()
        p1.x = tagnum_x[x]/100
        p1.y = tagnum_y[x]/100
        p1.z = 0
        points_tag.points.append(p1)
    pub.publish(points_tag)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import lifetime [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 lifetime [as 别名]
	def set_position_callback(self,marker,joy):
		print self.position_marker.ns
		print joy.buttons[3] == 1
		print marker.ns == "PEOPLE"
		if (self.position_marker.ns == "NONE" and joy.buttons[3] == 1 and marker.ns == "PEOPLE"):
			self.position_marker = marker
			print "set position"
			ref_marker = Marker()
			ref_marker.header.frame_id = "base_footprint"
			ref_marker.header.stamp = rospy.get_rostime()
			ref_marker.ns = "robot"
			ref_marker.type = 9
			ref_marker.action = 0
			ref_marker.pose.position.x = self.position_marker.pose.position.x
			ref_marker.pose.position.y = self.position_marker.pose.position.y
			ref_marker.pose.position.z = self.position_marker.pose.position.z
			ref_marker.scale.x = .25
			ref_marker.scale.y = .25
			ref_marker.scale.z = .25
			ref_marker.color.r = 1.0
			ref_marker.color.g = 0.0
			ref_marker.color.a = 1.0
			ref_marker.lifetime = rospy.Duration(0)
			self.ref_viz_pub.publish(ref_marker)
		else:
			pass

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import lifetime [as 别名]
    def draw_bounding_box(self, id, box_msg, color=(1.0, 1.0, 0.0, 0.0), duration=0.0):
        """
        Draws a bounding box as detectd by detect_bounding_box.
        
        Parameters: 
        box_msg is a FindClusterBoundingBoxResponse msg.
        color: a quadruple with alpha, r,g,b
        duration: how long should the bounding box last. 0 means forever.
        """

        marker = Marker()
        marker.header.stamp = rospy.Time.now()
        marker.ns = "object_detector"
        marker.type = Marker.CUBE
        marker.action = marker.ADD
        marker.id = id
        marker.header.frame_id = box_msg.pose.header.frame_id

        marker.pose = box_msg.pose.pose
        marker.scale.x = box_msg.box_dims.x
        marker.scale.y = box_msg.box_dims.y
        marker.scale.z = box_msg.box_dims.z

        marker.color.a = color[0]
        marker.color.r = color[1]
        marker.color.g = color[2]
        marker.color.b = color[3]
        marker.lifetime = rospy.Duration(duration)

        self.box_drawer.publish(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import lifetime [as 别名]
    def draw_box_marker(
        self, id, x, y, z, dimx, dimy, dimz, color=(1.0, 1.0, 1.0, 0.0), duration=0.0, frame_id="base_link"
    ):

        marker = Marker()
        marker.header.stamp = rospy.Time.now()
        marker.ns = "object_detector"
        marker.type = Marker.CUBE
        marker.action = marker.ADD
        marker.id = id
        marker.header.frame_id = "base_link"

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

        marker.scale.x = dimx
        marker.scale.y = dimy
        marker.scale.z = dimz

        marker.color.a = color[0]
        marker.color.r = color[1]
        marker.color.g = color[2]
        marker.color.b = color[3]
        marker.lifetime = rospy.Duration(duration)

        self.box_drawer.publish(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import lifetime [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 lifetime [as 别名]
    def pub_body(self, pos, quat, scale, color, num, shape, text = ''):
        marker = Marker()
        marker.header.frame_id = self.frame
        marker.header.stamp = rospy.Time.now()

        marker.ns = "basic_shapes"
        marker.id = num

        marker.type = shape
        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 = quat[0]
        marker.pose.orientation.y = quat[1]
        marker.pose.orientation.z = quat[2]
        marker.pose.orientation.w = quat[3]
        marker.scale.x = scale[0]
        marker.scale.y = scale[1]
        marker.scale.z = scale[2]
        marker.color.r = color[0]
        marker.color.g = color[1]
        marker.color.b = color[2]
        marker.color.a = color[3]
        marker.lifetime = rospy.Duration()
        marker.text = text
        self.pub.publish(marker)