Add start-offset play option (ros2#931)

* Add `start_offset` field to `PlayOptions`, exposed to CLI as `ros2 bag play --start-offset SECONDS`, which allows for starting playback after the beginning of a bag.

Signed-off-by: Abrar Rahman Protyasha <[email protected]>

Author: aprotyas

Diff for: ros2bag/ros2bag/verb/play.py

@@ -90,6 +90,9 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
         parser.add_argument(
             '-p', '--start-paused', action='store_true', default=False,
             help='Start the playback player in a paused state.')
+        parser.add_argument(
+            '--start-offset', type=check_positive_float, default=0.0,
+            help='Start the playback player this many seconds into the bag file.')
 
     def main(self, *, args):  # noqa: D102
         qos_profile_overrides = {}  # Specify a valid default
@@ -127,6 +130,7 @@ def main(self, *, args):  # noqa: D102
         play_options.delay = args.delay
         play_options.disable_keyboard_controls = args.disable_keyboard_controls
         play_options.start_paused = args.start_paused
+        play_options.start_offset = args.start_offset
 
         player = Player()
         player.play(storage_options, play_options)

Diff for: rosbag2_py/src/rosbag2_py/_transport.cpp

@@ -74,6 +74,16 @@ struct OptionsWrapper : public T
     return RCUTILS_NS_TO_S(static_cast<double>(this->delay.nanoseconds()));
   }
 
+  void setStartOffset(double start_offset)
+  {
+    this->start_offset = static_cast<rcutils_time_point_value_t>(RCUTILS_S_TO_NS(start_offset));
+  }
+
+  double getStartOffset() const
+  {
+    return RCUTILS_NS_TO_S(static_cast<double>(this->start_offset));
+  }
+
   void setTopicQoSProfileOverrides(const py::dict & overrides)
   {
     py_dict = overrides;
@@ -233,6 +243,10 @@ PYBIND11_MODULE(_transport, m) {
     &PlayOptions::setDelay)
   .def_readwrite("disable_keyboard_controls", &PlayOptions::disable_keyboard_controls)
   .def_readwrite("start_paused", &PlayOptions::start_paused)
+  .def_property(
+    "start_offset",
+    &PlayOptions::getStartOffset,
+    &PlayOptions::setStartOffset)
   ;
 
   py::class_<RecordOptions>(m, "RecordOptions")

Diff for: rosbag2_transport/include/rosbag2_transport/play_options.hpp

@@ -53,6 +53,9 @@ struct PlayOptions
   // Start paused.
   bool start_paused = false;
 
+  // Time to start playback as an offset from the beginning of the bag.
+  rcutils_time_point_value_t start_offset = 0;
+
   bool disable_keyboard_controls = false;
   // keybindings
   KeyboardHandler::KeyCode pause_resume_toggle_key = KeyboardHandler::KeyCode::SPACE;

Diff for: rosbag2_transport/src/rosbag2_transport/player.cpp

@@ -142,6 +142,17 @@ Player::Player(
     auto metadata = reader_->get_metadata();
     starting_time_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
       metadata.starting_time.time_since_epoch()).count();
+    // If a non-default (positive) starting time offset is provided in PlayOptions,
+    // then add the offset to the starting time obtained from reader metadata
+    if (play_options_.start_offset < 0) {
+      RCLCPP_WARN_STREAM(
+        get_logger(),
+        "Invalid start offset value: " <<
+          RCUTILS_NS_TO_S(static_cast<double>(play_options_.start_offset)) <<
+          ". Negative start offset ignored.");
+    } else {
+      starting_time_ += play_options_.start_offset;
+    }
     clock_ = std::make_unique<rosbag2_cpp::TimeControllerClock>(
       starting_time_, std::chrono::steady_clock::now,
       std::chrono::milliseconds{100}, play_options_.start_paused);

Diff for: rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp

@@ -190,3 +190,43 @@ TEST_F(PlayerTestFixture, play_ignores_invalid_delay)
   EXPECT_THAT(replay_time, Gt(lower_expected_duration));
   EXPECT_THAT(replay_time, Lt(upper_expected_duration));
 }
+
+TEST_F(PlayerTestFixture, play_respects_start_offset)
+{
+  rclcpp::Duration start_delay(0, static_cast<uint32_t>(RCUTILS_S_TO_NS(0.5)));
+  rclcpp::Duration delay_margin(1, 0);
+
+  // Start 0.5 seconds into the bag
+  play_options_.start_offset = static_cast<rcutils_time_point_value_t>(RCUTILS_S_TO_NS(0.5));
+  // Subtract the delay since we've sought further into the bag
+  auto lower_expected_duration = message_time_difference - start_delay;
+  auto upper_expected_duration = message_time_difference - start_delay + delay_margin;
+  auto player = std::make_shared<rosbag2_transport::Player>(
+    std::move(reader), storage_options_, play_options_);
+
+  auto start = clock.now();
+  player->play();
+  auto replay_duration = clock.now() - start;
+
+  EXPECT_THAT(replay_duration, Gt(lower_expected_duration));
+  EXPECT_THAT(replay_duration, Lt(upper_expected_duration));
+}
+
+TEST_F(PlayerTestFixture, play_ignores_invalid_start_offset)
+{
+  rclcpp::Duration delay_margin(1, 0);
+
+  // Player should ignore an invalid (negative) start offset
+  play_options_.start_offset = -5;
+  auto lower_expected_duration = message_time_difference;
+  auto upper_expected_duration = message_time_difference + delay_margin;
+  auto player = std::make_shared<rosbag2_transport::Player>(
+    std::move(reader), storage_options_, play_options_);
+
+  auto start = clock.now();
+  player->play();
+  auto replay_duration = clock.now() - start;
+
+  EXPECT_THAT(replay_duration, Gt(lower_expected_duration));
+  EXPECT_THAT(replay_duration, Lt(upper_expected_duration));
+}