Add delay option (ros2#789)

* Add delay option

Signed-off-by: kosuke55 <[email protected]>

Author: kosuke55

ros2bag/ros2bag/verb/play.py

@@ -82,6 +82,9 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
             '--clock', type=positive_float, nargs='?', const=40, default=0,
             help='Publish to /clock at a specific frequency in Hz, to act as a ROS Time Source. '
                  'Value must be positive. Defaults to not publishing.')
+        parser.add_argument(
+            '-d', '--delay', type=float, default=0.0,
+            help='Sleep SEC seconds before play. Valid range > 0.0')
 
     def main(self, *, args):  # noqa: D102
         qos_profile_overrides = {}  # Specify a valid default
@@ -116,6 +119,7 @@ def main(self, *, args):  # noqa: D102
         play_options.loop = args.loop
         play_options.topic_remapping_options = topic_remapping
         play_options.clock_publish_frequency = args.clock
+        play_options.delay = args.delay
 
         player = Player()
         player.play(storage_options, play_options)

rosbag2_py/src/rosbag2_py/_transport.cpp

@@ -225,6 +225,7 @@ PYBIND11_MODULE(_transport, m) {
   .def_readwrite("loop", &PlayOptions::loop)
   .def_readwrite("topic_remapping_options", &PlayOptions::topic_remapping_options)
   .def_readwrite("clock_publish_frequency", &PlayOptions::clock_publish_frequency)
+  .def_readwrite("delay", &PlayOptions::delay)
   ;
 
   py::class_<RecordOptions>(m, "RecordOptions")

rosbag2_transport/include/rosbag2_transport/play_options.hpp

@@ -44,6 +44,9 @@ struct PlayOptions
   // Rate in Hz at which to publish to /clock.
   // 0 (or negative) means that no publisher will be created
   double clock_publish_frequency = 0.0;
+
+  // Sleep SEC seconds before play. Valid range > 0.0.
+  float delay = 0.0;
 };
 
 }  // namespace rosbag2_transport

rosbag2_transport/src/rosbag2_transport/player.cpp

@@ -213,8 +213,25 @@ bool Player::is_storage_completely_loaded() const
 void Player::play()
 {
   is_in_play_ = true;
+
+  float delay;
+  if (play_options_.delay >= 0.0) {
+    delay = play_options_.delay;
+  } else {
+    RCLCPP_WARN(
+      this->get_logger(),
+      "Invalid delay value: %f. Delay is disabled.",
+      play_options_.delay);
+    delay = 0.0;
+  }
+
   try {
     do {
+      if (delay > 0.0) {
+        RCLCPP_INFO_STREAM(this->get_logger(), "Sleep " << delay << " sec");
+        std::chrono::duration<float> duration(delay);
+        std::this_thread::sleep_for(duration);
+      }
       reader_->open(storage_options_, {"", rmw_get_serialization_format()});
       const auto starting_time = std::chrono::duration_cast<std::chrono::nanoseconds>(
         reader_->get_metadata().starting_time.time_since_epoch()).count();

rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp

@@ -43,6 +43,8 @@ TEST_F(RosBag2PlayTestFixture, play_bag_file_twice) {
   const size_t read_ahead_queue_size = 1000;
   const float rate = 1.0;
   const bool loop_playback = false;
+  double clock_publish_frequency = 0.0;
+  const float delay = 1.0;
 
   auto primitive_message1 = get_messages_basic_types()[0];
   primitive_message1->int32_value = test_value;
@@ -64,8 +66,9 @@ TEST_F(RosBag2PlayTestFixture, play_bag_file_twice) {
 
   auto await_received_messages = sub_->spin_subscriptions();
 
-  rosbag2_transport::PlayOptions play_options =
-  {read_ahead_queue_size, "", rate, {}, {}, loop_playback, {}};
+  rosbag2_transport::PlayOptions play_options = {
+    read_ahead_queue_size, "", rate, {}, {}, loop_playback, {},
+    clock_publish_frequency, delay};
   auto player = std::make_shared<rosbag2_transport::Player>(
     std::move(
       reader), storage_options_, play_options);
@@ -100,6 +103,8 @@ TEST_F(RosBag2PlayTestFixture, messages_played_in_loop) {
   const size_t read_ahead_queue_size = 1000;
   const float rate = 1.0;
   const bool loop_playback = true;
+  const double clock_publish_frequency = 0.0;
+  const float delay = 1.0;
 
   auto primitive_message1 = get_messages_basic_types()[0];
   primitive_message1->int32_value = test_value;
@@ -122,7 +127,7 @@ TEST_F(RosBag2PlayTestFixture, messages_played_in_loop) {
   auto await_received_messages = sub_->spin_subscriptions();
 
   rosbag2_transport::PlayOptions play_options{read_ahead_queue_size, "", rate, {}, {},
-    loop_playback, {}};
+    loop_playback, {}, clock_publish_frequency, delay};
   auto player = std::make_shared<rosbag2_transport::Player>(
     std::move(
       reader), storage_options_, play_options);

rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp

@@ -78,8 +78,7 @@ TEST_F(PlayerTestFixture, playing_respects_relative_timing_of_stored_messages)
   // messages
   auto start = std::chrono::steady_clock::now();
   auto player = std::make_shared<rosbag2_transport::Player>(
-    std::move(
-      reader), storage_options_, play_options_);
+    std::move(reader), storage_options_, play_options_);
   player->play();
   auto replay_time = std::chrono::steady_clock::now() - start;
 
@@ -113,8 +112,7 @@ TEST_F(PlayerTestFixture, playing_respects_rate)
     prepared_mock_reader->prepare(messages, topics_and_types);
     auto reader = std::make_unique<rosbag2_cpp::Reader>(std::move(prepared_mock_reader));
     auto player = std::make_shared<rosbag2_transport::Player>(
-      std::move(
-        reader), storage_options_, play_options_);
+      std::move(reader), storage_options_, play_options_);
     auto start = std::chrono::steady_clock::now();
     player->play();
     auto replay_time = std::chrono::steady_clock::now() - start;
@@ -131,8 +129,7 @@ TEST_F(PlayerTestFixture, playing_respects_rate)
     prepared_mock_reader->prepare(messages, topics_and_types);
     auto reader = std::make_unique<rosbag2_cpp::Reader>(std::move(prepared_mock_reader));
     auto player = std::make_shared<rosbag2_transport::Player>(
-      std::move(
-        reader), storage_options_, play_options_);
+      std::move(reader), storage_options_, play_options_);
     auto start = std::chrono::steady_clock::now();
     player->play();
     auto replay_time = std::chrono::steady_clock::now() - start;
@@ -148,8 +145,7 @@ TEST_F(PlayerTestFixture, playing_respects_rate)
     prepared_mock_reader->prepare(messages, topics_and_types);
     auto reader = std::make_unique<rosbag2_cpp::Reader>(std::move(prepared_mock_reader));
     auto player = std::make_shared<rosbag2_transport::Player>(
-      std::move(
-        reader), storage_options_, play_options_);
+      std::move(reader), storage_options_, play_options_);
     auto start = std::chrono::steady_clock::now();
     player->play();
     auto replay_time = std::chrono::steady_clock::now() - start;
@@ -165,8 +161,7 @@ TEST_F(PlayerTestFixture, playing_respects_rate)
     prepared_mock_reader->prepare(messages, topics_and_types);
     auto reader = std::make_unique<rosbag2_cpp::Reader>(std::move(prepared_mock_reader));
     auto player = std::make_shared<rosbag2_transport::Player>(
-      std::move(
-        reader), storage_options_, play_options_);
+      std::move(reader), storage_options_, play_options_);
     auto start = std::chrono::steady_clock::now();
     player->play();
     auto replay_time = std::chrono::steady_clock::now() - start;
@@ -182,12 +177,70 @@ TEST_F(PlayerTestFixture, playing_respects_rate)
     prepared_mock_reader->prepare(messages, topics_and_types);
     auto reader = std::make_unique<rosbag2_cpp::Reader>(std::move(prepared_mock_reader));
     auto player = std::make_shared<rosbag2_transport::Player>(
-      std::move(
-        reader), storage_options_, play_options_);
+      std::move(reader), storage_options_, play_options_);
     auto start = std::chrono::steady_clock::now();
     player->play();
     auto replay_time = std::chrono::steady_clock::now() - start;
 
     ASSERT_THAT(replay_time, Gt(message_time_difference));
   }
 }
