Don't record sim-time messages before first /clock (#1354)

* Wait for clock at discovery startup so that messages are not written at time 0

Signed-off-by: Emerson Knapp <emerson.b.knapp@gmail.com>
(cherry picked from commit da7528f)

README.md

@@ -93,6 +93,15 @@ In the same fashion, this auto discovery can be disabled with `--no-discovery`.
 If not further specified, `ros2 bag record` will create a new folder named to the current time stamp and stores all data within this folder.
 A user defined name can be given with `-o, --output`.
 
+#### Simulation time
+
+In ROS 2, "simulation time" refers to publishing a clock value on the `/clock` topic, instead of using the system clock to tell time.
+By passing `--use-sim-time` argument to `ros2 bag record`, we turn on this option for the recording node.
+Messages written to the bag will use the latest received value of `/clock` for the timestamp of the recorded message.
+
+Note: Until the first `/clock` message is received, the recorder will not write any messages.
+Before that message is received, the time is 0, which leads to a significant time jump once simulation time begins, making the bag essentially unplayable if messages are written first with time 0 and then time N from `/clock`.
+
 #### Splitting recorded bag files
 
 rosbag2 offers the capability to split bag files when they reach a maximum size or after a specified duration. By default rosbag2 will record all data into a single bag file, but this can be changed using the CLI options.

ros2bag/ros2bag/verb/record.py

@@ -129,7 +129,8 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
             help='Start the recorder in a paused state.')
         parser.add_argument(
             '--use-sim-time', action='store_true', default=False,
-            help='Use simulation time.')
+            help='Use simulation time for message timestamps by subscribing to the /clock topic. '
+                 'Until first /clock message is received, no messages will be written to bag.')
         parser.add_argument(
             '--node-name', type=str, default='rosbag2_recorder',
             help='Specify the recorder node name. Default is %(default)s.')

rosbag2_transport/src/rosbag2_transport/recorder.cpp

@@ -364,6 +364,18 @@ bool RecorderImpl::is_paused()
 
 void RecorderImpl::topics_discovery()
 {
+  // If using sim time - wait until /clock topic received before even creating subscriptions
+  if (record_options_.use_sim_time) {
+    RCLCPP_INFO(
+      node->get_logger(),
+      "use_sim_time set, waiting for /clock before starting recording...");
+    while (rclcpp::ok()) {
+      if (node->get_clock()->wait_until_started(record_options_.topic_polling_interval)) {
+        break;
+      }
+    }
+    RCLCPP_INFO(node->get_logger(), "Sim time /clock found, starting recording.");
+  }
   while (rclcpp::ok() && stop_discovery_ == false) {
     auto topics_to_subscribe =
       get_requested_or_available_topics();