diff --git a/README.md b/README.md index 96ebe64c46..04198b2e86 100644 --- a/README.md +++ b/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. diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index 13f771c387..e939201d34 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/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.') diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index da7ac7bad7..5c2fb2d857 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/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();