diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index 3d66208e1e..2a7f2fc0da 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -52,6 +52,9 @@ def add_arguments(self, parser, cli_name): # noqa: D102 '-l', '--loop', action='store_true', help='enables loop playback when playing a bagfile: it starts back at the beginning ' 'on reaching the end and plays indefinitely.') + parser.add_argument( + '-p', '--paused', action='store_true', + help='start rosbag2 in paused mode. No messages are played at startup.') parser.add_argument( '--remap', '-m', default='', nargs='+', help='list of topics to be remapped: in the form ' @@ -82,4 +85,5 @@ def main(self, *, args): # noqa: D102 topics=args.topics, qos_profile_overrides=qos_profile_overrides, loop=args.loop, + paused=args.paused, topic_remapping=args.remap) diff --git a/rosbag2_transport/CMakeLists.txt b/rosbag2_transport/CMakeLists.txt index e4b0dbd284..5fe56726b2 100644 --- a/rosbag2_transport/CMakeLists.txt +++ b/rosbag2_transport/CMakeLists.txt @@ -22,8 +22,10 @@ endif() find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) +find_package(lifecycle_msgs REQUIRED) find_package(rcl REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(rcutils REQUIRED) find_package(rmw REQUIRED) find_package(rosbag2_compression REQUIRED) @@ -46,8 +48,10 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME} + lifecycle_msgs rcl rclcpp + rclcpp_lifecycle rcutils rmw rosbag2_compression @@ -125,7 +129,9 @@ function(create_tests_for_rmw_implementation) $ AMENT_DEPS ament_index_cpp + lifecycle_msgs rclcpp + rclcpp_lifecycle rosbag2_cpp rosbag2_test_common test_msgs diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp index b616a76591..208cb77a34 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -39,6 +39,7 @@ struct PlayOptions std::unordered_map topic_qos_profile_overrides = {}; bool loop = false; + bool paused = false; std::vector topic_remapping_options = {}; }; diff --git a/rosbag2_transport/include/rosbag2_transport/rosbag2_transport.hpp b/rosbag2_transport/include/rosbag2_transport/rosbag2_transport.hpp index 02bcb37e00..7569dc5bc5 100644 --- a/rosbag2_transport/include/rosbag2_transport/rosbag2_transport.hpp +++ b/rosbag2_transport/include/rosbag2_transport/rosbag2_transport.hpp @@ -84,6 +84,18 @@ class Rosbag2Transport ROSBAG2_TRANSPORT_PUBLIC void print_bag_info(const std::string & uri, const std::string & storage_id); + /** + * Resume playback. + */ + ROSBAG2_TRANSPORT_PUBLIC + void resume(); + + /** + * Pause playback. + */ + ROSBAG2_TRANSPORT_PUBLIC + void pause(); + private: std::shared_ptr setup_node( std::string node_prefix = "", diff --git a/rosbag2_transport/package.xml b/rosbag2_transport/package.xml index 8732c4ead1..e789e2611d 100644 --- a/rosbag2_transport/package.xml +++ b/rosbag2_transport/package.xml @@ -10,8 +10,10 @@ ament_cmake_ros + lifecycle_msgs python_cmake_module rclcpp + rclcpp_lifecycle rosbag2_compression rosbag2_cpp rosbag2_storage diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 4309e46053..0602f4b5ff 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -22,6 +22,8 @@ #include #include +#include "lifecycle_msgs/msg/state.hpp" + #include "rcl/graph.h" #include "rclcpp/rclcpp.hpp" @@ -75,10 +77,13 @@ namespace rosbag2_transport const std::chrono::milliseconds Player::queue_read_wait_period_ = std::chrono::milliseconds(100); +const std::chrono::milliseconds +Player::pause_sleep_period_ = std::chrono::milliseconds(30); Player::Player( std::shared_ptr reader, std::shared_ptr rosbag2_transport) -: reader_(std::move(reader)), rosbag2_transport_(rosbag2_transport) +: reader_(std::move(reader)), played_all_(false), + rosbag2_transport_(rosbag2_transport) {} bool Player::is_storage_completely_loaded() const @@ -102,6 +107,14 @@ void Player::play(const PlayOptions & options) wait_for_filled_queue(options); + rosbag2_transport_->configure(); + // If playback should not be paused in the beginning, activate lifecycle node right away. + // If playback should be paused, move on. + // Playback will detect when node is activated, either from here or externally. + if (!options.paused) { + rosbag2_transport_->activate(); + } + play_messages_from_queue(options); } @@ -158,6 +171,7 @@ void Player::enqueue_up_to_boundary(const TimePoint & time_first_message, uint64 void Player::play_messages_from_queue(const PlayOptions & options) { start_time_ = std::chrono::system_clock::now(); + paused_duration_ = std::chrono::nanoseconds(0); do { play_messages_until_queue_empty(options); if (!is_storage_completely_loaded() && rclcpp::ok()) { @@ -166,6 +180,8 @@ void Player::play_messages_from_queue(const PlayOptions & options) "increasing the --read-ahead-queue-size option."); } } while (!is_storage_completely_loaded() && rclcpp::ok()); + + played_all_ = true; } void Player::play_messages_until_queue_empty(const PlayOptions & options) @@ -178,12 +194,37 @@ void Player::play_messages_until_queue_empty(const PlayOptions & options) rate = options.rate; } + static size_t counter = 0; while (message_queue_.try_dequeue(message) && rclcpp::ok()) { std::this_thread::sleep_until( start_time_ + std::chrono::duration_cast( - 1.0 / rate * message.time_since_start)); + 1.0 / rate * message.time_since_start + paused_duration_)); + + if (rosbag2_transport_->get_current_state().id() != + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + ROSBAG2_TRANSPORT_LOG_INFO("Paused"); + std::chrono::time_point pause_start = + std::chrono::system_clock::now(); + + // Sleep until activated externally + while (rosbag2_transport_->get_current_state().id() != + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && rclcpp::ok()) + { + std::this_thread::sleep_for(pause_sleep_period_); + } + + paused_duration_ += std::chrono::system_clock::now() - pause_start; + + if (rclcpp::ok()) { + ROSBAG2_TRANSPORT_LOG_INFO("Resumed"); + } + } + if (rclcpp::ok()) { publishers_[message.message->topic_name]->publish(message.message->serialized_data); + // TODO(mabelzhang) TEMPORARY, remove when done debugging. And counter above. + fprintf(stderr, "publishing message %zu\n", (++counter)); } } } diff --git a/rosbag2_transport/src/rosbag2_transport/player.hpp b/rosbag2_transport/src/rosbag2_transport/player.hpp index cc7a612ae1..eb8a967109 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.hpp +++ b/rosbag2_transport/src/rosbag2_transport/player.hpp @@ -60,12 +60,15 @@ class Player void play_messages_from_queue(const PlayOptions & options); void play_messages_until_queue_empty(const PlayOptions & options); void prepare_publishers(const PlayOptions & options); + static constexpr double read_ahead_lower_bound_percentage_ = 0.9; static const std::chrono::milliseconds queue_read_wait_period_; - + static const std::chrono::milliseconds pause_sleep_period_; std::shared_ptr reader_; moodycamel::ReaderWriterQueue message_queue_; std::chrono::time_point start_time_; + std::chrono::nanoseconds paused_duration_; + bool played_all_; mutable std::future storage_loading_future_; std::shared_ptr rosbag2_transport_; std::unordered_map> publishers_; diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 8bdf39a504..3081ce5701 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -23,6 +23,8 @@ #include #include +#include "lifecycle_msgs/msg/state.hpp" + #include "rosbag2_cpp/writer.hpp" #include "rosbag2_transport/logging.hpp" @@ -184,7 +186,9 @@ Recorder::create_subscription( void Recorder::record_messages() const { - spin(node_); + rclcpp::executors::MultiThreadedExecutor exe; + exe.add_node(node_->get_node_base_interface()); + exe.spin(); } std::string Recorder::serialized_offered_qos_profiles_for_topic(const std::string & topic_name) diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_node.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_node.cpp index 6a2b92dd6b..4256bcace8 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_node.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_node.cpp @@ -34,11 +34,11 @@ namespace rosbag2_transport { Rosbag2Node::Rosbag2Node(const std::string & node_name) -: rclcpp::Node(node_name) +: rclcpp_lifecycle::LifecycleNode(node_name) {} Rosbag2Node::Rosbag2Node(const std::string & node_name, const rclcpp::NodeOptions & options) -: rclcpp::Node(node_name, options) +: rclcpp_lifecycle::LifecycleNode(node_name, options) {} std::shared_ptr Rosbag2Node::create_generic_publisher( diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_node.hpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_node.hpp index 785f8688d2..96a2bac578 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_node.hpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_node.hpp @@ -21,9 +21,10 @@ #include #include -#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp/serialized_message.hpp" #include "rclcpp/node_options.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rcpputils/shared_library.hpp" #include "rcutils/types.h" @@ -33,7 +34,7 @@ namespace rosbag2_transport { -class Rosbag2Node : public rclcpp::Node +class Rosbag2Node : public rclcpp_lifecycle::LifecycleNode { public: explicit Rosbag2Node(const std::string & node_name); diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp index c46144ff39..df28aad28b 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp @@ -126,4 +126,24 @@ void Rosbag2Transport::print_bag_info(const std::string & uri, const std::string Formatter::format_bag_meta_data(metadata); } +void Rosbag2Transport::resume() +{ + if (transport_node_) { + transport_node_->activate(); + } else { + ROSBAG2_TRANSPORT_LOG_ERROR( + "transport_node_ not initialized. Call play() before activating lifecycle node."); + } +} + +void Rosbag2Transport::pause() +{ + if (transport_node_) { + transport_node_->deactivate(); + } else { + ROSBAG2_TRANSPORT_LOG_ERROR( + "transport_node_ not initialized. Call play() before deactivating lifecycle node."); + } +} + } // namespace rosbag2_transport diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp index 10163ed90b..2772879315 100644 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp +++ b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp @@ -236,6 +236,7 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k "topics", "qos_profile_overrides", "loop", + "paused", "topic_remapping", nullptr }; @@ -248,9 +249,10 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k PyObject * topics = nullptr; PyObject * qos_profile_overrides{nullptr}; bool loop = false; + bool paused = false; PyObject * topic_remapping = nullptr; if (!PyArg_ParseTupleAndKeywords( - args, kwargs, "sss|kfOObO", const_cast(kwlist), + args, kwargs, "sss|kfOObbO", const_cast(kwlist), &uri, &storage_id, &node_prefix, @@ -259,6 +261,7 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k &topics, &qos_profile_overrides, &loop, + &paused, &topic_remapping)) { return nullptr; @@ -271,6 +274,7 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k play_options.read_ahead_queue_size = read_ahead_queue_size; play_options.rate = rate; play_options.loop = loop; + play_options.paused = paused; if (topics) { PyObject * topic_iterator = PyObject_GetIter(topics); diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 58e47a4e36..294f964d67 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -202,6 +202,82 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics) EXPECT_THAT(replayed_test_arrays, SizeIs(Ge(2u))); } +TEST_F(RosBag2PlayTestFixture, starting_paused_can_resume) +{ + auto primitive_message1 = get_messages_basic_types()[0]; + primitive_message1->int32_value = 42; + + auto complex_message1 = get_messages_arrays()[0]; + complex_message1->float32_values = {{40.0f, 2.0f, 0.0f}}; + complex_message1->bool_values = {{true, false, true}}; + + auto topic_types = std::vector{ + {"topic1", "test_msgs/BasicTypes", "", ""}, + {"topic2", "test_msgs/Arrays", "", ""}, + }; + + // 3 messages of each type to be played + std::vector> messages = + {serialize_test_message("topic1", 500, primitive_message1), + serialize_test_message("topic1", 700, primitive_message1), + serialize_test_message("topic1", 900, primitive_message1), + serialize_test_message("topic2", 550, complex_message1), + serialize_test_message("topic2", 750, complex_message1), + serialize_test_message("topic2", 950, complex_message1)}; + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, topic_types); + reader_ = std::make_unique(std::move(prepared_mock_reader)); + + // Due to a problem related to the subscriber, we play many (3) messages but make the subscriber + // node spin only until 2 have arrived. Hence the 2 as `launch_subscriber()` argument. + sub_->add_subscription("/topic1", 2); + sub_->add_subscription("/topic2", 2); + + auto await_received_messages = sub_->spin_subscriptions(); + + // Start playback paused + play_options_.paused = true; + + Rosbag2Transport rosbag2_transport(reader_, writer_, info_); + + std::future future_handle = std::async( + std::launch::async, [&rosbag2_transport]() mutable { + // Wait for node initialization, then resume + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + rosbag2_transport.resume(); + }); + + rosbag2_transport.play(storage_options_, play_options_); + + await_received_messages.get(); + + auto replayed_test_primitives = sub_->get_received_messages( + "/topic1"); + EXPECT_THAT(replayed_test_primitives, SizeIs(Ge(2u))); + EXPECT_THAT( + replayed_test_primitives, + Each(Pointee(Field(&test_msgs::msg::BasicTypes::int32_value, 42)))); + + auto replayed_test_arrays = sub_->get_received_messages( + "/topic2"); + EXPECT_THAT(replayed_test_arrays, SizeIs(Ge(2u))); + EXPECT_THAT( + replayed_test_arrays, + Each( + Pointee( + Field( + &test_msgs::msg::Arrays::bool_values, + ElementsAre(true, false, true))))); + EXPECT_THAT( + replayed_test_arrays, + Each( + Pointee( + Field( + &test_msgs::msg::Arrays::float32_values, + ElementsAre(40.0f, 2.0f, 0.0f))))); +} + class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture { public: diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp index 695830eebf..1051426e7a 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_timing.cpp @@ -156,3 +156,105 @@ TEST_F(Rosbag2TransportTestFixture, playing_respects_rate) rclcpp::shutdown(); } + +TEST_F(Rosbag2TransportTestFixture, playing_starts_paused) +{ + rclcpp::init(0, nullptr); + 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(); + + // Start unpaused normally + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, topics_and_types); + reader_ = std::make_unique(std::move(prepared_mock_reader)); + + play_options_.paused = false; + + auto start = std::chrono::steady_clock::now(); + Rosbag2Transport rosbag2_transport(reader_, writer_, info_); + rosbag2_transport.play(storage_options_, play_options_); + auto replay_time = std::chrono::steady_clock::now() - start; + + ASSERT_THAT(replay_time, Gt(message_time_difference)); + + // Start paused, resume after some time + prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, topics_and_types); + reader_ = std::make_unique(std::move(prepared_mock_reader)); + + play_options_.paused = true; + + start = std::chrono::steady_clock::now(); + rosbag2_transport = Rosbag2Transport(reader_, writer_, info_); + + auto pause_time = std::chrono::seconds(1); + + // Activate lifecycle node after pausing for some time + // Pass node by reference so that it is initialized by play() in the test + // before activate() call in the thread + std::future future_handle = std::async( + std::launch::async, [&rosbag2_transport, pause_time]() mutable { + std::this_thread::sleep_for(pause_time); + rosbag2_transport.resume(); + }); + + // Call play() to initialize the node and wait for activation + rosbag2_transport.play(storage_options_, play_options_); + // Time the play after full execution of play() + replay_time = std::chrono::steady_clock::now() - start; + + ASSERT_THAT(replay_time, Gt(message_time_difference + pause_time)); + + // Start unpaused, pause, unpause again + auto primitive_message3 = get_messages_strings()[0]; + primitive_message3->string_value = "Hello World 3"; + messages.push_back(serialize_test_message("topic1", 0, primitive_message3)); + messages[2]->time_stamp = + messages[1]->time_stamp + std::chrono::nanoseconds(message_time_difference).count(); + + prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, topics_and_types); + reader_ = std::make_unique(std::move(prepared_mock_reader)); + + play_options_.paused = false; + + start = std::chrono::steady_clock::now(); + rosbag2_transport = Rosbag2Transport(reader_, writer_, info_); + + // Pass node by reference so that it is initialized by play(). + future_handle = std::async( + std::launch::async, [&rosbag2_transport, message_time_difference]() mutable { + // Wait for node to initialize, pause + std::this_thread::sleep_for(message_time_difference); + rosbag2_transport.pause(); + + // Wait, resume + std::this_thread::sleep_for(message_time_difference); + rosbag2_transport.resume(); + }); + + rosbag2_transport.play(storage_options_, play_options_); + replay_time = std::chrono::steady_clock::now() - start; + + std::cerr << replay_time.count() << std::endl; + std::cerr << (3 * message_time_difference).count() << std::endl; + // 1 second for message 0 and 1 in parallel to initialization wait, + // 1 second for pause, and 1 second for message 2. + ASSERT_THAT(replay_time, Gt(3 * message_time_difference)); + + rclcpp::shutdown(); +} diff --git a/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp b/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp index 961ca08ace..546deb9e49 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_rosbag2_node.cpp @@ -46,6 +46,7 @@ class RosBag2NodeFixture : public Test RosBag2NodeFixture() { node_ = std::make_shared("rosbag2"); + exe_.add_node(node_->get_node_base_interface()); publisher_node_ = std::make_shared( "publisher_node", rclcpp::NodeOptions().start_parameter_event_publisher(false)); @@ -83,7 +84,7 @@ class RosBag2NodeFixture : public Test }); while (counter < expected_messages_number) { - rclcpp::spin_some(node_); + exe_.spin_some(); } return messages; } @@ -110,13 +111,14 @@ class RosBag2NodeFixture : public Test if ((clock::now() - start) > timeout) { return false; } - rclcpp::spin_some(node_); + exe_.spin_some(); } return true; } MemoryManagement memory_management_; std::shared_ptr node_; + rclcpp::executors::MultiThreadedExecutor exe_; rclcpp::Node::SharedPtr publisher_node_; std::vector> publishers_; };