Trajectory visualization in rViz using Marker types to create solid or dotted lines tracking a point as it traverses in space (e.g. the trace of the end-effector as it moves through space).

trajectory_visualization.py

#!/usr/bin/env python
import sys
import rospy
from matplotlib import colors
from geometry_msgs.msg import Point, PoseStamped
from visualization_msgs.msg import Marker

# Log levels
INFO  = 0
WARN  = 1
ERROR = 2


class TrajectoryVisualizer:
    """
    Trajectory visualization for rViz using Marker display types. Currently supports
    dotted and solid lines.
    """

    def __init__(self, pose_topic, marker_topic='visualization_marker', style='point',
                 size=0.01, color='darkgreen', base_frame='world', rate=100):
        """
        Args:
            pose_topic:   Topic this node subscribes to in order to receive pose coordinates
                          that markers will be displayed at.
            marker_topic: Topic that visualization Markers will be published to.
            style:        Style of marker, either 'point' (dotted line) or 'line' (solid line)
            size:         Size of the marker to be displayed.
            color:        Color of the marker (see _get_marker function for color options)
            base_frame:   Frame to publish the marker with respect to.
            rate:         Rate at which marker msgs will be published at.
        """
        self.base_frame = base_frame
        self.marker = self._get_marker(style, float(size), color)
        self.pose = None
        self.rate = rospy.Rate(float(rate))
        self.marker_pub = rospy.Publisher(marker_topic, Marker, queue_size=1)
        rospy.Subscriber(pose_topic, PoseStamped, self._pose_cb)

    def run(self):
        """
        Wait until a pose is received that will determine where the marker should be
        displayed, then publish the marker repeatedly while updating the position.
        """
        if self.marker is None:
            self._log("Marker could not be created.", ERROR)
            return None

        self._log("Waiting for pose...")
        while not rospy.is_shutdown() and self.pose is None:
            self.rate.sleep()
        self._log("Pose received.")
        self._log("Publishing markers...")
        while not rospy.is_shutdown():
            self._publish_marker()
            self.rate.sleep()

    def stop(self):
        self._log("Complete. Exiting.")

        

    def _publish_marker(self):
        """
        Publish the populated marker.
        """
        self.marker.points.append(Point(self.pose.position.x,
                                        self.pose.position.y,
                                        self.pose.position.z))
        self.marker_pub.publish(self.marker)
        
    def _pose_cb(self, pose_stmp_msg):
        """
        Callback for a PoseStamped msg, the xyz position of the pose
        determines where the marker will be displayed. Pose should be
        expressed in the same frame as self.base_frame.
        """
        self.pose = pose_stmp_msg.pose

    def _get_marker(self, style='point', size=0.01, color='darkgreen'):
        """Create a visualization Marker instance.

        Args:
            style: Style of marker. 'point' will create a dotted line,
                   'line' will create a solid line.
            size:  Size of the visualization points that will be displayed.
            color: Color of marker, can be any name from this page:
                   http://matplotlib.org/mpl_examples/color/named_colors.hires.png
        """
        converter = colors.ColorConverter()
        c = converter.to_rgba(colors.cnames[color])
        m = Marker()
        m.header.frame_id = self.base_frame
        m.header.stamp = rospy.Time()
        m.id = 0
        if style == 'point':
            m.type = Marker.POINTS
        elif style == 'line':
            m.type = Marker.LINE_STRIP
        else:
            self._log("Unknown marker style: %s" % style, ERROR)
            return None
        m.points = []
        m.action = Marker.MODIFY
        m.pose.orientation.w = 1.0
        m.scale.x = size
        m.color.r = c[0]
        m.color.g = c[1]
        m.color.b = c[2]
        m.color.a = c[3]
        return m

    def _log(self, msg, level=INFO):
        """
        Logging message that will display the node's name in the log msg.
        """
        msg = "[%s] %s" % (self.__class__.__name__, msg)
        if level == WARN:
            rospy.logwarn(msg)
        elif level == ERROR:
            rospy.logerr(msg)
        else:
            rospy.loginfo(msg)

    
if __name__ == '__main__':
    rospy.init_node('visualize_trajectory')
    tv = TrajectoryVisualizer(*rospy.myargv(argv=sys.argv[1:]))
    rospy.on_shutdown(tv.stop)
    try:
        tv.run()
    except rospy.ROSInterruptException:
        pass