KeyPointsManager.py 文件源码

def addWaypointMarker(self, keyPoint):
        rospy.loginfo('Publishing marker')
        # Set up our waypoint markers
        marker_scale = 0.2
        marker_lifetime = 0 # 0 is forever
        marker_ns = 'waypoints'
        marker_id = keyPoint.id
        marker_color = {'r': 0.0, 'g': 1.0, 'b': 0.0, 'a': 1.0}

        # Initialize the marker points list.
        self.waypoint_markers = Marker()
        self.waypoint_markers.ns = marker_ns
        self.waypoint_markers.id = marker_id
        self.waypoint_markers.type = Marker.CUBE_LIST
        self.waypoint_markers.action = Marker.ADD
        self.waypoint_markers.lifetime = rospy.Duration(marker_lifetime)
        self.waypoint_markers.scale.x = marker_scale
        self.waypoint_markers.scale.y = marker_scale
        self.waypoint_markers.color.r = marker_color['r']
        self.waypoint_markers.color.g = marker_color['g']
        self.waypoint_markers.color.b = marker_color['b']
        self.waypoint_markers.color.a = marker_color['a']

        self.waypoint_markers.header.frame_id = 'map'
        self.waypoint_markers.header.stamp = rospy.Time.now()
        self.waypoint_markers.points = list()
        p = Point(keyPoint.pose.position.x, keyPoint.pose.position.y, keyPoint.pose.position.z)
        self.waypoint_markers.points.append(p)

        # Publish the waypoint markers
        self.marker_pub = rospy.Publisher('waypoint_markers', Marker)

        self.marker_pub.publish(self.waypoint_markers)