diff --git a/docs/design/rosbag2_record_replay_service.md b/docs/design/rosbag2_record_replay_service.md index 48ce2a524..981329dee 100644 --- a/docs/design/rosbag2_record_replay_service.md +++ b/docs/design/rosbag2_record_replay_service.md @@ -150,6 +150,10 @@ Added or changed parameters: Rename from `--exclude`. Exclude topics and services containing provided regular expression. +- `--progress-bar [ProgressBarFrequency]` + + Print a progress bar of the playback player at a specific frequency in Hz. Negative values mark an update for every published message, while a zero value disables the progress bar. Default to a positive low value. + `-e REGEX, --regex REGEX` affects both topics and services. ## Implementation Staging diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index c645de431..8d3d3863e 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -181,6 +181,23 @@ def add_arguments(self, parser, cli_name): # noqa: D102 '--log-level', type=str, default='info', choices=['debug', 'info', 'warn', 'error', 'fatal'], help='Logging level.') + parser.add_argument( + '--progress-bar', type=int, default=3, + help='Print a progress bar for the playback at a specific rate in times per second. ' + 'Default is %(default)d. ' + 'Negative values mark an update for every published message, while ' + ' a zero value disables the progress bar.', + metavar='PROGRESS_BAR_UPDATE_RATE') + parser.add_argument( + '--progress-bar-separation-lines', type=check_not_negative_int, default=2, + help='Number of lines to separate the progress bar from the rest of the ' + 'playback player output. Negative values are invalid. Default is %(default)d. ' + 'This option makes sense only if the progress bar is enabled. ' + 'Large values are useful if external log messages from other nodes are shorter ' + 'than the progress bar string and are printed at a rate higher than the ' + 'progress bar update rate. In these cases, a larger value will avoid ' + 'the external log message to be mixed with the progress bar string.', + metavar='NUM_SEPARATION_LINES') def get_playback_until_from_arg_group(self, playback_until_sec, playback_until_nsec) -> int: nano_scale = 1000 * 1000 * 1000 @@ -292,6 +309,9 @@ def main(self, *, args): # noqa: D102 'sent': MessageOrder.SENT_TIMESTAMP, }.get(args.message_order) + play_options.progress_bar_update_rate = args.progress_bar + play_options.progress_bar_separation_lines = args.progress_bar_separation_lines + player = Player(args.log_level) try: player.play(storage_options, play_options) diff --git a/rosbag2_py/rosbag2_py/_transport.pyi b/rosbag2_py/rosbag2_py/_transport.pyi index c9fc3a23f..727c469a9 100644 --- a/rosbag2_py/rosbag2_py/_transport.pyi +++ b/rosbag2_py/rosbag2_py/_transport.pyi @@ -34,6 +34,8 @@ class PlayOptions: node_prefix: str playback_duration: float playback_until_timestamp: int + progress_bar_separation_lines: int + progress_bar_update_rate: int publish_service_requests: bool rate: float read_ahead_queue_size: int diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index efb7fc536..929d7e15f 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -556,6 +556,8 @@ PYBIND11_MODULE(_transport, m) { "playback_until_timestamp", &PlayOptions::getPlaybackUntilTimestamp, &PlayOptions::setPlaybackUntilTimestamp) + .def_readwrite("progress_bar_update_rate", &PlayOptions::progress_bar_update_rate) + .def_readwrite("progress_bar_separation_lines", &PlayOptions::progress_bar_separation_lines) .def_readwrite("wait_acked_timeout", &PlayOptions::wait_acked_timeout) .def_readwrite("disable_loan_message", &PlayOptions::disable_loan_message) .def_readwrite("publish_service_requests", &PlayOptions::publish_service_requests) diff --git a/rosbag2_transport/CMakeLists.txt b/rosbag2_transport/CMakeLists.txt index 318367255..eefe93a5e 100644 --- a/rosbag2_transport/CMakeLists.txt +++ b/rosbag2_transport/CMakeLists.txt @@ -46,6 +46,7 @@ add_library(${PROJECT_NAME} SHARED src/rosbag2_transport/bag_rewrite.cpp src/rosbag2_transport/player.cpp src/rosbag2_transport/play_options.cpp + src/rosbag2_transport/player_progress_bar.cpp src/rosbag2_transport/player_service_client.cpp src/rosbag2_transport/reader_writer_factory.cpp src/rosbag2_transport/recorder.cpp @@ -534,6 +535,9 @@ if(BUILD_TESTING) rcpputils::rcpputils rosbag2_test_common::rosbag2_test_common ) + + ament_add_gmock(test_player_progress_bar test/rosbag2_transport/test_player_progress_bar.cpp) + target_link_libraries(test_player_progress_bar ${PROJECT_NAME}) endif() ament_package() diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp index b7d305dbf..71e68c90d 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -137,6 +137,12 @@ struct PlayOptions // replayed messages may not be correctly ordered. A possible solution could be to increase the // read_ahead_queue_size value to buffer (and order) more messages. MessageOrder message_order = MessageOrder::RECEIVED_TIMESTAMP; + + // Progress bar update rate in times per second (Hz) + int32_t progress_bar_update_rate = 3; + + // Number of separation lines to print in between the playback output and the progress bar. + uint32_t progress_bar_separation_lines = 3; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp b/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp new file mode 100644 index 000000000..224094d32 --- /dev/null +++ b/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp @@ -0,0 +1,90 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_HPP_ +#define ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_HPP_ + +#include +#include + +#include "rcutils/time.h" +#include "rosbag2_transport/visibility_control.hpp" + +namespace rosbag2_transport +{ +class PlayerProgressBarImpl; + +class ROSBAG2_TRANSPORT_PUBLIC PlayerProgressBar +{ +public: + enum class PlayerStatus : char + { + BURST = 'B', + DELAYED = 'D', + PAUSED = 'P', + RUNNING = 'R', + STOPPED = 'S', + }; + + /// PlayerProgressBar constructor + /// \param output_stream Reference to output stream where progress bar and help information + /// will be printed out. Could be std::cout, std::cerr or std::ostringstream for tests. + /// \param starting_time Time stamp of the first message in the bag. + /// \param ending_time Time stamp of the last message in the bag. + /// \param progress_bar_update_rate The progress bar maximum update rate in times per second (Hz). + /// If update rate equal 0 the progress bar will be disabled and will not output any information. + /// If update rate less than 0 the progress bar will be updated for every update(..) and + /// update_with_limited_rate(..) calls. + /// \param progress_bar_separation_lines Number of separation lines to print in between the + /// playback output and the progress bar. + explicit PlayerProgressBar( + std::ostream & output_stream, + rcutils_time_point_value_t starting_time, + rcutils_time_point_value_t ending_time, + int32_t progress_bar_update_rate = 3, + uint32_t progress_bar_separation_lines = 3); + + virtual ~PlayerProgressBar(); + + /// \brief Prints help string about player progress bar. + /// Expected to be printed once at the beginning. + void print_help_str() const; + + /// \brief Updates progress bar with the specified timestamp and player status, taking into + /// account the update rate set by the user. + /// \note The function should be used for regular progress bar updates, for example + /// after the publishing the next message. + /// \warning This function is not thread safe and shall not be called concurrently from multiple + /// threads. + /// \param timestamp Timestamp of the last published message. + /// \param status The player status to be updated on progress bar. + void update_with_limited_rate( + const rcutils_time_point_value_t & timestamp, + const PlayerStatus & status); + + /// \brief Updates progress bar with the specified player status, irrespective to the update rate + /// set by the user. + /// \note The function should be called for extraordinary progress bar updates, for example + /// when player changed its internal status or a log message is printed, and we want to 'redraw' + /// the progress bar. + /// \param status The player status to be updated on progress bar. + void update(const PlayerStatus & status); + +private: + std::unique_ptr pimpl_; +}; + +} // namespace rosbag2_transport + +#endif // ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_HPP_ diff --git a/rosbag2_transport/src/rosbag2_transport/play_options.cpp b/rosbag2_transport/src/rosbag2_transport/play_options.cpp index 64861f893..72aefb57d 100644 --- a/rosbag2_transport/src/rosbag2_transport/play_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/play_options.cpp @@ -59,6 +59,9 @@ Node convert::encode( node["disable_loan_message"] = play_options.disable_loan_message; + node["progress_bar_update_rate"] = play_options.progress_bar_update_rate; + node["progress_bar_separation_lines"] = play_options.progress_bar_separation_lines; + return node; } @@ -114,6 +117,12 @@ bool convert::decode( optional_assign(node, "disable_loan_message", play_options.disable_loan_message); + optional_assign( + node, "progress_bar_update_rate", play_options.progress_bar_update_rate); + + optional_assign( + node, "progress_bar_separation_lines", play_options.progress_bar_separation_lines); + return true; } diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 1547eba4d..66f000ff8 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -39,6 +39,7 @@ #include "rosbag2_storage/qos.hpp" #include "rosbag2_transport/config_options_from_node_params.hpp" #include "rosbag2_transport/player_service_client.hpp" +#include "rosbag2_transport/player_progress_bar.hpp" #include "rosbag2_transport/reader_writer_factory.hpp" #include "logging.hpp" @@ -88,6 +89,7 @@ class PlayerImpl using callback_handle_t = Player::callback_handle_t; using play_msg_callback_t = Player::play_msg_callback_t; using reader_storage_options_pair_t = Player::reader_storage_options_pair_t; + using PlayerStatus = PlayerProgressBar::PlayerStatus; PlayerImpl( Player * owner, @@ -120,7 +122,7 @@ class PlayerImpl /// \brief Set the playback rate. /// \return false if an invalid value was provided (<= 0). - virtual bool set_rate(double); + bool set_rate(double); /// \brief Playing next message from queue when in pause. /// \details This is blocking call and it will wait until next available message will be @@ -374,6 +376,8 @@ class PlayerImpl std::shared_ptr player_service_client_manager_; + std::unique_ptr progress_bar_; + BagMessageComparator get_bag_message_comparator(const MessageOrder & order); /// Comparator for SerializedBagMessageSharedPtr to order chronologically by recv_timestamp. @@ -452,6 +456,7 @@ PlayerImpl::PlayerImpl( { std::lock_guard lk(reader_mutex_); starting_time_ = std::numeric_limits::max(); + rcutils_time_point_value_t ending_time = std::numeric_limits::min(); for (const auto & [reader, storage_options] : readers_with_options_) { // keep readers open until player is destroyed reader->open(storage_options, {"", rmw_get_serialization_format()}); @@ -459,9 +464,14 @@ PlayerImpl::PlayerImpl( const auto metadata = reader->get_metadata(); const auto metadata_starting_time = std::chrono::duration_cast( metadata.starting_time.time_since_epoch()).count(); + const auto metadata_bag_duration = std::chrono::duration_cast( + metadata.duration).count(); if (metadata_starting_time < starting_time_) { starting_time_ = metadata_starting_time; } + if (metadata_starting_time + metadata_bag_duration > ending_time) { + ending_time = metadata_starting_time + metadata_bag_duration; + } } // If a non-default (positive) starting time offset is provided in PlayOptions, // then add the offset to the starting time obtained from reader metadata @@ -474,6 +484,12 @@ PlayerImpl::PlayerImpl( } else { starting_time_ += play_options_.start_offset; } + + progress_bar_ = std::make_unique( + std::cout, starting_time_, ending_time, + play_options.progress_bar_update_rate, + play_options.progress_bar_separation_lines); + clock_ = std::make_unique( starting_time_, std::chrono::steady_clock::now, std::chrono::milliseconds{100}, play_options_.start_paused); @@ -484,6 +500,8 @@ PlayerImpl::PlayerImpl( } create_control_services(); add_keyboard_callbacks(); + progress_bar_->print_help_str(); + progress_bar_->update(PlayerStatus::STOPPED); } PlayerImpl::~PlayerImpl() @@ -506,6 +524,7 @@ PlayerImpl::~PlayerImpl() reader->close(); } } + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); } const std::chrono::milliseconds @@ -529,6 +548,7 @@ bool PlayerImpl::play() RCLCPP_WARN_STREAM( owner_->get_logger(), "Trying to play() while in playback, dismissing request."); + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); return false; } } @@ -555,6 +575,7 @@ bool PlayerImpl::play() try { if (delay > rclcpp::Duration(0, 0)) { RCLCPP_INFO_STREAM(owner_->get_logger(), "Sleep " << delay.nanoseconds() << " ns"); + progress_bar_->update(PlayerStatus::DELAYED); std::chrono::nanoseconds delay_duration(delay.nanoseconds()); std::this_thread::sleep_for(delay_duration); } @@ -569,6 +590,9 @@ bool PlayerImpl::play() if (clock_publish_timer_ != nullptr) { clock_publish_timer_->reset(); } + + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + load_storage_content_ = true; storage_loading_future_ = std::async( std::launch::async, [this]() { @@ -599,6 +623,8 @@ bool PlayerImpl::play() ready_to_play_from_queue_cv_.notify_all(); } + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + // Wait for all published messages to be acknowledged. if (play_options_.wait_acked_timeout >= 0) { std::chrono::milliseconds timeout(play_options_.wait_acked_timeout); @@ -636,6 +662,7 @@ bool PlayerImpl::play() play_next_result_ = false; finished_play_next_cv_.notify_all(); } + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); }); return true; } @@ -672,6 +699,8 @@ void PlayerImpl::stop() cancel_wait_for_next_message_ = true; } + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + if (clock_->is_paused()) { // Wake up the clock in case it's in a sleep_until(time) call clock_->wakeup(); @@ -692,18 +721,21 @@ void PlayerImpl::pause() { clock_->pause(); RCLCPP_INFO_STREAM(owner_->get_logger(), "Pausing play."); + progress_bar_->update(PlayerStatus::PAUSED); } void PlayerImpl::resume() { clock_->resume(); RCLCPP_INFO_STREAM(owner_->get_logger(), "Resuming play."); + progress_bar_->update(PlayerStatus::RUNNING); } void PlayerImpl::toggle_paused() { // Note: Use upper level public API from owner class to facilitate unit tests owner_->is_paused() ? owner_->resume() : owner_->pause(); + progress_bar_->update(owner_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); } bool PlayerImpl::is_paused() const @@ -724,6 +756,7 @@ bool PlayerImpl::set_rate(double rate) } else { RCLCPP_WARN_STREAM(owner_->get_logger(), "Failed to set rate to invalid value " << rate); } + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); return ok; } @@ -753,6 +786,7 @@ bool PlayerImpl::play_next() } if (!clock_->is_paused()) { RCLCPP_WARN_STREAM(owner_->get_logger(), "Called play next, but not in paused state."); + progress_bar_->update(PlayerStatus::RUNNING); return false; } @@ -783,9 +817,11 @@ size_t PlayerImpl::burst(const size_t num_messages) { if (!clock_->is_paused()) { RCLCPP_WARN_STREAM(owner_->get_logger(), "Burst can only be used when in the paused state."); + progress_bar_->update(PlayerStatus::RUNNING); return 0; } + progress_bar_->update(PlayerStatus::BURST); uint64_t messages_played = 0; for (auto ii = 0u; ii < num_messages || num_messages == 0; ++ii) { @@ -797,6 +833,7 @@ size_t PlayerImpl::burst(const size_t num_messages) } RCLCPP_INFO_STREAM(owner_->get_logger(), "Burst " << messages_played << " messages."); + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); return messages_played; } @@ -1054,6 +1091,15 @@ void PlayerImpl::play_messages_from_queue() finished_play_next_cv_.notify_all(); } } + // Updating progress bar in this code section protected + // by the mutex skip_message_in_main_play_loop_mutex_. + if (play_options_.message_order == MessageOrder::RECEIVED_TIMESTAMP) { + progress_bar_->update_with_limited_rate( + message_ptr->recv_timestamp, PlayerStatus::RUNNING); + } else if (play_options_.message_order == MessageOrder::SENT_TIMESTAMP) { + progress_bar_->update_with_limited_rate( + message_ptr->send_timestamp, PlayerStatus::RUNNING); + } } message_ptr = take_next_message_from_queue(); } diff --git a/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp new file mode 100644 index 000000000..d583720d9 --- /dev/null +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp @@ -0,0 +1,53 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/logger.hpp" +#include "rcutils/time.h" +#include "rosbag2_transport/player_progress_bar.hpp" +#include "player_progress_bar_impl.hpp" + +namespace rosbag2_transport +{ + +PlayerProgressBar::PlayerProgressBar( + std::ostream & output_stream, + rcutils_time_point_value_t starting_time, + rcutils_time_point_value_t ending_time, + int32_t progress_bar_update_rate, + uint32_t progress_bar_separation_lines) +{ + pimpl_ = std::make_unique( + output_stream, starting_time, ending_time, + progress_bar_update_rate, progress_bar_separation_lines); +} + +PlayerProgressBar::~PlayerProgressBar() = default; + +void PlayerProgressBar::print_help_str() const +{ + pimpl_->print_help_str(); +} + +void PlayerProgressBar::update_with_limited_rate( + const rcutils_time_point_value_t & timestamp, const PlayerStatus & status) +{ + pimpl_->update_with_limited_rate(timestamp, status); +} + +void PlayerProgressBar::update(const PlayerStatus & status) +{ + pimpl_->update(status); +} + +} // namespace rosbag2_transport diff --git a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp new file mode 100644 index 000000000..ab0d1ad88 --- /dev/null +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp @@ -0,0 +1,171 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_IMPL_HPP_ +#define ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_IMPL_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "rcutils/time.h" +#include "rosbag2_transport/player_progress_bar.hpp" + +namespace rosbag2_transport +{ +class PlayerProgressBarImpl { +public: + using PlayerStatus = PlayerProgressBar::PlayerStatus; + + PlayerProgressBarImpl( + std::ostream & output_stream, + rcutils_time_point_value_t starting_time, + rcutils_time_point_value_t ending_time, + int32_t progress_bar_update_rate, + uint32_t progress_bar_separation_lines) + : o_stream_(output_stream), + enable_progress_bar_(progress_bar_update_rate != 0), + progress_bar_update_period_ms_(progress_bar_update_rate > 0 ? + (1000 / progress_bar_update_rate) : 0), + progress_bar_separation_lines_(progress_bar_separation_lines) + { + starting_time_secs_ = RCUTILS_NS_TO_S( + static_cast(std::min(starting_time, ending_time))); + duration_secs_ = RCUTILS_NS_TO_S( + std::max(static_cast(ending_time - starting_time), 0.0)); + progress_current_time_secs_ = starting_time_secs_; + progress_secs_from_start_ = progress_current_time_secs_ - starting_time_secs_; + std::ostringstream oss_clear_and_move_cursor_down; + + for (uint32_t i = 0; i < progress_bar_separation_lines_; i++) { + // The ANSI control code "\033[2K" is used to clear an entire line in the terminal. + // Cleanup current line and jump down to one new line with "\n" + oss_clear_and_move_cursor_down << "\033[2K\n"; + } + + progress_bar_helper_clear_and_move_cursor_down_ = oss_clear_and_move_cursor_down.str(); + std::ostringstream oss_move_cursor_up; + // Move cursor up by a specific number of lines using "Cursor Up" '\033[A' ANSI control code + oss_move_cursor_up << "\033[" << + progress_bar_separation_lines_ + progress_bar_lines_count_ << "A"; + progress_bar_helper_move_cursor_up_ = oss_move_cursor_up.str(); + } + + ~PlayerProgressBarImpl() + { + // arrange cursor position to be after the progress bar + if (enable_progress_bar_) { + std::stringstream ss; + ss << "\033[" << progress_bar_separation_lines_ + progress_bar_lines_count_ << "B"; + o_stream_ << ss.rdbuf() << std::flush; + } + } + + void print_help_str() const + { + std::stringstream ss; + if (enable_progress_bar_) { + if (progress_bar_update_period_ms_ > 0) { + ss << "Progress bar enabled at " << (1000 / progress_bar_update_period_ms_) << " Hz.\n"; + } else { + ss << "Progress bar enabled for every message.\n"; + } + ss << "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped\n"; + } else { + ss << "Progress bar disabled.\n"; + } + o_stream_ << ss.rdbuf() << std::flush; + } + + void update_with_limited_rate( + const rcutils_time_point_value_t & timestamp, + const PlayerStatus & status) + { + if (!enable_progress_bar_) { + return; + } + + // If we are not updating the progress bar for every call, check if we should update it now + // based on the update rate set by the user + if (progress_bar_update_period_ms_ > 0) { + std::chrono::steady_clock::time_point steady_time_now = std::chrono::steady_clock::now(); + if (std::chrono::duration_cast( + steady_time_now - progress_bar_last_time_updated_).count() < progress_bar_update_period_ms_) + { + return; + } + progress_bar_last_time_updated_ = steady_time_now; + } + + draw_progress_bar(timestamp, status); + } + + void update(const PlayerStatus & status) + { + if (!enable_progress_bar_) { + return; + } + // Update progress bar irrespective of the update rate set by the user. + draw_progress_bar(-1, status); + } + + void draw_progress_bar(const rcutils_time_point_value_t & timestamp, const PlayerStatus & status) + { + if (timestamp >= 0) { + progress_current_time_secs_ = RCUTILS_NS_TO_S(static_cast(timestamp)); + progress_secs_from_start_ = progress_current_time_secs_ - starting_time_secs_; + } + std::stringstream ss; + ss << + // Clear and print newlines + progress_bar_helper_clear_and_move_cursor_down_ << + // Print progress bar + "====== Playback Progress ======\n" << + "[" << std::fixed << std::setprecision(9) << progress_current_time_secs_ << + "] Duration " << std::setprecision(2) << progress_secs_from_start_ << + // Spaces at the end are used to clear any previous progress bar in case the new one is + // shorter, which can happen when the playback starts a new loop. + "/" << duration_secs_ << " [" << static_cast(status) << "] \n" << + // Go up to the beginning of the blank lines + progress_bar_helper_move_cursor_up_; + o_stream_ << ss.rdbuf() << std::flush; + } + +private: + std::ostream & o_stream_; + double starting_time_secs_ = 0.0; + double duration_secs_ = 0.0; + std::string progress_bar_helper_clear_and_move_cursor_down_; + std::string progress_bar_helper_move_cursor_up_; + bool enable_progress_bar_; + uint16_t progress_bar_update_period_ms_; + std::chrono::steady_clock::time_point progress_bar_last_time_updated_{}; + uint32_t progress_bar_separation_lines_ = 3; + double progress_secs_from_start_ = 0.0; + double progress_current_time_secs_ = 0.0; + + /// progress_bar_lines_count_ - The number of lines in progress bar to be printed out. + /// ====== Playback Progress ====== + /// [0.000000000] Duration 0.00/0.00 [R] + static const size_t progress_bar_lines_count_ = 2; +}; +} // namespace rosbag2_transport + +#endif // ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_IMPL_HPP_ diff --git a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp new file mode 100644 index 000000000..df3151d1a --- /dev/null +++ b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp @@ -0,0 +1,324 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include + +#include +#include + +#include "rosbag2_transport/player_progress_bar.hpp" + +using namespace ::testing; // NOLINT +using namespace rosbag2_transport; // NOLINT + +class TestPlayerProgressBar : public Test +{ +public: + std::string GenerateClearAndMoveCursorDownRegexString(uint32_t num_lines) + { + std::ostringstream oss; + for (uint32_t i = 0; i < num_lines; i++) { + // The ANSI control code "\033[2K" is used to clear an entire line in the terminal. + // Cleanup current line and jump down to one new line with "\n" + oss << "\033\\[2K\n"; + } + return oss.str(); + } + + std::string GenerateMoveCursorUpRegexString(uint32_t num_lines) + { + static const size_t progress_bar_lines_count = 2; + // ====== Playback Progress ====== + // [0.000000000] Duration 0.00/0.00 [R] + std::ostringstream oss; + // Move cursor up by a specific number of lines using "Cursor Up" '\033[A' escape sequence + oss << "\033\\[" << num_lines + progress_bar_lines_count << "A"; + return oss.str(); + } + + std::vector status_list_ = { + PlayerProgressBar::PlayerStatus::DELAYED, + PlayerProgressBar::PlayerStatus::RUNNING, + PlayerProgressBar::PlayerStatus::PAUSED, + PlayerProgressBar::PlayerStatus::BURST, + PlayerProgressBar::PlayerStatus::STOPPED + }; +}; + +TEST_F(TestPlayerProgressBar, default_ctor_dtor) { + std::ostringstream oss; + { + auto progress_bar = std::make_unique(oss, 0, 0); + } +} + +TEST_F(TestPlayerProgressBar, can_dtor_after_output) { + std::ostringstream oss; + { + auto progress_bar = std::make_unique( + oss, RCUTILS_S_TO_NS(1000000000), RCUTILS_S_TO_NS(2000000000), 1, 0); + progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); + } + EXPECT_THAT(oss.str(), + MatchesRegex( + "====== Playback Progress ======\n" + "\\[1000000000.000000000\\] Duration 0\\.00/1000000000\\.00 \\[D\\].*" + ) + ); +} + +TEST_F(TestPlayerProgressBar, print_help_str_with_zero_update_rate) { + std::ostringstream oss; + auto progress_bar = std::make_unique(oss, 0, 0, 0); + progress_bar->print_help_str(); + EXPECT_EQ(oss.str(), "Progress bar disabled.\n"); +} + +TEST_F(TestPlayerProgressBar, print_help_str_with_negative_update_rate) { + std::ostringstream oss; + auto progress_bar = std::make_unique(oss, 0, 0, -1); + progress_bar->print_help_str(); + EXPECT_EQ(oss.str(), + "Progress bar enabled for every message." + "\nProgress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped\n"); +} + +TEST_F(TestPlayerProgressBar, print_help_str_with_positive_update_rate) { + std::ostringstream oss; + auto progress_bar = std::make_unique(oss, 0, 0, 1); + progress_bar->print_help_str(); + EXPECT_EQ(oss.str(), + "Progress bar enabled at 1 Hz." + "\nProgress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped\n"); +} + +TEST_F(TestPlayerProgressBar, update_status_with_disabled_progress_bar) { + std::ostringstream oss; + auto progress_bar = std::make_unique(oss, 0, 0, 0); + for (const auto & status : status_list_) { + progress_bar->update(status); + EXPECT_THAT(oss.str(), IsEmpty()); + oss.clear(); + oss.str(""); + } +} + +TEST_F(TestPlayerProgressBar, update_status_with_enabled_progress_bar) { + { + // Test Playback Progress header and status update with update rate 1 Hz + std::ostringstream oss; + auto progress_bar = std::make_unique(oss, 0, 0, 1); + progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update(PlayerProgressBar::PlayerStatus::STOPPED); + EXPECT_THAT(oss.str(), + MatchesRegex( + ".*====== Playback Progress ======\n" + "\\[0\\.000000000\\] Duration 0\\.00/0\\.00 \\[R\\].*" + ".*====== Playback Progress ======\n" + "\\[0\\.000000000\\] Duration 0\\.00/0\\.00 \\[S\\].*" + ) + ); + } + + { + // Test status update on all statuses + std::ostringstream oss; + auto progress_bar = std::make_unique(oss, 0, 0, 1); + for (const auto & status : status_list_) { + progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update(status); + std::string status_str(1, static_cast(status)); + EXPECT_THAT(oss.str(), MatchesRegex( + ".*\\[R\\].*\n.*\\[" + status_str + "\\].*")); + oss.clear(); + oss.str(""); + } + } +} + +TEST_F(TestPlayerProgressBar, update_status_with_separation_lines) { + { + // Test status update with 2 separation lines + std::ostringstream oss; + uint32_t num_separation_lines = 2; + std::string expected_pre_line_separator = + GenerateClearAndMoveCursorDownRegexString(num_separation_lines); + std::string cursor_up_regex_string = GenerateMoveCursorUpRegexString(num_separation_lines); + auto progress_bar = std::make_unique(oss, 0, 0, 1, num_separation_lines); + progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); + progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); + EXPECT_THAT(oss.str(), MatchesRegex( + expected_pre_line_separator + + ".*\\[D\\].*" + cursor_up_regex_string + + expected_pre_line_separator + + ".*\\[R\\].*" + cursor_up_regex_string)); + } + + { + // Test status update with 5 separation lines and external log messages + std::ostringstream oss; + uint32_t num_separation_lines = 5; + std::string expected_pre_line_separator = + GenerateClearAndMoveCursorDownRegexString(num_separation_lines); + std::string cursor_up_regex_string = GenerateMoveCursorUpRegexString(num_separation_lines); + auto progress_bar = std::make_unique(oss, 0, 0, 1, num_separation_lines); + progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); + oss << "log msg 1\n"; + oss << "log msg 2\n"; + oss << "log msg 3\n"; + oss << "log msg 4\n"; + progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); + oss << "log msg 5\n"; + oss << "log msg 6\n"; + progress_bar->update(PlayerProgressBar::PlayerStatus::PAUSED); + EXPECT_THAT(oss.str(), MatchesRegex( + expected_pre_line_separator + + ".*\\[D\\].*" + cursor_up_regex_string + + "log msg 1\nlog msg 2\nlog msg 3\nlog msg 4\n" + + expected_pre_line_separator + + ".*\\[R\\].*" + cursor_up_regex_string + + "log msg 5\nlog msg 6\n" + + expected_pre_line_separator + + ".*\\[P\\].*" + cursor_up_regex_string)); + } +} + +TEST_F(TestPlayerProgressBar, update_status_without_separation_lines) { + std::ostringstream oss; + // Test status update without separation lines + const uint32_t num_separation_lines = 0; + std::string cursor_up_regex_string = GenerateMoveCursorUpRegexString(num_separation_lines); + auto progress_bar = std::make_unique(oss, 0, 0, 1, num_separation_lines); + progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); + EXPECT_THAT(oss.str(), MatchesRegex( + ".*\\[D\\].*" + cursor_up_regex_string)); + + progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); + EXPECT_THAT(oss.str(), MatchesRegex( + ".*\\[D\\].*" + cursor_up_regex_string + + ".*\\[R\\].*" + cursor_up_regex_string)); +} + +TEST_F(TestPlayerProgressBar, update_with_limited_rate_respect_update_rate) { + rcutils_time_point_value_t starting_time = RCUTILS_S_TO_NS(1); + rcutils_time_point_value_t ending_time = starting_time + RCUTILS_S_TO_NS(5); + + // Test if update rate of the progress bar is respected + std::ostringstream oss; + const size_t update_rate = 4; + const rcutils_duration_value_t update_period_ns = RCUTILS_S_TO_NS(1) / update_rate; + auto progress_bar = + std::make_unique(oss, starting_time, ending_time, update_rate, 0); + + std::vector timestamps; + timestamps.push_back(starting_time); // 1st update of the progress bar + + // Add timestamps relative to the 1st update of the progress bar + timestamps.push_back( + starting_time + static_cast(0.3 * update_period_ns)); // skip update + timestamps.push_back( + starting_time + static_cast(0.5 * update_period_ns)); // skip update + + rcutils_time_point_value_t second_update_timestamp = starting_time + + static_cast(1.3 * update_period_ns); // more than one period + + timestamps.push_back(second_update_timestamp); // 2nd update + + // Add timestamps relative to the 2nd update of the progress bar + timestamps.push_back(second_update_timestamp + + static_cast(0.3 * update_period_ns)); // skip update + + timestamps.push_back(second_update_timestamp + + static_cast(0.5 * update_period_ns)); // skip update + + timestamps.push_back(second_update_timestamp + + static_cast(1.5 * update_period_ns)); // 3rd update + + progress_bar->update_with_limited_rate(timestamps[0], PlayerProgressBar::PlayerStatus::RUNNING); + for (size_t i = 1; i < timestamps.size(); i++) { + std::this_thread::sleep_for(std::chrono::nanoseconds(timestamps[i] - timestamps[i - 1])); + + progress_bar->update_with_limited_rate( + timestamps[i], PlayerProgressBar::PlayerStatus::RUNNING); + } + + // Check if the progress bar is updated at the correct 3 timestamps + EXPECT_THAT(oss.str(), + MatchesRegex( + ".*====== Playback Progress ======\n" + "\\[1\\.000000000\\] Duration 0\\.00/5\\.00" + ".*====== Playback Progress ======\n" + "\\[1\\.325000000\\] Duration 0\\.32/5\\.00" + ".*====== Playback Progress ======\n" + "\\[1\\.700000000\\] Duration 0\\.70/5\\.00.*" + ) + ); +} + +TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_negative_update_rate) { + rcutils_time_point_value_t starting_time = RCUTILS_S_TO_NS(1000000000); + rcutils_time_point_value_t ending_time = starting_time + RCUTILS_S_TO_NS(5); + const int32_t update_rate = -1; + + // Test if progress bar is updated every time + std::ostringstream oss; + auto progress_bar = + std::make_unique(oss, starting_time, ending_time, update_rate, 0); + rcutils_time_point_value_t timestamp_1 = starting_time; + rcutils_time_point_value_t timestamp_2 = starting_time + RCUTILS_MS_TO_NS(1); + rcutils_time_point_value_t timestamp_3 = starting_time + RCUTILS_MS_TO_NS(2); + progress_bar->update_with_limited_rate(timestamp_1, PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update_with_limited_rate(timestamp_2, PlayerProgressBar::PlayerStatus::STOPPED); + progress_bar->update_with_limited_rate(timestamp_3, PlayerProgressBar::PlayerStatus::RUNNING); + EXPECT_THAT(oss.str(), MatchesRegex( + ".*\\[1000000000\\.000000000\\] Duration 0\\.00/5\\.00" + ".*\\[1000000000\\.0009.*\\] Duration 0\\.00/5\\.00" + ".*\\[1000000000\\.0019.*\\] Duration 0\\.00/5\\.00.*")); +} + +TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_update_rate) { + rcutils_time_point_value_t starting_time = RCUTILS_S_TO_NS(1000000000); + rcutils_time_point_value_t ending_time = starting_time + RCUTILS_S_TO_NS(5); + int32_t update_rate = 0; + + // Test if progress bar is not updated + std::ostringstream oss; + auto progress_bar = std::make_unique( + oss, starting_time, ending_time, update_rate); + rcutils_time_point_value_t timestamp_1 = starting_time; + rcutils_time_point_value_t timestamp_2 = starting_time + RCUTILS_MS_TO_NS(1); + rcutils_time_point_value_t timestamp_3 = starting_time + RCUTILS_MS_TO_NS(2); + progress_bar->update_with_limited_rate(timestamp_1, PlayerProgressBar::PlayerStatus::DELAYED); + progress_bar->update_with_limited_rate(timestamp_2, PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update_with_limited_rate(timestamp_3, PlayerProgressBar::PlayerStatus::RUNNING); + EXPECT_THAT(oss.str(), IsEmpty()); +} + +TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_timestamp) { + rcutils_time_point_value_t starting_time = RCUTILS_S_TO_NS(1); + rcutils_time_point_value_t ending_time = starting_time + RCUTILS_S_TO_NS(5); + std::ostringstream oss; + auto progress_bar = + std::make_unique(oss, starting_time, ending_time, 1, 0); + + progress_bar->update_with_limited_rate(0, PlayerProgressBar::PlayerStatus::RUNNING); + + EXPECT_THAT(oss.str(), + MatchesRegex( + ".*====== Playback Progress ======\n" + "\\[0\\.000000000\\] Duration -1\\.00/5\\.00.*" + ) + ); +}