+
+TEST_F(PlayerTestFixture, playing_respects_delay)
+{
+  auto primitive_message1 = get_messages_strings()[0];
+  primitive_message1->string_value = "Hello World 1";
+
+  auto primitive_message2 = get_messages_strings()[0];
+  primitive_message2->string_value = "Hello World 2";
+
+  auto message_time_difference = std::chrono::seconds(1);
+  auto topics_and_types =
+    std::vector<rosbag2_storage::TopicMetadata>{{"topic1", "test_msgs/Strings", "", ""}};
+  std::vector<std::shared_ptr<rosbag2_storage::SerializedBagMessage>> messages =
+  {serialize_test_message("topic1", 0, primitive_message1),
+    serialize_test_message("topic1", 0, primitive_message2)};
+
+  messages[0]->time_stamp = 100;
+  messages[1]->time_stamp =
+    messages[0]->time_stamp + std::chrono::nanoseconds(message_time_difference).count();
+
+  float delay_margin = 1.0;
+
+  // Sleep 5.0 seconds before play
+  {
+    play_options_.delay = 5.0;
+    std::chrono::duration<float> delay(play_options_.delay);
+    std::chrono::duration<float> delay_uppper(play_options_.delay + delay_margin);
+
+    auto prepared_mock_reader = std::make_unique<MockSequentialReader>();
+    prepared_mock_reader->prepare(messages, topics_and_types);
+    auto reader = std::make_unique<rosbag2_cpp::Reader>(std::move(prepared_mock_reader));
+    auto player = std::make_shared<rosbag2_transport::Player>(
+      std::move(reader), storage_options_, play_options_);
+    auto start = std::chrono::steady_clock::now();
+    player->play();
+    auto replay_time = std::chrono::steady_clock::now() - start;
+
+    ASSERT_THAT(replay_time, Gt(message_time_difference + delay));
+    ASSERT_THAT(replay_time, Lt(message_time_difference + delay_uppper));
+  }
+
+  // Invalid value should result in playing at default delay 0.0
+  {
+    play_options_.delay = -5.0;
+    std::chrono::duration<float> delay_uppper(delay_margin);
+
+    auto prepared_mock_reader = std::make_unique<MockSequentialReader>();
+    prepared_mock_reader->prepare(messages, topics_and_types);
+    auto reader = std::make_unique<rosbag2_cpp::Reader>(std::move(prepared_mock_reader));
+    auto player = std::make_shared<rosbag2_transport::Player>(
+      std::move(reader), storage_options_, play_options_);
+    auto start = std::chrono::steady_clock::now();
+    player->play();
+    auto replay_time = std::chrono::steady_clock::now() - start;
+
+    ASSERT_THAT(replay_time, Gt(message_time_difference));
+    ASSERT_THAT(replay_time, Lt(message_time_difference + delay_uppper));
+  }
+}