[backport galactic] Add delay option (#789) (#812)

Backport #789 to galactic

* Add delay option

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

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

@@ -220,6 +220,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));
+ }
+}