Python Marker.action方法代码示例

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import action [as 别名]
	def publishWaypointMarker(self, wp, delete = False):
		marker = Marker()
		marker.header.frame_id = "world"  
		marker.header.stamp = rospy.Time.now()
		marker.ns = "waypoints"
		marker.id = self.wp_ids[wp]
		marker.lifetime = rospy.Duration(0)
		if delete:
			marker.action = marker.DELETE
		else:
			marker.type = marker.SPHERE
			marker.action = marker.ADD
			marker.pose.position.x = wp.x
			marker.pose.position.y = wp.y
			marker.pose.orientation.w = 1.0
			marker.scale.y = 0.02
			marker.scale.x = 0.02
			marker.scale.z = 0.01
			marker.color.a = 1.0
			if wp.action == Waypoint.VIEW:
				marker.color.g = 1.0
			else:
				marker.color.r = 1.0

		# print marker
		self.vizPub.publish(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import action [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 action [as 别名]
   def plot_3d_vel(self, p_arr, v_arr, v_scale=1.0):

      marker_array = MarkerArray()

      for i in xrange(len(p_arr)):
	 p = p_arr[i]
	 v = vx,vy,vz = v_arr[i]
	 marker = Marker()
	 marker.header.frame_id = "/openni_rgb_optical_frame"
	 marker.type = marker.ARROW
	 marker.action = marker.ADD
	 marker.color.a = 1.0
	 marker.color.r = 1.0
	 marker.color.g = 0.0
	 marker.color.b = 0.0
	 marker.points.append(Point(p[0],p[1],p[2]))
	 marker.points.append(Point(p[0]+vx*v_scale,p[1]+vy*v_scale,p[2]+vz*v_scale)) 
	 marker.scale.x=0.05
	 marker.scale.y=0.1
	 marker.id = i 

	 marker_array.markers.append(marker)
      
      for i in xrange(len(p_arr),self.max_features):
	 marker = Marker()
	 marker.action = marker.DELETE
	 marker.id = i
	 marker_array.markers.append(marker)


      self.marker_pub.publish(marker_array)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import action [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 action [as 别名]
    def publishPositions(self):
        if not self.initialized:
            return
        nr = 0
        markerArray = MarkerArray()
        marker = Marker()
        marker.header.frame_id = "/map"
        marker.type = marker.MESH_RESOURCE
        marker.action = marker.ADD
        marker.scale.x = 0.2
        marker.scale.y = 0.2
        marker.scale.z = 0.2

        marker.mesh_use_embedded_materials = True
        marker.mesh_resource = "package://pacman_controller/meshes/pacman.dae"

        marker.color.a = 1.0
        marker.color.r = 0.0
        marker.color.g = 1.0
        marker.color.b = 0.0
        marker.pose.orientation = self.pacman["orientation"]
        marker.pose.position.x = self.pacman["x"]
        marker.pose.position.y = self.pacman["y"]
        marker.pose.position.z = 0.0
        marker.id = nr
        markerArray.markers.append(marker)

        for ghost in self.ghosts:
            curGhost = self.ghosts[ghost]
            if not curGhost["initialized"]:
                continue
            nr += 1
            marker = Marker()
            marker.header.frame_id = "/map"
            marker.type = marker.MESH_RESOURCE
            marker.action = marker.ADD
            marker.scale.x = 0.3
            marker.scale.y = 0.3
            marker.scale.z = 0.3

            marker.mesh_use_embedded_materials = True
            if curGhost["eaten"]:
                marker.mesh_resource = "package://pacman_controller/meshes/dead.dae"
            elif self.state == State.FLEEING:
                marker.mesh_resource = "package://pacman_controller/meshes/ghost_catchable.dae"
            else:
                marker.mesh_resource = "package://pacman_controller/meshes/%s.dae" % ghost
            marker.color.a = 1.0
            marker.color.r = 0.0
            marker.color.g = 1.0
            marker.color.b = 0.0
            marker.pose.orientation = curGhost["orientation"]
            marker.pose.position.x = curGhost["x"]
            marker.pose.position.y = curGhost["y"]
            marker.pose.position.z = 0.0
            marker.id = nr
            markerArray.markers.append(marker)

        self.pubPositions.publish(markerArray)
        return

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import action [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 action [as 别名]
    def publishMap(self):
        markerArray = MarkerArray()
        marker = Marker()
        marker.header.frame_id = "/map"
        marker.type = marker.SPHERE_LIST
        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 = 1.0
        marker.color.g = 1.0
        marker.color.b = 0.0
        marker.pose.orientation.w = 1.0
        marker.pose.position.x = 0.0
        marker.pose.position.y = 0.0
        marker.pose.position.z = 0.0
        marker.id = 0
        for p in self.mapPoints:
            if p["eaten"]:
                continue
            if p["powerup"]:
                continue
            point = Point()
            point.x = p["x"]
            point.y = p["y"]
            point.z = 0.15
            marker.points.append(point)
        markerArray.markers.append(marker)

        marker = Marker()
        marker.header.frame_id = "/map"
        marker.type = marker.SPHERE_LIST
        marker.action = marker.ADD
        marker.scale.x = 0.3
        marker.scale.y = 0.3
        marker.scale.z = 0.3
        marker.color.a = 1.0
        marker.color.r = 1.0
        marker.color.g = 1.0
        marker.color.b = 0.0
        marker.pose.orientation.w = 1.0
        marker.pose.position.x = 0.0
        marker.pose.position.y = 0.0
        marker.pose.position.z = 0.0
        marker.id = 1
        for p in self.mapPoints:
            if p["eaten"]:
                continue
            if not p["powerup"]:
                continue
            point = Point()
            point.x = p["x"]
            point.y = p["y"]
            point.z = 0.2
            marker.points.append(point)
        markerArray.markers.append(marker)

        self.pubMap.publish(markerArray)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import action [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 action [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 action [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 action [as 别名]
    def publishState(self):
        self.pubState.publish(str(self.state))
        marker = Marker()
        marker.header.frame_id = "/map"
        marker.header.stamp = rospy.get_rostime()
        marker.type = marker.TEXT_VIEW_FACING
        marker.action = marker.ADD
        marker.scale.x = 1.0
        marker.scale.y = 0.2
        marker.scale.z = 0.5
        marker.color.a = 1.0
        marker.color.r = 1.0
        marker.color.g = 0.0
        marker.color.b = 0.0
        marker.pose.orientation.w = 1.0
        marker.pose.position.x = self.infoLoc["x"]
        marker.pose.position.y = self.infoLoc["y"]
        marker.pose.position.z = 0.0
        marker.id = 0
        if self.state in [State.RUNNING]:
            if not self.sendStart:
                marker.text = "Start"
                self.sendStart = True
                self.timeSend = rospy.get_rostime()
            elif (rospy.get_rostime() - self.timeSend).to_sec() > 0.2:
                # marker.type = marker.SPHERE
                marker.action = marker.DELETE
            else:
                marker.text = "Start"

        if self.state == State.FLEEING:
            timeFleeing = TIME_FLEEING - (rospy.get_rostime() - self.timeStartFleeing).to_sec()
            if timeFleeing > 0:
                marker.text = str(timeFleeing)
            else:
                marker.action = marker.DELETE
                self.state = State.RUNNING

        if self.state in [State.GAME_OVER, State.STOPPED, State.SETUP]:
            marker.text = "GAME OVER!"

        if self.state in [State.INIT]:
            marker.text = "Initializing Game"

        if self.state in [State.PAUSED]:
            marker.text = "Game Paused"

        if self.state in [State.WON]:
            marker.text = "Congratulations, you have won the Game! Now get back to you real job!"

        self.pubInfo.publish(marker)

# 需要导入模块: from visualization_msgs.msg import Marker [as 别名]
# 或者: from visualization_msgs.msg.Marker import action [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 action [as 别名]
 def addPolygonFilled(self, points):
     oid = self.oid+1
     name = "/polygonFilled"+str(self.oid)
     marker = Marker()
     marker.id = self.oid
     marker.ns = "/polygonFilled"
     marker.header.frame_id = name
     marker.type = marker.TRIANGLE_LIST
     marker.action = marker.ADD
     marker.scale.x = 1
     marker.scale.y = 1
     marker.scale.z = 1
     marker.color.r = 1.0
     marker.color.g = 1.0
     marker.color.b = 1.0
     marker.color.a = 1.0
     marker.pose.orientation.w = 1.0
     marker.pose.position.x = 0
     marker.pose.position.y = 0
     marker.pose.position.z = 0
     marker.points = []
     for i in range(0,len(points)-2,1):
             pt = Point(points[0][0], points[0][1], points[0][2])
             marker.points.append(pt)
             pt = Point(points[i+1][0], points[i+1][1], points[i+1][2])
             marker.points.append(pt)
             pt = Point(points[i+2][0], points[i+2][1], points[i+2][2])
             marker.points.append(pt)
     self.markerArray.markers.append(marker)

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