rqt_bag: publish clock time from bag

rqt_bag/package.xml

@@ -18,6 +18,7 @@
 
   <run_depend>python-rospkg</run_depend>
   <run_depend>rosbag</run_depend>
+  <run_depend>rosgraph_msgs</run_depend>
   <run_depend>roslib</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend version_gte="0.2.12">rqt_gui</run_depend>

rqt_bag/src/rqt_bag/bag.py

@@ -47,12 +47,14 @@ def __init__(self, context):
         """
         super(Bag, self).__init__(context)
         self.setObjectName('Bag')
-        self._widget = BagWidget(context)
+
+        args = self._parse_args(context.argv())
+
+        self._widget = BagWidget(context, args.clock)
         if context.serial_number() > 1:
             self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
         context.add_widget(self._widget)
 
-        args = self._parse_args(context.argv())
         for bagfile in args.bagfiles:
             self._widget.load_bag(bagfile)
 
@@ -64,6 +66,7 @@ def _parse_args(self, argv):
     @staticmethod
     def add_arguments(parser):
         group = parser.add_argument_group('Options for rqt_bag plugin')
+        group.add_argument('--clock', action='store_true', help='publish the clock time')
         group.add_argument('bagfiles', type=argparse.FileType('r'), nargs='*', default=[], help='Bagfiles to load')
 
     def shutdown_plugin(self):

rqt_bag/src/rqt_bag/bag_timeline.py

@@ -56,7 +56,7 @@ class BagTimeline(QGraphicsScene):
     """
     status_bar_changed_signal = Signal()
 
-    def __init__(self, context):
+    def __init__(self, context, publish_clock):
         """
         :param context: plugin context hook to enable adding rqt_bag plugin widgets as ROS_GUI snapin panes, ''PluginContext''
         """
@@ -80,6 +80,7 @@ def __init__(self, context):
         self._messages = {}  # topic -> (bag, msg_data)
         self._message_listener_threads = {}  # listener -> MessageListenerThread
         self._player = False
+        self._publish_clock = publish_clock
         self._recorder = None
         self.last_frame = None
         self.last_playhead = None
@@ -494,6 +495,8 @@ def _create_player(self):
         if not self._player:
             try:
                 self._player = Player(self)
+                if self._publish_clock:
+                    self._player.start_clock_publishing()
             except Exception as ex:
                 qWarning('Error starting player; aborting publish: %s' % str(ex))
                 return False

rqt_bag/src/rqt_bag/bag_widget.py

@@ -55,7 +55,7 @@ class BagWidget(QWidget):
     Widget for use with Bag class to display and replay bag files
     Handles all widget callbacks and contains the instance of BagTimeline for storing visualizing bag data
     """
-    def __init__(self, context):
+    def __init__(self, context, publish_clock):
         """
         :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext''
         """
@@ -66,7 +66,7 @@ def __init__(self, context):
 
         self.setObjectName('BagWidget')
 
-        self._timeline = BagTimeline(context)
+        self._timeline = BagTimeline(context, publish_clock)
         self.graphics_view.setScene(self._timeline)
 
         self.graphics_view.resizeEvent = self._resizeEvent

rqt_bag/src/rqt_bag/player.py

@@ -35,10 +35,11 @@
 """
 
 import rospy
-
+import rosgraph_msgs
 
 from python_qt_binding.QtCore import QObject
 
+CLOCK_TOPIC = "/clock"
 
 class Player(QObject):
     """
@@ -51,6 +52,8 @@ def __init__(self, timeline):
         self._publishing = set()
         self._publishers = {}
 
+        self._publish_clock = False
+
     def is_publishing(self, topic):
         return topic in self._publishing
 
@@ -71,9 +74,32 @@ def stop_publishing(self, topic):
 
         self._publishing.remove(topic)
 
+    def start_clock_publishing(self):
+        if CLOCK_TOPIC not in self._publishers:
+            # Activate clock publishing only if the publisher was created successful
+            self._publish_clock = self.create_publisher(CLOCK_TOPIC, rosgraph_msgs.msg.Clock())
+
+    def stop_clock_publishing(self):
+        self._publish_clock = False
+        if CLOCK_TOPIC in self._publishers:
+            self._publishers[CLOCK_TOPIC].unregister()
+            del self._publishers[CLOCK_TOPIC]
+
     def stop(self):
         for topic in list(self._publishing):
             self.stop_publishing(topic)
+        self.stop_clock_publishing()
+
+    def create_publisher(self, topic, msg):
+        try:
+            self._publishers[topic] = rospy.Publisher(topic, type(msg))
+            return True
+        except Exception as ex:
+            # Any errors, stop listening/publishing to this topic
+            rospy.logerr('Error creating publisher on topic %s for type %s. \nError text: %s' % (topic, str(type(msg)), str(ex)))
+            if topic != CLOCK_TOPIC:
+                self.stop_publishing(topic)
+            return False
 
     def message_viewed(self, bag, msg_data):
         """
@@ -85,16 +111,16 @@ def message_viewed(self, bag, msg_data):
         if self.timeline.play_speed <= 0.0:
             return
 
-        topic, msg, _ = msg_data
+        topic, msg, clock = msg_data
 
         # Create publisher if this is the first message on the topic
         if topic not in self._publishers:
-            try:
-                self._publishers[topic] = rospy.Publisher(topic, type(msg))
-            except Exception as ex:
-                # Any errors, stop listening/publishing to this topic
-                rospy.logerr('Error creating publisher on topic %s for type %s. \nError text: %s' % (topic, str(type(msg)), str(ex)))
-                self.stop_publishing(topic)
+            self.create_publisher(topic, msg)
+
+        if self._publish_clock:
+            time_msg = rosgraph_msgs.msg.Clock()
+            time_msg.clock = clock
+            self._publishers[CLOCK_TOPIC].publish(time_msg)
 
         self._publishers[topic].publish(msg)