diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index 1d71974824..d62b4425e2 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/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) diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 01fb7ae299..f0afd8043c 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/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_(m, "RecordOptions") diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp index e6971266d7..24b20a9fe5 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/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 diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 467091c85c..2148f8da28 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/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 duration(delay); + std::this_thread::sleep_for(duration); + } reader_->open(storage_options_, {"", rmw_get_serialization_format()}); const auto starting_time = std::chrono::duration_cast( reader_->get_metadata().starting_time.time_since_epoch()).count(); diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp index bb7a916bdb..135fbd2a83 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp +++ b/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( 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( std::move( reader), storage_options_, play_options); diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp index 1aad2668c8..2cde4e3071 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp +++ b/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( - 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(std::move(prepared_mock_reader)); auto player = std::make_shared( - 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(std::move(prepared_mock_reader)); auto player = std::make_shared( - 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(std::move(prepared_mock_reader)); auto player = std::make_shared( - 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(std::move(prepared_mock_reader)); auto player = std::make_shared( - 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,8 +177,7 @@ TEST_F(PlayerTestFixture, playing_respects_rate) prepared_mock_reader->prepare(messages, topics_and_types); auto reader = std::make_unique(std::move(prepared_mock_reader)); auto player = std::make_shared( - 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; @@ -191,3 +185,62 @@ TEST_F(PlayerTestFixture, playing_respects_rate) 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{{"topic1", "test_msgs/Strings", "", ""}}; + std::vector> 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 delay(play_options_.delay); + std::chrono::duration delay_uppper(play_options_.delay + delay_margin); + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, topics_and_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + auto player = std::make_shared( + 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 delay_uppper(delay_margin); + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, topics_and_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + auto player = std::make_shared( + 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)); + } +}