From 3cd9a98d44566f9218e533e1ae412026242e7a7e Mon Sep 17 00:00:00 2001 From: umiklos Date: Mon, 13 Jul 2026 15:36:30 +0200 Subject: [PATCH] Add playback progress output to rosbag2 player --- .../clocks/time_controller_clock.hpp | 4 +- .../clocks/time_controller_clock.cpp | 6 +- .../include/rosbag2_transport/player.hpp | 8 +++ .../src/rosbag2_transport/player.cpp | 60 ++++++++++++++++++- .../mock_sequential_reader.hpp | 5 ++ .../test/rosbag2_transport/test_play.cpp | 36 +++++++++++ 6 files changed, 114 insertions(+), 5 deletions(-) diff --git a/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp b/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp index 692972b93a..9b8bec890f 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp @@ -39,8 +39,8 @@ class TimeControllerClock : public PlayerClock * \param now_fn: Function used to get the current steady time * defaults to std::chrono::steady_clock::now * Used to control for unit testing, or for specialized needs - * \param sleep_time_while_paused: Amount of time to sleep in `sleep_until` when the clock - * is paused. Allows the caller to spin at a defined rate while receiving `false` + * \param sleep_time_while_paused: Maximum interval used by `sleep_until` while the clock + * is paused or waiting. Allows the caller to spin at a defined rate while receiving `false` * \param paused: Start the clock paused */ ROSBAG2_CPP_PUBLIC diff --git a/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp b/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp index 0ad1939561..35811c3d2a 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp @@ -175,8 +175,10 @@ bool TimeControllerClock::sleep_until(rcutils_time_point_value_t until) } else { const auto steady_until = impl_->ros_to_steady(until); // wait only if necessary for performance - if (steady_until > impl_->now_fn()) { - impl_->cv.wait_until(lock, steady_until); + const auto steady_now = impl_->now_fn(); + if (steady_until > steady_now) { + const auto wakeup_time = std::min(steady_until, steady_now + impl_->sleep_time_while_paused); + impl_->cv.wait_until(lock, wakeup_time); } } if (impl_->paused) { diff --git a/rosbag2_transport/include/rosbag2_transport/player.hpp b/rosbag2_transport/include/rosbag2_transport/player.hpp index 47673e5168..bb0bb7cd9f 100644 --- a/rosbag2_transport/include/rosbag2_transport/player.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player.hpp @@ -204,6 +204,7 @@ class Player : public rclcpp::Node bool publish_message(rosbag2_storage::SerializedBagMessageSharedPtr message); static constexpr double read_ahead_lower_bound_percentage_ = 0.9; static const std::chrono::milliseconds queue_read_wait_period_; + static const std::chrono::milliseconds progress_bar_update_period_; std::atomic_bool cancel_wait_for_next_message_{false}; std::mutex reader_mutex_; @@ -216,6 +217,10 @@ class Player : public rclcpp::Node void add_keyboard_callbacks(); void create_control_services(); + void print_playback_progress( + rcutils_time_point_value_t current_time, + const char * state, + bool force = false); rosbag2_storage::StorageOptions storage_options_; rosbag2_transport::PlayOptions play_options_; @@ -229,6 +234,9 @@ class Player : public rclcpp::Node skip_message_in_main_play_loop_mutex_) = false; rcutils_time_point_value_t starting_time_; + rcutils_time_point_value_t bag_duration_ns_ = 0; + rcutils_time_point_value_t last_logged_playback_timestamp_ = -1; + bool has_playback_progress_output_ = false; // control services rclcpp::Service::SharedPtr srv_pause_; diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index c8e9e5f58f..ab56453956 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -147,6 +148,7 @@ Player::Player( auto metadata = reader_->get_metadata(); starting_time_ = std::chrono::duration_cast( metadata.starting_time.time_since_epoch()).count(); + bag_duration_ns_ = metadata.duration.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) { @@ -157,6 +159,8 @@ Player::Player( ". Negative start offset ignored."); } else { starting_time_ += play_options_.start_offset; + bag_duration_ns_ = std::max( + 0, bag_duration_ns_ - play_options_.start_offset); } clock_ = std::make_unique( starting_time_, std::chrono::steady_clock::now, @@ -186,6 +190,9 @@ Player::~Player() const std::chrono::milliseconds Player::queue_read_wait_period_ = std::chrono::milliseconds(100); +const std::chrono::milliseconds +Player::progress_bar_update_period_ = std::chrono::milliseconds(100); + bool Player::is_storage_completely_loaded() const { if (storage_loading_future_.valid() && @@ -198,6 +205,8 @@ bool Player::is_storage_completely_loaded() const void Player::play() { + last_logged_playback_timestamp_ = -1; + has_playback_progress_output_ = false; rclcpp::Duration delay(0, 0); if (play_options_.delay >= rclcpp::Duration(0, 0)) { delay = play_options_.delay; @@ -219,6 +228,7 @@ void Player::play() reader_->seek(starting_time_); clock_->jump(starting_time_); } + print_playback_progress(starting_time_, is_paused() ? "PAUSED" : "RUNNING", true); storage_loading_future_ = std::async(std::launch::async, [this]() {load_storage_content();}); wait_for_filled_queue(); play_messages_from_queue(); @@ -235,6 +245,11 @@ void Player::play() is_ready_to_play_from_queue_ = false; ready_to_play_from_queue_cv_.notify_all(); + if (has_playback_progress_output_) { + std::fputc('\n', stdout); + std::fflush(stdout); + } + // Wait for all published messages to be acknowledged. if (play_options_.wait_acked_timeout >= 0) { std::chrono::milliseconds timeout(play_options_.wait_acked_timeout); @@ -264,12 +279,14 @@ void Player::play() void Player::pause() { clock_->pause(); + print_playback_progress(clock_->now(), "PAUSED", true); RCLCPP_INFO_STREAM(get_logger(), "Pausing play."); } void Player::resume() { clock_->resume(); + print_playback_progress(clock_->now(), "RUNNING", true); RCLCPP_INFO_STREAM(get_logger(), "Resuming play."); } @@ -352,6 +369,9 @@ bool Player::play_next() { next_message_published = publish_message(message_ptr); clock_->jump(message_ptr->time_stamp); + if (next_message_published) { + print_playback_progress(message_ptr->time_stamp, "PAUSED", true); + } } message_queue_.pop(); message_ptr = peek_next_message_from_queue(); @@ -397,6 +417,7 @@ void Player::seek(rcutils_time_point_value_t time_point) while (message_queue_.pop()) {} reader_->seek(time_point); clock_->jump(time_point); + print_playback_progress(time_point, is_paused() ? "PAUSED" : "RUNNING", true); // Restart queuing thread if it has finished running (previously reached end of bag), // otherwise, queueing should continue automatically after releasing mutex if (is_storage_completely_loaded() && rclcpp::ok()) { @@ -464,6 +485,7 @@ void Player::play_messages_from_queue() // Do not move on until sleep_until returns true // It will always sleep, so this is not a tight busy loop on pause while (rclcpp::ok() && !clock_->sleep_until(message_ptr->time_stamp)) { + print_playback_progress(clock_->now(), is_paused() ? "PAUSED" : "RUNNING"); if (std::atomic_exchange(&cancel_wait_for_next_message_, false)) { break; } @@ -476,11 +498,14 @@ void Player::play_messages_from_queue() message_ptr = peek_next_message_from_queue(); continue; } - publish_message(message_ptr); + if (publish_message(message_ptr)) { + print_playback_progress(message_ptr->time_stamp, "RUNNING"); + } } message_queue_.pop(); message_ptr = peek_next_message_from_queue(); } + print_playback_progress(clock_->now(), is_paused() ? "PAUSED" : "RUNNING", true); // while we're in pause state, make sure we don't return // if we happen to be at the end of queue while (is_paused() && rclcpp::ok()) { @@ -594,6 +619,39 @@ bool Player::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr mess return message_published; } +void Player::print_playback_progress( + rcutils_time_point_value_t current_time, + const char * state, + bool force) +{ + if (!force) { + if (last_logged_playback_timestamp_ >= 0 && + current_time - last_logged_playback_timestamp_ < + std::chrono::duration_cast(progress_bar_update_period_).count()) + { + return; + } + } + + auto clamped_time = std::max(current_time, starting_time_); + auto playback_duration_ns = std::max(0, clamped_time - starting_time_); + auto playback_duration_s = RCUTILS_NS_TO_S(static_cast(playback_duration_ns)); + auto bag_duration_s = RCUTILS_NS_TO_S(static_cast(bag_duration_ns_)); + auto bag_time_s = RCUTILS_NS_TO_S(static_cast(clamped_time)); + + std::fprintf( + stdout, + "\r [%-7s] Bag Time: %13.6f Duration: %.6f / %.6f ", + state, + bag_time_s, + playback_duration_s, + bag_duration_s); + std::fflush(stdout); + + last_logged_playback_timestamp_ = clamped_time; + has_playback_progress_output_ = true; +} + void Player::add_key_callback( KeyboardHandler::KeyCode key, const std::function & cb, diff --git a/rosbag2_transport/test/rosbag2_transport/mock_sequential_reader.hpp b/rosbag2_transport/test/rosbag2_transport/mock_sequential_reader.hpp index 84fe1d3569..c31572be0b 100644 --- a/rosbag2_transport/test/rosbag2_transport/mock_sequential_reader.hpp +++ b/rosbag2_transport/test/rosbag2_transport/mock_sequential_reader.hpp @@ -111,6 +111,11 @@ class MockSequentialReader : public rosbag2_cpp::reader_interfaces::BaseReaderIn topics_ = std::move(topics); } + void set_metadata(const rosbag2_storage::BagMetadata & metadata) + { + metadata_ = metadata; + } + size_t max_messages_per_file() const { return max_messages_per_file_; diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 0b090752d6..6d17a22b57 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -109,6 +109,42 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics) ElementsAre(40.0f, 2.0f, 0.0f))))); } +TEST_F(RosBag2PlayTestFixture, playback_progress_is_printed_to_stdout) +{ + auto primitive_message = get_messages_basic_types()[0]; + primitive_message->int32_value = 42; + + auto topic_types = std::vector{ + {"topic1", "test_msgs/BasicTypes", "", ""}, + }; + + std::vector> messages = { + serialize_test_message("topic1", 1000000000, primitive_message), + serialize_test_message("topic1", 2000000000, primitive_message), + }; + + rosbag2_storage::BagMetadata metadata; + metadata.starting_time = std::chrono::high_resolution_clock::time_point(std::chrono::seconds(1)); + metadata.duration = std::chrono::seconds(1); + metadata.message_count = messages.size(); + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, topic_types); + prepared_mock_reader->set_metadata(metadata); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + + auto player = std::make_shared( + std::move(reader), storage_options_, play_options_); + + testing::internal::CaptureStdout(); + player->play(); + const auto output = testing::internal::GetCapturedStdout(); + + EXPECT_THAT(output, HasSubstr("[RUNNING]")); + EXPECT_THAT(output, HasSubstr("Bag Time:")); + EXPECT_THAT(output, HasSubstr("Duration: 1.000000 / 1.000000")); +} + TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics_with_unknown_type) { auto primitive_message1 = get_messages_basic_types()[0];