Python Marker.ns方法代码示例

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ns [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 ns [as 别名]
 def add_marker(self, array, track, color, tid):
     m1 = Marker()
     m1.header.frame_id = "camera_rgb_optical_frame"
     m1.ns = "line"
     m1.id = tid
     m1.type = 4 #lines
     m1.action = 0
     m1.scale.x = .002
     m1.color.a = 1.
     m1.color.r = color[0]/255.
     m1.color.g = color[1]/255.
     m1.color.b = color[2]/255.
     m1.points = track
     array.append(m1)
     m2 = Marker()
     m2.header.frame_id = "camera_rgb_optical_frame"
     m2.ns = "point"
     m2.id = tid
     m2.type = 8 #points
     m2.action = 0
     m2.scale.x = .008
     m2.scale.y = .008
     m2.color.a = 1.
     m2.color.r = color[0]/255.
     m2.color.g = color[1]/255.
     m2.color.b = color[2]/255.
     m2.points = [ track[-1] ]
     array.append(m2)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ns [as 别名]
    def publish_debug_info(self, footprint_point, distance, scan_point):
        array = MarkerArray()

        fp = Marker()
        fp.id = 1
        fp.type = fp.SPHERE
        scale = 0.1
        fp.pose.position.x,fp.pose.position.y, fp.pose.position.z = footprint_point[0], footprint_point[1], 0
        fp.scale.x, fp.scale.y, fp.scale.z = scale, scale, scale
        fp.color.r, fp.color.g, fp.color.b, fp.color.a = (0, 0, 1, 1)
        fp.header.frame_id = "{}/base_link".format(self.robot.robot_name)
        fp.frame_locked = True
        fp.action = fp.ADD
        fp.ns = "door_opening"
        array.markers += [fp]

        sp = Marker()
        sp.id = 2
        sp.type = sp.SPHERE
        scale = 0.1
        sp.pose.position.x,sp.pose.position.y, sp.pose.position.z = scan_point[0], scan_point[1], 0
        sp.scale.x, sp.scale.y, sp.scale.z = scale, scale, scale
        sp.color.r, sp.color.g, sp.color.b, sp.color.a = (1, 0, 0, 1)
        sp.header.frame_id = "{}/base_link".format(self.robot.robot_name)
        sp.frame_locked = False
        sp.action = sp.ADD
        sp.ns = "door_opening"
        array.markers += [sp]

        self.debug_vizualizer.publish(array)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ns [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 ns [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 ns [as 别名]
 def publish(self, target_frame, timestamp):
     ma = MarkerArray()
     for id in self.marker_list:
         marker = Marker()
         marker.header.stamp = timestamp
         marker.header.frame_id = target_frame
         marker.ns = "landmark_kf"
         marker.id = id
         marker.type = Marker.CYLINDER
         marker.action = Marker.ADD
         Lkf = self.marker_list[id]
         marker.pose.position.x = Lkf.L[0,0]
         marker.pose.position.y = Lkf.L[1,0]
         marker.pose.position.z = 0
         marker.pose.orientation.x = 0
         marker.pose.orientation.y = 0
         marker.pose.orientation.z = 1
         marker.pose.orientation.w = 0
         marker.scale.x = max(3*sqrt(Lkf.P[0,0]),0.05)
         marker.scale.y = max(3*sqrt(Lkf.P[1,1]),0.05)
         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=3.0;
         ma.markers.append(marker)
         marker = Marker()
         marker.header.stamp = timestamp
         marker.header.frame_id = target_frame
         marker.ns = "landmark_kf"
         marker.id = 1000+id
         marker.type = Marker.TEXT_VIEW_FACING
         marker.action = Marker.ADD
         Lkf = self.marker_list[id]
         marker.pose.position.x = Lkf.L[0,0]
         marker.pose.position.y = Lkf.L[1,0]
         marker.pose.position.z = 1.0
         marker.pose.orientation.x = 0
         marker.pose.orientation.y = 0
         marker.pose.orientation.z = 1
         marker.pose.orientation.w = 0
         marker.text = str(id)
         marker.scale.x = 1.0
         marker.scale.y = 1.0
         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.lifetime.secs=3.0;
         ma.markers.append(marker)
     self.marker_pub.publish(ma)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ns [as 别名]
    def pubViz(self, ast, bst):

        rate = rospy.Rate(10)

        for i in range(self.winSize):

            msgs = MarkerArray()
            
            #use1について
            msg = Marker()
            #markerのプロパティ
            msg.header.frame_id = 'camera_link'
            msg.header.stamp = rospy.Time.now()
            msg.ns = 'j1'
            msg.action = 0
            msg.id = 1
            msg.type = 8
            msg.scale.x = 0.1
            msg.scale.y = 0.1
            msg.color = self.carray[2]
            #ジョイントポイントを入れる処理
            for j1 in range(self.jointSize):
                point = Point()
                point.x = self.pdata[0][ast+i][j1][0]
                point.y = self.pdata[0][ast+i][j1][1]
                point.z = self.pdata[0][ast+i][j1][2]
                msg.points.append(point) 
            msg.pose.orientation.w = 1.0
            msgs.markers.append(msg)    
            
            msg = Marker()
            msg.header.frame_id = 'camera_link'
            msg.header.stamp = rospy.Time.now()
            msg.ns = 'j2'
            msg.action = 0
            msg.id = 2
            msg.type = 8
            msg.scale.x = 0.1
            msg.scale.y = 0.1
            msg.color = self.carray[1]

            for j2 in range(self.jointSize):
                point = Point()
                point.x = self.pdata[1][bst+i][j2][0]
                point.y = self.pdata[1][bst+i][j2][1]
                point.z = self.pdata[1][bst+i][j2][2]
                msg.points.append(point) 
            msg.pose.orientation.w = 1.0

            msgs.markers.append(msg)

            self.mpub.publish(msgs)
            rate.sleep()

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ns [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 ns [as 别名]
    def pubRviz(self, pos, joints):

        msgs = MarkerArray()
        for p in range(len(pos)):
            msg = Marker()

            msg.header.frame_id = 'camera_link'
            msg.header.stamp = rospy.Time.now()
            msg.ns = 'marker'
            msg.action = 0
            msg.id = p
            msg.type = 4
            msg.scale.x = 0.1
            msg.scale.y = 0.1
            msg.color = self.carray[2]

            for i in range(len(pos[p])):
                point = Point()
                point.x = pos[p][i][0]
                point.y = pos[p][i][1]
                point.z = pos[p][i][2]
                msg.points.append(point) 
            msg.pose.orientation.w = 1.0
            msgs.markers.append(msg)

        for j in range(len(joints)):
            msg = Marker()

            msg.header.frame_id = 'camera_link'
            msg.header.stamp = rospy.Time.now()
            msg.ns = 'joints'
            msg.action = 0
            msg.id = j
            msg.type = 8
            msg.scale.x = 0.1
            msg.scale.y = 0.1
            msg.color = self.carray[j]

            #print "joints len:"+str(len(joints[j]))
            for i in range(len(joints[j])):
                point = Point()
                point.x = joints[j][i][0]
                point.y = joints[j][i][1]
                point.z = joints[j][i][2]
                msg.points.append(point) 
            msg.pose.orientation.w = 1.0
            msgs.markers.append(msg)

        self.mpub.publish(msgs)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ns [as 别名]
def publish_markerArray(publisher, points, rgba=(1, 0, 0, 1), shape=Marker.CUBE, duration=rospy.Duration(360), ns='ns'):
        #points is expected to be a list of tuples (x,y)
        #It's recommended that the publisher is created with latch=True
        _id = 0
        ma = MarkerArray()
        for p in points:
            m = Marker()
            m.header.frame_id = '/map'
            m.header.stamp = rospy.get_rostime()
            m.ns = ns
            m.id = _id
            m.type = shape
            m.action = m.ADD
            m.pose.position.x = p[0]
            m.pose.position.y = p[1]
            m.pose.orientation.w = 1
            m.scale.x = 0.5
            m.scale.y = 0.5
            m.scale.z = 0.5
            m.color.r = rgba[0]
            m.color.g = rgba[1]
            m.color.b = rgba[2]
            m.color.a = rgba[3]
            m.lifetime = duration
            ma.markers.append(m)
            _id += 1
        publisher.publish(ma)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ns [as 别名]
 def new_marker(self, ns="/debug", frame="enu", time=None, type = Marker.CUBE , position=(0,0,0), orientation=(0,0,0,1), color=(1,0,0)):
     marker = Marker()
     marker.ns = ns
     if time != None:
         marker.header.stamp = time
     marker.header.frame_id = frame
     marker.type = type
     marker.action = marker.ADD
     marker.scale.x = 1.0
     marker.scale.y = 1.0
     marker.scale.z = 1.0
     marker.color.r = color[0]
     marker.color.g = color[1]
     marker.color.b = color[2]
     marker.color.a = 1.0
     marker.id = self.last_id
     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.pose.position.x = position[0]
     marker.pose.position.y = position[1]
     marker.pose.position.z = position[2]
     self.last_id += 1
     self.markers.markers.append(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ns [as 别名]
 def __publish_marker_for_object(self, object, txt):
     marker = Marker()
     marker.header.frame_id = object.object.header.frame_id
     marker.header.stamp = rospy.Time(0)
     marker.ns = "scene_classifier_marker"
     self.marker_id = self.marker_id + 1
     marker.id = self.marker_id
     marker.type = Marker.TEXT_VIEW_FACING
     marker.action = Marker.ADD
     marker.pose.position.x = object.c_centroid.x
     marker.pose.position.y = object.c_centroid.y
     marker.pose.position.z = object.c_centroid.z
     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 = 0.5
     marker.scale.y = 0.1
     marker.scale.z = 0.1
     marker.color.a = 1.0
     marker.color.r = 0.0
     marker.color.g = 1.0
     marker.color.b = 0.0
     marker.text = txt
     marker.lifetime = rospy.Duration.from_sec(10)
     self.marker_publisher.publish(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ns [as 别名]
    def to_marker(self):
        """
        :return: a marker representing the map.
        :type: Marker
        """
        marker = Marker()
        marker.type = Marker.CUBE_LIST
        for x in range(0, len(self.field)):
            for y in range(0, len(self.field[0])):
                marker.header.frame_id = '/odom_combined'
                marker.ns = 'suturo_planning/map'
                marker.id = 23
                marker.action = Marker.ADD
                (x_i, y_i) = self.index_to_coordinates(x, y)
                marker.pose.position.x = 0
                marker.pose.position.y = 0
                marker.pose.position.z = 0
                marker.pose.orientation.w = 1
                marker.scale.x = self.cell_size
                marker.scale.y = self.cell_size
                marker.scale.z = 0.01

                p = Point()
                p.x = x_i
                p.y = y_i

                marker.points.append(p)
                c = self.get_cell_by_index(x, y)
                marker.colors.append(self.__cell_to_color(c))
                marker.lifetime = rospy.Time(0)

        return marker

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import ns [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 ns [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)