From c4a723136795a54ce09865655fb32ce8a597d9f6 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Fri, 10 Jan 2025 20:36:03 -0800 Subject: [PATCH 01/13] Squashed commit of the following: commit 913de31ae38e9c9c525cba98a7a38065098cb3e7 Author: Nicola Loi Date: Sun Dec 8 18:27:06 2024 +0100 stubgen Signed-off-by: Nicola Loi commit 8625164069aeb46df0f278c289df3ffd7b322512 Author: Nicola Loi Date: Sun Dec 8 17:14:26 2024 +0100 add custom space between playback output and progress bar Signed-off-by: Nicola Loi commit 6a47a7f5dac40ef2080f037e158e0c3396a7a1cc Merge: fb464b02 8f1527e5 Author: Nicola Loi Date: Sun Dec 8 11:14:01 2024 +0100 Merge branch 'rolling' into nicolaloi/bag-play-progress-bar Signed-off-by: Nicola Loi commit fb464b027075bd981200886f59a7781362f50b75 Author: Nicola Loi Date: Fri Dec 6 22:40:45 2024 +0100 comments Signed-off-by: Nicola Loi commit 19df032cb2fb786e2cacfd1882cbdd63a820884b Author: Nicola Loi Date: Fri Dec 6 22:28:51 2024 +0100 change progress bar string, change 'frequency' terms to 'update rate' Signed-off-by: Nicola Loi commit 47dff3d1bfa4ccfb18354084c28993f96763ae5f Author: Nicola Loi Date: Thu Dec 5 22:06:34 2024 +0000 Progress bar with nanosecond precision and shorter length Signed-off-by: Nicola Loi commit 0ef3f260ef310b23c3d508722588dc149040dc35 Author: Nicola Loi Date: Wed Dec 4 22:35:10 2024 +0000 comments Signed-off-by: Nicola Loi commit 6051e6cfc3310d3f6622233586d6157c2343ae99 Author: Nicola Loi Date: Mon Dec 2 23:29:10 2024 +0000 some updates after merge with rolling Signed-off-by: Nicola Loi commit 372b8a2118eeef105c99a5cee3f2b995bdb9fc15 Merge: 0c99597f 125db50b Author: Nicola Loi Date: Mon Dec 2 23:35:09 2024 +0100 Merge branch 'rolling' into nicolaloi/bag-play-progress-bar Signed-off-by: Nicola Loi commit 0c99597fe6bb77030ac0ec32ef1860e9456b7104 Author: Nicola Loi Date: Mon Nov 18 21:53:16 2024 +0100 add burst status, reduce cout setprecision Signed-off-by: Nicola Loi commit bac4e9fcc79147f271ec0f57bd1e03cd4953021e Author: Nicola Loi Date: Sun Nov 17 21:05:43 2024 +0100 update to avoid synchronization issues Signed-off-by: Nicola Loi commit 9cdf5adcb514036e2f131f04ce7add1e0890a9e0 Author: Nicola Loi Date: Sun Nov 10 22:59:27 2024 +0100 update default rate Signed-off-by: Nicola Loi commit 43fa0f89ef2d51a786953088dbdea18a2ce8b39e Author: Nicola Loi Date: Mon Oct 28 23:00:06 2024 +0000 Code review Signed-off-by: Nicola Loi commit a00bdc43eb8a3ff7bc8e54e2b2cba7516eb59425 Author: Nicola Loi Date: Thu Oct 24 00:06:25 2024 +0200 convert to template Signed-off-by: Nicola Loi commit 4ba1a8c0a9bfb02eac79320a9b05820cb27654d6 Author: Nicola Loi Date: Mon Oct 21 00:49:08 2024 +0200 remove new progress bar class, integrate functionality in PlayerImpl Signed-off-by: Nicola Loi commit 2cb732d2b54942130b85c00b90fe717f949ec5dd Author: Nicola Loi Date: Wed Oct 16 00:21:08 2024 +0200 clean #include Signed-off-by: Nicola Loi commit 616df0d3d93f8ea777c668cdc7a34342419f7be3 Author: Nicola Loi Date: Tue Oct 15 22:34:34 2024 +0200 Start adding progress-bar for ros2 bag play Signed-off-by: Nicola Loi Signed-off-by: Michael Orlov --- docs/design/rosbag2_record_replay_service.md | 4 + ros2bag/ros2bag/verb/play.py | 20 ++ rosbag2_py/rosbag2_py/_transport.pyi | 2 + rosbag2_py/src/rosbag2_py/_transport.cpp | 2 + .../rosbag2_transport/play_options.hpp | 7 + .../src/rosbag2_transport/play_options.cpp | 9 + .../src/rosbag2_transport/player.cpp | 196 +++++++++++++++++- 7 files changed, 236 insertions(+), 4 deletions(-) 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..79ed25611 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=float, default=3.0, + help='Print a progress bar of the playback player at a specific frequency in Hz. ' + 'Default is %(default)d. ' + 'Negative values mark an update for every published message, while ' + ' a zero value disables the progress bar.', + metavar='PROGRESS_BAR_FREQUENCY') + 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='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..010a1125b 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: float 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/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp index b7d305dbf..c4e3f9be0 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -137,6 +137,13 @@ 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; + + // Rate in Hz at which to print progress bar. + double progress_bar_update_rate = 3.0; + + // Number of separation lines to print in between the playback output + // and the progress bar. + int progress_bar_separation_lines = 3; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/src/rosbag2_transport/play_options.cpp b/rosbag2_transport/src/rosbag2_transport/play_options.cpp index 64861f893..4fd8d1207 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..b7369a847 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -80,6 +80,15 @@ rclcpp::QoS publisher_qos_for_topic( namespace rosbag2_transport { + +enum class PlayerStatus : char +{ + RUNNING = 'R', + PAUSED = 'P', + BURST = 'B', + DELAYED = 'D', +}; + constexpr Player::callback_handle_t Player::invalid_callback_handle; class PlayerImpl @@ -123,12 +132,13 @@ class PlayerImpl virtual bool set_rate(double); /// \brief Playing next message from queue when in pause. + /// \param in_burst_mode If true, it will not call the progress bar update to avoid delays. /// \details This is blocking call and it will wait until next available message will be /// published or rclcpp context shut down. /// \note If internal player queue is starving and storage has not been completely loaded, /// this method will wait until new element will be pushed to the queue. /// \return true if player in pause mode and successfully played next message, otherwise false. - virtual bool play_next(); + virtual bool play_next(bool in_burst_mode = false); /// \brief Burst the next \p num_messages messages from the queue when paused. /// \param num_messages The number of messages to burst from the queue. Specifying zero means no @@ -306,6 +316,32 @@ class PlayerImpl rcutils_time_point_value_t get_message_order_timestamp( const rosbag2_storage::SerializedBagMessageSharedPtr & message) const; + std::atomic_bool is_delayed_status_{false}; + + std::string progress_bar_helper_clear_and_move_cursor_down_; + std::string progress_bar_helper_move_cursor_up_; + + void print_progress_bar_help_str() const; + // Update progress bar with an input timestamp, + // taking into account the update rate set by the user. + // The function should be called for regular progress bar updates, for example + // after the recurrent publishing of the messages. + // Call update_progress_bar_check_rate function only where it cannot run + // contemporaneously in multiple threads, i.e. function calls are already protected by a mutex. + // To avoid locking overhead no new mutex inside the function is directly protecting + // the access to the class attribute progress_bar_last_time_updated_. + void update_progress_bar_check_rate( + const rcutils_time_point_value_t & timestamp, + const PlayerStatus & status); + // Update progress bar with the current playback timestamp, + // irrespective of the update rate set by the user. + // The function should be called for extraordinary progress bar updates, for example + // when a log message is printed and we want to 'redraw' the progress bar. + void update_progress_bar(const PlayerStatus & status) const; + void cout_progress_bar( + const rcutils_time_point_value_t & timestamp, + const PlayerStatus & status) const; + static constexpr double read_ahead_lower_bound_percentage_ = 0.9; static const std::chrono::milliseconds queue_read_wait_period_; std::atomic_bool cancel_wait_for_next_message_{false}; @@ -352,6 +388,9 @@ class PlayerImpl std::atomic_bool play_next_result_{false}; rcutils_time_point_value_t starting_time_; + rcutils_time_point_value_t ending_time_; + double starting_time_secs_; + double duration_secs_; // control services rclcpp::Service::SharedPtr srv_pause_; @@ -397,6 +436,12 @@ class PlayerImpl return l->send_timestamp > r->send_timestamp; } } bag_message_chronological_send_timestamp_comparator; + + // progress bar + const bool enable_progress_bar_; + const bool progress_bar_update_always_; + const rcutils_duration_value_t progress_bar_update_period_; + std::chrono::steady_clock::time_point progress_bar_last_time_updated_{}; }; PlayerImpl::BagMessageComparator PlayerImpl::get_bag_message_comparator(const MessageOrder & order) @@ -423,7 +468,12 @@ PlayerImpl::PlayerImpl( play_options_(play_options), message_queue_(get_bag_message_comparator(play_options_.message_order)), keyboard_handler_(std::move(keyboard_handler)), - player_service_client_manager_(std::make_shared()) + player_service_client_manager_(std::make_shared()), + enable_progress_bar_(play_options.progress_bar_update_rate != 0), + progress_bar_update_always_(play_options.progress_bar_update_rate < 0), + progress_bar_update_period_(play_options.progress_bar_update_rate != 0 ? + RCUTILS_S_TO_NS(1.0 / play_options.progress_bar_update_rate) : + std::numeric_limits::max()) { for (auto & topic : play_options_.topics_to_filter) { topic = rclcpp::expand_topic_or_service_name( @@ -449,9 +499,20 @@ PlayerImpl::PlayerImpl( owner_->get_namespace(), false); } + std::ostringstream oss_clear_and_move_cursor_down; + for (int i = 0; i < play_options_.progress_bar_separation_lines; i++) { + oss_clear_and_move_cursor_down << "\033[2K\n"; + } + oss_clear_and_move_cursor_down << "\033[2K"; + progress_bar_helper_clear_and_move_cursor_down_ = oss_clear_and_move_cursor_down.str(); + std::ostringstream oss_move_cursor_up; + oss_move_cursor_up << "\033[" << play_options_.progress_bar_separation_lines + 1 << "F"; + progress_bar_helper_move_cursor_up_ = oss_move_cursor_up.str(); + { std::lock_guard lk(reader_mutex_); starting_time_ = std::numeric_limits::max(); + 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 +520,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 +540,10 @@ PlayerImpl::PlayerImpl( } else { starting_time_ += play_options_.start_offset; } + 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)); clock_ = std::make_unique( starting_time_, std::chrono::steady_clock::now, std::chrono::milliseconds{100}, play_options_.start_paused); @@ -484,6 +554,7 @@ PlayerImpl::PlayerImpl( } create_control_services(); add_keyboard_callbacks(); + print_progress_bar_help_str(); } PlayerImpl::~PlayerImpl() @@ -506,6 +577,13 @@ PlayerImpl::~PlayerImpl() reader->close(); } } + update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + // arrange cursor position to be after the progress bar + if (enable_progress_bar_) { + std::ostringstream oss; + oss << "\033[" << play_options_.progress_bar_separation_lines + 1 << "B\n"; + std::cout << oss.str() << std::flush; + } } const std::chrono::milliseconds @@ -529,6 +607,7 @@ bool PlayerImpl::play() RCLCPP_WARN_STREAM( owner_->get_logger(), "Trying to play() while in playback, dismissing request."); + update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); return false; } } @@ -554,9 +633,12 @@ bool PlayerImpl::play() [&, delay]() { try { if (delay > rclcpp::Duration(0, 0)) { + is_delayed_status_ = true; RCLCPP_INFO_STREAM(owner_->get_logger(), "Sleep " << delay.nanoseconds() << " ns"); + update_progress_bar(PlayerStatus::DELAYED); std::chrono::nanoseconds delay_duration(delay.nanoseconds()); std::this_thread::sleep_for(delay_duration); + is_delayed_status_ = false; } do { { @@ -569,6 +651,9 @@ bool PlayerImpl::play() if (clock_publish_timer_ != nullptr) { clock_publish_timer_->reset(); } + + update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + load_storage_content_ = true; storage_loading_future_ = std::async( std::launch::async, [this]() { @@ -599,6 +684,8 @@ bool PlayerImpl::play() ready_to_play_from_queue_cv_.notify_all(); } + update_progress_bar(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 +723,7 @@ bool PlayerImpl::play() play_next_result_ = false; finished_play_next_cv_.notify_all(); } + update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); }); return true; } @@ -672,6 +760,8 @@ void PlayerImpl::stop() cancel_wait_for_next_message_ = true; } + update_progress_bar(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,12 +782,14 @@ void PlayerImpl::pause() { clock_->pause(); RCLCPP_INFO_STREAM(owner_->get_logger(), "Pausing play."); + update_progress_bar(PlayerStatus::PAUSED); } void PlayerImpl::resume() { clock_->resume(); RCLCPP_INFO_STREAM(owner_->get_logger(), "Resuming play."); + update_progress_bar(PlayerStatus::RUNNING); } void PlayerImpl::toggle_paused() @@ -724,6 +816,7 @@ bool PlayerImpl::set_rate(double rate) } else { RCLCPP_WARN_STREAM(owner_->get_logger(), "Failed to set rate to invalid value " << rate); } + update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); return ok; } @@ -745,7 +838,7 @@ rosbag2_storage::SerializedBagMessageSharedPtr PlayerImpl::take_next_message_fro return message_queue_.take().value_or(nullptr); } -bool PlayerImpl::play_next() +bool PlayerImpl::play_next(bool is_burst_mode) { if (!is_in_playback_) { RCLCPP_WARN_STREAM(owner_->get_logger(), "Called play next, but player is not playing."); @@ -753,6 +846,7 @@ bool PlayerImpl::play_next() } if (!clock_->is_paused()) { RCLCPP_WARN_STREAM(owner_->get_logger(), "Called play next, but not in paused state."); + update_progress_bar(PlayerStatus::RUNNING); return false; } @@ -776,6 +870,11 @@ bool PlayerImpl::play_next() finished_play_next_ = false; finished_play_next_cv_.wait(lk, [this] {return finished_play_next_.load();}); play_next_ = false; + + if (!is_burst_mode) { + update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + } + return play_next_result_.exchange(false); } @@ -783,13 +882,15 @@ 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."); + update_progress_bar(PlayerStatus::RUNNING); return 0; } + update_progress_bar(PlayerStatus::BURST); uint64_t messages_played = 0; for (auto ii = 0u; ii < num_messages || num_messages == 0; ++ii) { - if (play_next()) { + if (play_next(true)) { ++messages_played; } else { break; @@ -797,6 +898,7 @@ size_t PlayerImpl::burst(const size_t num_messages) } RCLCPP_INFO_STREAM(owner_->get_logger(), "Burst " << messages_played << " messages."); + update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); return messages_played; } @@ -1053,6 +1155,11 @@ void PlayerImpl::play_messages_from_queue() play_next_result_ = true; finished_play_next_cv_.notify_all(); } + } else { + // update_progress_bar_check_rate in this code section is protected + // by the mutex skip_message_in_main_play_loop_mutex_. + update_progress_bar_check_rate( + message_ptr->recv_timestamp, PlayerStatus::RUNNING); } } message_ptr = take_next_message_from_queue(); @@ -1685,6 +1792,87 @@ const rosbag2_transport::PlayOptions & PlayerImpl::get_play_options() return play_options_; } +void PlayerImpl::print_progress_bar_help_str() const +{ + std::string help_str; + if (enable_progress_bar_) { + if (progress_bar_update_always_) { + help_str = "Progress bar enabled for every message."; + } else { + std::ostringstream oss; + oss << "Progress bar enabled at " << + play_options_.progress_bar_update_rate << " Hz"; + help_str = oss.str(); + } + RCLCPP_INFO_STREAM(owner_->get_logger(), help_str); + std::string help_str2 = "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed"; + RCLCPP_INFO_STREAM(owner_->get_logger(), help_str2); + } else { + help_str = "Progress bar disabled"; + RCLCPP_INFO_STREAM(owner_->get_logger(), help_str); + } +} + +void PlayerImpl::update_progress_bar_check_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_always_) { + 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_) + { + return; + } + progress_bar_last_time_updated_ = steady_time_now; + } + + cout_progress_bar(timestamp, status); +} + +void PlayerImpl::update_progress_bar(const PlayerStatus & status) const +{ + if (!enable_progress_bar_) { + return; + } + + // Update progress bar irrespective of the update rate set by the user. + // Overwrite the input status if we are in a special case. + if (is_delayed_status_) { + cout_progress_bar(starting_time_, PlayerStatus::DELAYED); + } else { + cout_progress_bar(std::min(clock_->now(), ending_time_), status); + } +} + +void PlayerImpl::cout_progress_bar( + const rcutils_time_point_value_t & timestamp, + const PlayerStatus & status) const +{ + const double current_time_secs = RCUTILS_NS_TO_S(static_cast(timestamp)); + const double progress_secs = current_time_secs - starting_time_secs_; + std::ostringstream oss; + oss << + // Clear and print newlines + progress_bar_helper_clear_and_move_cursor_down_ << + // Print progress bar + "====== Playback Progress ======\n" << + "[" << std::setw(13) << std::fixed << std::setprecision(9) << current_time_secs << + "] Duration " << std::setprecision(2) << progress_secs << + // 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) << "] " << + // Go up to the beginning of the blank lines + progress_bar_helper_move_cursor_up_; + std::cout << oss.str() << std::flush; +} + /////////////////////////////// // Player public interface From 347c46191d6b51914e8ec1fcb17f7b448fc8c30d Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 11 Jan 2025 00:26:44 -0800 Subject: [PATCH 02/13] Change type of progress_bar_update_rate to int32_t Signed-off-by: Michael Orlov --- ros2bag/ros2bag/verb/play.py | 8 ++++---- .../include/rosbag2_transport/play_options.hpp | 6 +++--- rosbag2_transport/src/rosbag2_transport/play_options.cpp | 4 ++-- rosbag2_transport/src/rosbag2_transport/player.cpp | 2 +- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index 79ed25611..8d3d3863e 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -182,12 +182,12 @@ def add_arguments(self, parser, cli_name): # noqa: D102 choices=['debug', 'info', 'warn', 'error', 'fatal'], help='Logging level.') parser.add_argument( - '--progress-bar', type=float, default=3.0, - help='Print a progress bar of the playback player at a specific frequency in Hz. ' + '--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_FREQUENCY') + 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 ' @@ -197,7 +197,7 @@ def add_arguments(self, parser, cli_name): # noqa: D102 '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='SEPARATION_LINES') + metavar='NUM_SEPARATION_LINES') def get_playback_until_from_arg_group(self, playback_until_sec, playback_until_nsec) -> int: nano_scale = 1000 * 1000 * 1000 diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp index c4e3f9be0..cc022b77a 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -138,12 +138,12 @@ struct PlayOptions // read_ahead_queue_size value to buffer (and order) more messages. MessageOrder message_order = MessageOrder::RECEIVED_TIMESTAMP; - // Rate in Hz at which to print progress bar. - double progress_bar_update_rate = 3.0; + // 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. - int progress_bar_separation_lines = 3; + int32_t progress_bar_separation_lines = 3; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/src/rosbag2_transport/play_options.cpp b/rosbag2_transport/src/rosbag2_transport/play_options.cpp index 4fd8d1207..9f386f084 100644 --- a/rosbag2_transport/src/rosbag2_transport/play_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/play_options.cpp @@ -117,10 +117,10 @@ bool convert::decode( optional_assign(node, "disable_loan_message", play_options.disable_loan_message); - optional_assign( + optional_assign( node, "progress_bar_update_rate", play_options.progress_bar_update_rate); - optional_assign( + 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 b7369a847..b7febdfd7 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -473,7 +473,7 @@ PlayerImpl::PlayerImpl( progress_bar_update_always_(play_options.progress_bar_update_rate < 0), progress_bar_update_period_(play_options.progress_bar_update_rate != 0 ? RCUTILS_S_TO_NS(1.0 / play_options.progress_bar_update_rate) : - std::numeric_limits::max()) + std::numeric_limits::max()) { for (auto & topic : play_options_.topics_to_filter) { topic = rclcpp::expand_topic_or_service_name( From 7dc400118c7e70d7309e534bfafeb60b91f4429d Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 11 Jan 2025 01:49:04 -0800 Subject: [PATCH 03/13] Remove "in_burst_mode" from play_next() method - Rationale: We will update progress bar with check rate from main playback loop even in burst mode. Signed-off-by: Michael Orlov --- .../src/rosbag2_transport/player.cpp | 21 +++++++------------ 1 file changed, 7 insertions(+), 14 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index b7febdfd7..484814c83 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -132,13 +132,12 @@ class PlayerImpl virtual bool set_rate(double); /// \brief Playing next message from queue when in pause. - /// \param in_burst_mode If true, it will not call the progress bar update to avoid delays. /// \details This is blocking call and it will wait until next available message will be /// published or rclcpp context shut down. /// \note If internal player queue is starving and storage has not been completely loaded, /// this method will wait until new element will be pushed to the queue. /// \return true if player in pause mode and successfully played next message, otherwise false. - virtual bool play_next(bool in_burst_mode = false); + virtual bool play_next(); /// \brief Burst the next \p num_messages messages from the queue when paused. /// \param num_messages The number of messages to burst from the queue. Specifying zero means no @@ -796,6 +795,7 @@ void PlayerImpl::toggle_paused() { // Note: Use upper level public API from owner class to facilitate unit tests owner_->is_paused() ? owner_->resume() : owner_->pause(); + update_progress_bar(owner_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); } bool PlayerImpl::is_paused() const @@ -838,7 +838,7 @@ rosbag2_storage::SerializedBagMessageSharedPtr PlayerImpl::take_next_message_fro return message_queue_.take().value_or(nullptr); } -bool PlayerImpl::play_next(bool is_burst_mode) +bool PlayerImpl::play_next() { if (!is_in_playback_) { RCLCPP_WARN_STREAM(owner_->get_logger(), "Called play next, but player is not playing."); @@ -870,11 +870,6 @@ bool PlayerImpl::play_next(bool is_burst_mode) finished_play_next_ = false; finished_play_next_cv_.wait(lk, [this] {return finished_play_next_.load();}); play_next_ = false; - - if (!is_burst_mode) { - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); - } - return play_next_result_.exchange(false); } @@ -890,7 +885,7 @@ size_t PlayerImpl::burst(const size_t num_messages) uint64_t messages_played = 0; for (auto ii = 0u; ii < num_messages || num_messages == 0; ++ii) { - if (play_next(true)) { + if (play_next()) { ++messages_played; } else { break; @@ -1155,12 +1150,10 @@ void PlayerImpl::play_messages_from_queue() play_next_result_ = true; finished_play_next_cv_.notify_all(); } - } else { - // update_progress_bar_check_rate in this code section is protected - // by the mutex skip_message_in_main_play_loop_mutex_. - update_progress_bar_check_rate( - message_ptr->recv_timestamp, PlayerStatus::RUNNING); } + // update_progress_bar_check_rate in this code section is protected + // by the mutex skip_message_in_main_play_loop_mutex_. + update_progress_bar_check_rate(message_ptr->recv_timestamp, PlayerStatus::RUNNING); } message_ptr = take_next_message_from_queue(); } From dea12f58225d9faa9c526ecce326b900e367618f Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 25 Jan 2025 15:54:40 -0800 Subject: [PATCH 04/13] Move progress bar to a separate PlayerProgressBar class - Rationale: To adhere single responsibility principle Signed-off-by: Michael Orlov --- .../src/rosbag2_transport/player.cpp | 357 +++++++++--------- 1 file changed, 189 insertions(+), 168 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 484814c83..8c1a40f3a 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -83,10 +83,159 @@ namespace rosbag2_transport enum class PlayerStatus : char { - RUNNING = 'R', - PAUSED = 'P', BURST = 'B', DELAYED = 'D', + PAUSED = 'P', + RUNNING = 'R', + STOPPED = 'S', +}; + +class PlayerProgressBar +{ +public: + explicit PlayerProgressBar( + rclcpp::Logger logger, + rcutils_time_point_value_t starting_time, + rcutils_time_point_value_t ending_time, + int32_t progress_bar_update_rate = 3, + int32_t progress_bar_separation_lines = 3) + : logger_(std::move(logger)), + enable_progress_bar_(progress_bar_update_rate != 0), + progress_bar_update_always_(progress_bar_update_rate < 0), + progress_bar_update_period_(progress_bar_update_rate != 0 ? + RCUTILS_S_TO_NS(1.0 / progress_bar_update_rate) : + std::numeric_limits::max()), + 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_secs_from_start_ = starting_time_secs_; + progress_current_time_secs_ = starting_time_secs_; + std::ostringstream oss_clear_and_move_cursor_down; + for (int i = 0; i < progress_bar_separation_lines; i++) { + oss_clear_and_move_cursor_down << "\033[2K\n"; + } + oss_clear_and_move_cursor_down << "\033[2K"; + progress_bar_helper_clear_and_move_cursor_down_ = oss_clear_and_move_cursor_down.str(); + std::ostringstream oss_move_cursor_up; + oss_move_cursor_up << "\033[" << progress_bar_separation_lines + 1 << "F"; + progress_bar_helper_move_cursor_up_ = oss_move_cursor_up.str(); + } + + ~PlayerProgressBar() + { + // arrange cursor position to be after the progress bar + if (enable_progress_bar_) { + std::ostringstream oss; + oss << "\033[" << progress_bar_separation_lines_ + 1 << "B\n"; + std::cout << oss.str() << std::flush; + } + } + void print_help_str() const + { + std::string help_str; + if (enable_progress_bar_) { + if (progress_bar_update_always_) { + help_str = "Progress bar enabled for every message."; + } else { + std::ostringstream oss; + oss << "Progress bar enabled at " << + (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz"; + help_str = oss.str(); + } + RCLCPP_INFO_STREAM(logger_, help_str); + std::string help_str2 = + "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped"; + RCLCPP_INFO_STREAM(logger_, help_str2); + } else { + help_str = "Progress bar disabled"; + RCLCPP_INFO_STREAM(logger_, help_str); + } + } + + // Update progress bar with an input timestamp, + // taking into account the update rate set by the user. + // The function should be called for regular progress bar updates, for example + // after the recurrent publishing of the messages. + // Call update_progress_bar_check_rate function only where it cannot run + // contemporaneously in multiple threads, i.e. function calls are already protected by a mutex. + // To avoid locking overhead no new mutex inside the function is directly protecting + // the access to the class attribute progress_bar_last_time_updated_. + 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_always_) { + 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_) + { + return; + } + progress_bar_last_time_updated_ = steady_time_now; + } + + draw_progress_bar(timestamp, status); + } + // Update progress bar with the current playback timestamp, + // irrespective of the update rate set by the user. + // The function should be called for extraordinary progress bar updates, for example + // when a log message is printed and we want to 'redraw' the progress bar. + 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::ostringstream oss; + oss << + // Clear and print newlines + progress_bar_helper_clear_and_move_cursor_down_ << + // Print progress bar + "====== Playback Progress ======\n" << + "[" << std::setw(13) << 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) << "] " << + // Go up to the beginning of the blank lines + progress_bar_helper_move_cursor_up_; + std::cout << oss.str() << std::flush; + } + +private: + rclcpp::Logger logger_; + 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_; + + // progress bar + bool enable_progress_bar_; + bool progress_bar_update_always_; + rcutils_duration_value_t progress_bar_update_period_; + std::chrono::steady_clock::time_point progress_bar_last_time_updated_{}; + int32_t progress_bar_separation_lines_ = 3; + double progress_secs_from_start_ = 0.0; + double progress_current_time_secs_ = 0.0; }; constexpr Player::callback_handle_t Player::invalid_callback_handle; @@ -129,7 +278,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 @@ -315,32 +464,6 @@ class PlayerImpl rcutils_time_point_value_t get_message_order_timestamp( const rosbag2_storage::SerializedBagMessageSharedPtr & message) const; - std::atomic_bool is_delayed_status_{false}; - - std::string progress_bar_helper_clear_and_move_cursor_down_; - std::string progress_bar_helper_move_cursor_up_; - - void print_progress_bar_help_str() const; - // Update progress bar with an input timestamp, - // taking into account the update rate set by the user. - // The function should be called for regular progress bar updates, for example - // after the recurrent publishing of the messages. - // Call update_progress_bar_check_rate function only where it cannot run - // contemporaneously in multiple threads, i.e. function calls are already protected by a mutex. - // To avoid locking overhead no new mutex inside the function is directly protecting - // the access to the class attribute progress_bar_last_time_updated_. - void update_progress_bar_check_rate( - const rcutils_time_point_value_t & timestamp, - const PlayerStatus & status); - // Update progress bar with the current playback timestamp, - // irrespective of the update rate set by the user. - // The function should be called for extraordinary progress bar updates, for example - // when a log message is printed and we want to 'redraw' the progress bar. - void update_progress_bar(const PlayerStatus & status) const; - void cout_progress_bar( - const rcutils_time_point_value_t & timestamp, - const PlayerStatus & status) const; - static constexpr double read_ahead_lower_bound_percentage_ = 0.9; static const std::chrono::milliseconds queue_read_wait_period_; std::atomic_bool cancel_wait_for_next_message_{false}; @@ -387,9 +510,6 @@ class PlayerImpl std::atomic_bool play_next_result_{false}; rcutils_time_point_value_t starting_time_; - rcutils_time_point_value_t ending_time_; - double starting_time_secs_; - double duration_secs_; // control services rclcpp::Service::SharedPtr srv_pause_; @@ -412,6 +532,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. @@ -435,12 +557,6 @@ class PlayerImpl return l->send_timestamp > r->send_timestamp; } } bag_message_chronological_send_timestamp_comparator; - - // progress bar - const bool enable_progress_bar_; - const bool progress_bar_update_always_; - const rcutils_duration_value_t progress_bar_update_period_; - std::chrono::steady_clock::time_point progress_bar_last_time_updated_{}; }; PlayerImpl::BagMessageComparator PlayerImpl::get_bag_message_comparator(const MessageOrder & order) @@ -467,12 +583,7 @@ PlayerImpl::PlayerImpl( play_options_(play_options), message_queue_(get_bag_message_comparator(play_options_.message_order)), keyboard_handler_(std::move(keyboard_handler)), - player_service_client_manager_(std::make_shared()), - enable_progress_bar_(play_options.progress_bar_update_rate != 0), - progress_bar_update_always_(play_options.progress_bar_update_rate < 0), - progress_bar_update_period_(play_options.progress_bar_update_rate != 0 ? - RCUTILS_S_TO_NS(1.0 / play_options.progress_bar_update_rate) : - std::numeric_limits::max()) + player_service_client_manager_(std::make_shared()) { for (auto & topic : play_options_.topics_to_filter) { topic = rclcpp::expand_topic_or_service_name( @@ -498,20 +609,10 @@ PlayerImpl::PlayerImpl( owner_->get_namespace(), false); } - std::ostringstream oss_clear_and_move_cursor_down; - for (int i = 0; i < play_options_.progress_bar_separation_lines; i++) { - oss_clear_and_move_cursor_down << "\033[2K\n"; - } - oss_clear_and_move_cursor_down << "\033[2K"; - progress_bar_helper_clear_and_move_cursor_down_ = oss_clear_and_move_cursor_down.str(); - std::ostringstream oss_move_cursor_up; - oss_move_cursor_up << "\033[" << play_options_.progress_bar_separation_lines + 1 << "F"; - progress_bar_helper_move_cursor_up_ = oss_move_cursor_up.str(); - { std::lock_guard lk(reader_mutex_); starting_time_ = std::numeric_limits::max(); - ending_time_ = std::numeric_limits::min(); + 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()}); @@ -524,8 +625,8 @@ PlayerImpl::PlayerImpl( 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 (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, @@ -539,10 +640,12 @@ PlayerImpl::PlayerImpl( } else { starting_time_ += play_options_.start_offset; } - 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_bar_ = std::make_unique( + owner_->get_logger(), 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); @@ -553,7 +656,8 @@ PlayerImpl::PlayerImpl( } create_control_services(); add_keyboard_callbacks(); - print_progress_bar_help_str(); + progress_bar_->print_help_str(); + progress_bar_->update(PlayerStatus::STOPPED); } PlayerImpl::~PlayerImpl() @@ -576,13 +680,7 @@ PlayerImpl::~PlayerImpl() reader->close(); } } - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); - // arrange cursor position to be after the progress bar - if (enable_progress_bar_) { - std::ostringstream oss; - oss << "\033[" << play_options_.progress_bar_separation_lines + 1 << "B\n"; - std::cout << oss.str() << std::flush; - } + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); } const std::chrono::milliseconds @@ -606,7 +704,7 @@ bool PlayerImpl::play() RCLCPP_WARN_STREAM( owner_->get_logger(), "Trying to play() while in playback, dismissing request."); - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); return false; } } @@ -632,12 +730,10 @@ bool PlayerImpl::play() [&, delay]() { try { if (delay > rclcpp::Duration(0, 0)) { - is_delayed_status_ = true; RCLCPP_INFO_STREAM(owner_->get_logger(), "Sleep " << delay.nanoseconds() << " ns"); - update_progress_bar(PlayerStatus::DELAYED); + progress_bar_->update(PlayerStatus::DELAYED); std::chrono::nanoseconds delay_duration(delay.nanoseconds()); std::this_thread::sleep_for(delay_duration); - is_delayed_status_ = false; } do { { @@ -651,7 +747,7 @@ bool PlayerImpl::play() clock_publish_timer_->reset(); } - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); load_storage_content_ = true; storage_loading_future_ = std::async( @@ -683,7 +779,7 @@ bool PlayerImpl::play() ready_to_play_from_queue_cv_.notify_all(); } - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + 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) { @@ -722,7 +818,7 @@ bool PlayerImpl::play() play_next_result_ = false; finished_play_next_cv_.notify_all(); } - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); }); return true; } @@ -759,7 +855,7 @@ void PlayerImpl::stop() cancel_wait_for_next_message_ = true; } - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + 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 @@ -781,21 +877,21 @@ void PlayerImpl::pause() { clock_->pause(); RCLCPP_INFO_STREAM(owner_->get_logger(), "Pausing play."); - update_progress_bar(PlayerStatus::PAUSED); + progress_bar_->update(PlayerStatus::PAUSED); } void PlayerImpl::resume() { clock_->resume(); RCLCPP_INFO_STREAM(owner_->get_logger(), "Resuming play."); - update_progress_bar(PlayerStatus::RUNNING); + 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(); - update_progress_bar(owner_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + progress_bar_->update(owner_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); } bool PlayerImpl::is_paused() const @@ -816,7 +912,7 @@ bool PlayerImpl::set_rate(double rate) } else { RCLCPP_WARN_STREAM(owner_->get_logger(), "Failed to set rate to invalid value " << rate); } - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); return ok; } @@ -846,7 +942,7 @@ bool PlayerImpl::play_next() } if (!clock_->is_paused()) { RCLCPP_WARN_STREAM(owner_->get_logger(), "Called play next, but not in paused state."); - update_progress_bar(PlayerStatus::RUNNING); + progress_bar_->update(PlayerStatus::RUNNING); return false; } @@ -877,11 +973,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."); - update_progress_bar(PlayerStatus::RUNNING); + progress_bar_->update(PlayerStatus::RUNNING); return 0; } - update_progress_bar(PlayerStatus::BURST); + progress_bar_->update(PlayerStatus::BURST); uint64_t messages_played = 0; for (auto ii = 0u; ii < num_messages || num_messages == 0; ++ii) { @@ -893,7 +989,7 @@ size_t PlayerImpl::burst(const size_t num_messages) } RCLCPP_INFO_STREAM(owner_->get_logger(), "Burst " << messages_played << " messages."); - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); return messages_played; } @@ -1151,9 +1247,15 @@ void PlayerImpl::play_messages_from_queue() finished_play_next_cv_.notify_all(); } } - // update_progress_bar_check_rate in this code section is protected + // Updating progress bar in this code section protected // by the mutex skip_message_in_main_play_loop_mutex_. - update_progress_bar_check_rate(message_ptr->recv_timestamp, PlayerStatus::RUNNING); + 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(); } @@ -1785,87 +1887,6 @@ const rosbag2_transport::PlayOptions & PlayerImpl::get_play_options() return play_options_; } -void PlayerImpl::print_progress_bar_help_str() const -{ - std::string help_str; - if (enable_progress_bar_) { - if (progress_bar_update_always_) { - help_str = "Progress bar enabled for every message."; - } else { - std::ostringstream oss; - oss << "Progress bar enabled at " << - play_options_.progress_bar_update_rate << " Hz"; - help_str = oss.str(); - } - RCLCPP_INFO_STREAM(owner_->get_logger(), help_str); - std::string help_str2 = "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed"; - RCLCPP_INFO_STREAM(owner_->get_logger(), help_str2); - } else { - help_str = "Progress bar disabled"; - RCLCPP_INFO_STREAM(owner_->get_logger(), help_str); - } -} - -void PlayerImpl::update_progress_bar_check_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_always_) { - 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_) - { - return; - } - progress_bar_last_time_updated_ = steady_time_now; - } - - cout_progress_bar(timestamp, status); -} - -void PlayerImpl::update_progress_bar(const PlayerStatus & status) const -{ - if (!enable_progress_bar_) { - return; - } - - // Update progress bar irrespective of the update rate set by the user. - // Overwrite the input status if we are in a special case. - if (is_delayed_status_) { - cout_progress_bar(starting_time_, PlayerStatus::DELAYED); - } else { - cout_progress_bar(std::min(clock_->now(), ending_time_), status); - } -} - -void PlayerImpl::cout_progress_bar( - const rcutils_time_point_value_t & timestamp, - const PlayerStatus & status) const -{ - const double current_time_secs = RCUTILS_NS_TO_S(static_cast(timestamp)); - const double progress_secs = current_time_secs - starting_time_secs_; - std::ostringstream oss; - oss << - // Clear and print newlines - progress_bar_helper_clear_and_move_cursor_down_ << - // Print progress bar - "====== Playback Progress ======\n" << - "[" << std::setw(13) << std::fixed << std::setprecision(9) << current_time_secs << - "] Duration " << std::setprecision(2) << progress_secs << - // 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) << "] " << - // Go up to the beginning of the blank lines - progress_bar_helper_move_cursor_up_; - std::cout << oss.str() << std::flush; -} - /////////////////////////////// // Player public interface From 00fbad5b2756f22e67cf3e323d487012557ed1db Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 25 Jan 2025 16:31:34 -0800 Subject: [PATCH 05/13] Move PlayerStatus inside PlayerProgressBar Signed-off-by: Michael Orlov --- .../src/rosbag2_transport/player.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 8c1a40f3a..ef594b9ac 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -81,18 +81,18 @@ rclcpp::QoS publisher_qos_for_topic( namespace rosbag2_transport { -enum class PlayerStatus : char -{ - BURST = 'B', - DELAYED = 'D', - PAUSED = 'P', - RUNNING = 'R', - STOPPED = 'S', -}; - class PlayerProgressBar { public: + enum class PlayerStatus : char + { + BURST = 'B', + DELAYED = 'D', + PAUSED = 'P', + RUNNING = 'R', + STOPPED = 'S', + }; + explicit PlayerProgressBar( rclcpp::Logger logger, rcutils_time_point_value_t starting_time, @@ -246,6 +246,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, From 6abbbe7099e157680275abf9b438bd82d1628231 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 25 Jan 2025 17:25:38 -0800 Subject: [PATCH 06/13] Move PlayerProgressBar class to a separate h(c)pp files Signed-off-by: Michael Orlov --- rosbag2_transport/CMakeLists.txt | 1 + .../rosbag2_transport/player_progress_bar.hpp | 90 ++++++++++ .../src/rosbag2_transport/player.cpp | 159 +----------------- .../rosbag2_transport/player_progress_bar.cpp | 156 +++++++++++++++++ 4 files changed, 248 insertions(+), 158 deletions(-) create mode 100644 rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp create mode 100644 rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp diff --git a/rosbag2_transport/CMakeLists.txt b/rosbag2_transport/CMakeLists.txt index 318367255..4cd4f2a4a 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 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..e2ee80e25 --- /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 "rclcpp/logger.hpp" +#include "rcutils/time.h" +#include "rosbag2_transport/visibility_control.hpp" + +namespace rosbag2_transport +{ + +class ROSBAG2_TRANSPORT_PUBLIC PlayerProgressBar +{ +public: + enum class PlayerStatus : char + { + BURST = 'B', + DELAYED = 'D', + PAUSED = 'P', + RUNNING = 'R', + STOPPED = 'S', + }; + + explicit PlayerProgressBar( + rclcpp::Logger logger, + rcutils_time_point_value_t starting_time, + rcutils_time_point_value_t ending_time, + int32_t progress_bar_update_rate = 3, + int32_t progress_bar_separation_lines = 3); + + virtual ~PlayerProgressBar(); + + void print_help_str() const; + + // Update progress bar with an input timestamp, + // taking into account the update rate set by the user. + // The function should be called for regular progress bar updates, for example + // after the recurrent publishing of the messages. + // Call update_progress_bar_check_rate function only where it cannot run + // contemporaneously in multiple threads, i.e. function calls are already protected by a mutex. + // To avoid locking overhead no new mutex inside the function is directly protecting + // the access to the class attribute progress_bar_last_time_updated_. + void update_with_limited_rate( + const rcutils_time_point_value_t & timestamp, + const PlayerStatus & status); + + // Update progress bar with the current playback timestamp, + // irrespective of the update rate set by the user. + // The function should be called for extraordinary progress bar updates, for example + // when a log message is printed and we want to 'redraw' the progress bar. + void update(const PlayerStatus & status); + + void draw_progress_bar( + const rcutils_time_point_value_t & timestamp, + const PlayerStatus & status); + +private: + rclcpp::Logger logger_; + 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_; + bool progress_bar_update_always_; + rcutils_duration_value_t progress_bar_update_period_; + std::chrono::steady_clock::time_point progress_bar_last_time_updated_{}; + int32_t progress_bar_separation_lines_ = 3; + double progress_secs_from_start_ = 0.0; + double progress_current_time_secs_ = 0.0; +}; + +} // namespace rosbag2_transport + +#endif // ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_HPP_ diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index ef594b9ac..a08aecda6 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" @@ -80,164 +81,6 @@ rclcpp::QoS publisher_qos_for_topic( namespace rosbag2_transport { - -class PlayerProgressBar -{ -public: - enum class PlayerStatus : char - { - BURST = 'B', - DELAYED = 'D', - PAUSED = 'P', - RUNNING = 'R', - STOPPED = 'S', - }; - - explicit PlayerProgressBar( - rclcpp::Logger logger, - rcutils_time_point_value_t starting_time, - rcutils_time_point_value_t ending_time, - int32_t progress_bar_update_rate = 3, - int32_t progress_bar_separation_lines = 3) - : logger_(std::move(logger)), - enable_progress_bar_(progress_bar_update_rate != 0), - progress_bar_update_always_(progress_bar_update_rate < 0), - progress_bar_update_period_(progress_bar_update_rate != 0 ? - RCUTILS_S_TO_NS(1.0 / progress_bar_update_rate) : - std::numeric_limits::max()), - 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_secs_from_start_ = starting_time_secs_; - progress_current_time_secs_ = starting_time_secs_; - std::ostringstream oss_clear_and_move_cursor_down; - for (int i = 0; i < progress_bar_separation_lines; i++) { - oss_clear_and_move_cursor_down << "\033[2K\n"; - } - oss_clear_and_move_cursor_down << "\033[2K"; - progress_bar_helper_clear_and_move_cursor_down_ = oss_clear_and_move_cursor_down.str(); - std::ostringstream oss_move_cursor_up; - oss_move_cursor_up << "\033[" << progress_bar_separation_lines + 1 << "F"; - progress_bar_helper_move_cursor_up_ = oss_move_cursor_up.str(); - } - - ~PlayerProgressBar() - { - // arrange cursor position to be after the progress bar - if (enable_progress_bar_) { - std::ostringstream oss; - oss << "\033[" << progress_bar_separation_lines_ + 1 << "B\n"; - std::cout << oss.str() << std::flush; - } - } - void print_help_str() const - { - std::string help_str; - if (enable_progress_bar_) { - if (progress_bar_update_always_) { - help_str = "Progress bar enabled for every message."; - } else { - std::ostringstream oss; - oss << "Progress bar enabled at " << - (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz"; - help_str = oss.str(); - } - RCLCPP_INFO_STREAM(logger_, help_str); - std::string help_str2 = - "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped"; - RCLCPP_INFO_STREAM(logger_, help_str2); - } else { - help_str = "Progress bar disabled"; - RCLCPP_INFO_STREAM(logger_, help_str); - } - } - - // Update progress bar with an input timestamp, - // taking into account the update rate set by the user. - // The function should be called for regular progress bar updates, for example - // after the recurrent publishing of the messages. - // Call update_progress_bar_check_rate function only where it cannot run - // contemporaneously in multiple threads, i.e. function calls are already protected by a mutex. - // To avoid locking overhead no new mutex inside the function is directly protecting - // the access to the class attribute progress_bar_last_time_updated_. - 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_always_) { - 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_) - { - return; - } - progress_bar_last_time_updated_ = steady_time_now; - } - - draw_progress_bar(timestamp, status); - } - // Update progress bar with the current playback timestamp, - // irrespective of the update rate set by the user. - // The function should be called for extraordinary progress bar updates, for example - // when a log message is printed and we want to 'redraw' the progress bar. - 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::ostringstream oss; - oss << - // Clear and print newlines - progress_bar_helper_clear_and_move_cursor_down_ << - // Print progress bar - "====== Playback Progress ======\n" << - "[" << std::setw(13) << 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) << "] " << - // Go up to the beginning of the blank lines - progress_bar_helper_move_cursor_up_; - std::cout << oss.str() << std::flush; - } - -private: - rclcpp::Logger logger_; - 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_; - - // progress bar - bool enable_progress_bar_; - bool progress_bar_update_always_; - rcutils_duration_value_t progress_bar_update_period_; - std::chrono::steady_clock::time_point progress_bar_last_time_updated_{}; - int32_t progress_bar_separation_lines_ = 3; - double progress_secs_from_start_ = 0.0; - double progress_current_time_secs_ = 0.0; -}; - constexpr Player::callback_handle_t Player::invalid_callback_handle; class PlayerImpl 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..2dc495c11 --- /dev/null +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp @@ -0,0 +1,156 @@ +// 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 +#include + +#include "rclcpp/logging.hpp" +#include "rosbag2_transport/player_progress_bar.hpp" + +namespace rosbag2_transport +{ + +PlayerProgressBar::PlayerProgressBar( + rclcpp::Logger logger, + rcutils_time_point_value_t starting_time, + rcutils_time_point_value_t ending_time, + int32_t progress_bar_update_rate, + int32_t progress_bar_separation_lines) +: logger_(std::move(logger)), + enable_progress_bar_(progress_bar_update_rate != 0), + progress_bar_update_always_(progress_bar_update_rate < 0), + progress_bar_update_period_(progress_bar_update_rate != 0 ? + RCUTILS_S_TO_NS(1.0 / progress_bar_update_rate) : + std::numeric_limits::max()), + 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_secs_from_start_ = starting_time_secs_; + progress_current_time_secs_ = starting_time_secs_; + std::ostringstream oss_clear_and_move_cursor_down; + for (int i = 0; i < progress_bar_separation_lines; i++) { + oss_clear_and_move_cursor_down << "\033[2K\n"; + } + oss_clear_and_move_cursor_down << "\033[2K"; + progress_bar_helper_clear_and_move_cursor_down_ = oss_clear_and_move_cursor_down.str(); + std::ostringstream oss_move_cursor_up; + oss_move_cursor_up << "\033[" << progress_bar_separation_lines + 1 << "F"; + progress_bar_helper_move_cursor_up_ = oss_move_cursor_up.str(); +} + +PlayerProgressBar::~PlayerProgressBar() +{ + // arrange cursor position to be after the progress bar + if (enable_progress_bar_) { + std::ostringstream oss; + oss << "\033[" << progress_bar_separation_lines_ + 1 << "B\n"; + std::cout << oss.str() << std::flush; + } +} + +void PlayerProgressBar::print_help_str() const +{ + std::string help_str; + if (enable_progress_bar_) { + if (progress_bar_update_always_) { + help_str = "Progress bar enabled for every message."; + } else { + std::ostringstream oss; + oss << "Progress bar enabled at " << + (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz"; + help_str = oss.str(); + } + RCLCPP_INFO_STREAM(logger_, help_str); + std::string help_str2 = + "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped"; + RCLCPP_INFO_STREAM(logger_, help_str2); + } else { + help_str = "Progress bar disabled"; + RCLCPP_INFO_STREAM(logger_, help_str); + } +} + +// Update progress bar with an input timestamp, +// taking into account the update rate set by the user. +// The function should be called for regular progress bar updates, for example +// after the recurrent publishing of the messages. +// Call update_progress_bar_check_rate function only where it cannot run +// contemporaneously in multiple threads, i.e. function calls are already protected by a mutex. +// To avoid locking overhead no new mutex inside the function is directly protecting +// the access to the class attribute progress_bar_last_time_updated_. +void PlayerProgressBar::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_always_) { + 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_) + { + return; + } + progress_bar_last_time_updated_ = steady_time_now; + } + + draw_progress_bar(timestamp, status); +} +// Update progress bar with the current playback timestamp, +// irrespective of the update rate set by the user. +// The function should be called for extraordinary progress bar updates, for example +// when a log message is printed and we want to 'redraw' the progress bar. +void PlayerProgressBar::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 PlayerProgressBar::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::ostringstream oss; + oss << + // Clear and print newlines + progress_bar_helper_clear_and_move_cursor_down_ << + // Print progress bar + "====== Playback Progress ======\n" << + "[" << std::setw(13) << 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) << "] " << + // Go up to the beginning of the blank lines + progress_bar_helper_move_cursor_up_; + std::cout << oss.str() << std::flush; +} + +} // namespace rosbag2_transport From cd4b0f0c23c8a953153d149f0d3d3eff422d522c Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 25 Jan 2025 18:40:18 -0800 Subject: [PATCH 07/13] Use Pimpl design pattern for PlayerProgressBar class - Also added appropriate Doxygen comments for public API Signed-off-by: Michael Orlov --- .../rosbag2_transport/player_progress_bar.hpp | 61 +++--- .../rosbag2_transport/player_progress_bar.cpp | 127 ++----------- .../player_progress_bar_impl.hpp | 173 ++++++++++++++++++ 3 files changed, 215 insertions(+), 146 deletions(-) create mode 100644 rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp diff --git a/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp b/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp index e2ee80e25..b5490762c 100644 --- a/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp @@ -15,14 +15,14 @@ #ifndef ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_HPP_ #define ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_HPP_ +#include #include -#include "rclcpp/logger.hpp" -#include "rcutils/time.h" #include "rosbag2_transport/visibility_control.hpp" namespace rosbag2_transport { +class PlayerProgressBarImpl; class ROSBAG2_TRANSPORT_PUBLIC PlayerProgressBar { @@ -36,6 +36,17 @@ class ROSBAG2_TRANSPORT_PUBLIC PlayerProgressBar STOPPED = 'S', }; + /// PlayerProgressBar constructor + /// \param logger The rclcpp::Logger to be used in cases when PlayerProgressBar needs to log its + /// own error messages. + /// \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( rclcpp::Logger logger, rcutils_time_point_value_t starting_time, @@ -45,44 +56,32 @@ class ROSBAG2_TRANSPORT_PUBLIC PlayerProgressBar virtual ~PlayerProgressBar(); + /// \brief Prints help string about player progress bar. + /// Expected to be printed once at the beginning. void print_help_str() const; - // Update progress bar with an input timestamp, - // taking into account the update rate set by the user. - // The function should be called for regular progress bar updates, for example - // after the recurrent publishing of the messages. - // Call update_progress_bar_check_rate function only where it cannot run - // contemporaneously in multiple threads, i.e. function calls are already protected by a mutex. - // To avoid locking overhead no new mutex inside the function is directly protecting - // the access to the class attribute progress_bar_last_time_updated_. + /// \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); - // Update progress bar with the current playback timestamp, - // irrespective of the update rate set by the user. - // The function should be called for extraordinary progress bar updates, for example - // when a log message is printed and we want to 'redraw' the progress bar. + /// \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); - void draw_progress_bar( - const rcutils_time_point_value_t & timestamp, - const PlayerStatus & status); - private: - rclcpp::Logger logger_; - 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_; - bool progress_bar_update_always_; - rcutils_duration_value_t progress_bar_update_period_; - std::chrono::steady_clock::time_point progress_bar_last_time_updated_{}; - int32_t progress_bar_separation_lines_ = 3; - double progress_secs_from_start_ = 0.0; - double progress_current_time_secs_ = 0.0; + std::unique_ptr pimpl_; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp index 2dc495c11..16ef51fbf 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp @@ -12,14 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include - -#include "rclcpp/logging.hpp" +#include "rclcpp/logger.hpp" +#include "rcutils/time.h" #include "rosbag2_transport/player_progress_bar.hpp" +#include "player_progress_bar_impl.hpp" namespace rosbag2_transport { @@ -30,127 +26,28 @@ PlayerProgressBar::PlayerProgressBar( rcutils_time_point_value_t ending_time, int32_t progress_bar_update_rate, int32_t progress_bar_separation_lines) -: logger_(std::move(logger)), - enable_progress_bar_(progress_bar_update_rate != 0), - progress_bar_update_always_(progress_bar_update_rate < 0), - progress_bar_update_period_(progress_bar_update_rate != 0 ? - RCUTILS_S_TO_NS(1.0 / progress_bar_update_rate) : - std::numeric_limits::max()), - 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_secs_from_start_ = starting_time_secs_; - progress_current_time_secs_ = starting_time_secs_; - std::ostringstream oss_clear_and_move_cursor_down; - for (int i = 0; i < progress_bar_separation_lines; i++) { - oss_clear_and_move_cursor_down << "\033[2K\n"; - } - oss_clear_and_move_cursor_down << "\033[2K"; - progress_bar_helper_clear_and_move_cursor_down_ = oss_clear_and_move_cursor_down.str(); - std::ostringstream oss_move_cursor_up; - oss_move_cursor_up << "\033[" << progress_bar_separation_lines + 1 << "F"; - progress_bar_helper_move_cursor_up_ = oss_move_cursor_up.str(); + pimpl_ = std::make_unique( + std::move(logger), starting_time, ending_time, + progress_bar_update_rate, progress_bar_separation_lines); } -PlayerProgressBar::~PlayerProgressBar() -{ - // arrange cursor position to be after the progress bar - if (enable_progress_bar_) { - std::ostringstream oss; - oss << "\033[" << progress_bar_separation_lines_ + 1 << "B\n"; - std::cout << oss.str() << std::flush; - } -} +PlayerProgressBar::~PlayerProgressBar() = default; void PlayerProgressBar::print_help_str() const { - std::string help_str; - if (enable_progress_bar_) { - if (progress_bar_update_always_) { - help_str = "Progress bar enabled for every message."; - } else { - std::ostringstream oss; - oss << "Progress bar enabled at " << - (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz"; - help_str = oss.str(); - } - RCLCPP_INFO_STREAM(logger_, help_str); - std::string help_str2 = - "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped"; - RCLCPP_INFO_STREAM(logger_, help_str2); - } else { - help_str = "Progress bar disabled"; - RCLCPP_INFO_STREAM(logger_, help_str); - } + pimpl_->print_help_str(); } -// Update progress bar with an input timestamp, -// taking into account the update rate set by the user. -// The function should be called for regular progress bar updates, for example -// after the recurrent publishing of the messages. -// Call update_progress_bar_check_rate function only where it cannot run -// contemporaneously in multiple threads, i.e. function calls are already protected by a mutex. -// To avoid locking overhead no new mutex inside the function is directly protecting -// the access to the class attribute progress_bar_last_time_updated_. void PlayerProgressBar::update_with_limited_rate( - const rcutils_time_point_value_t & timestamp, - const PlayerStatus & status) + 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_always_) { - 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_) - { - return; - } - progress_bar_last_time_updated_ = steady_time_now; - } - - draw_progress_bar(timestamp, status); + pimpl_->update_with_limited_rate(timestamp, status); } -// Update progress bar with the current playback timestamp, -// irrespective of the update rate set by the user. -// The function should be called for extraordinary progress bar updates, for example -// when a log message is printed and we want to 'redraw' the progress bar. + void PlayerProgressBar::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 PlayerProgressBar::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::ostringstream oss; - oss << - // Clear and print newlines - progress_bar_helper_clear_and_move_cursor_down_ << - // Print progress bar - "====== Playback Progress ======\n" << - "[" << std::setw(13) << 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) << "] " << - // Go up to the beginning of the blank lines - progress_bar_helper_move_cursor_up_; - std::cout << oss.str() << std::flush; + 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..626a85e7a --- /dev/null +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp @@ -0,0 +1,173 @@ +// 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( + rclcpp::Logger logger, + rcutils_time_point_value_t starting_time, + rcutils_time_point_value_t ending_time, + int32_t progress_bar_update_rate, + int32_t progress_bar_separation_lines) + : logger_(std::move(logger)), + enable_progress_bar_(progress_bar_update_rate != 0), + progress_bar_update_always_(progress_bar_update_rate < 0), + progress_bar_update_period_(progress_bar_update_rate != 0 ? + RCUTILS_S_TO_NS(1.0 / progress_bar_update_rate) : + std::numeric_limits::max()), + 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_secs_from_start_ = starting_time_secs_; + progress_current_time_secs_ = starting_time_secs_; + std::ostringstream oss_clear_and_move_cursor_down; + for (int i = 0; i < progress_bar_separation_lines_; i++) { + oss_clear_and_move_cursor_down << "\033[2K\n"; + } + oss_clear_and_move_cursor_down << "\033[2K"; + progress_bar_helper_clear_and_move_cursor_down_ = oss_clear_and_move_cursor_down.str(); + std::ostringstream oss_move_cursor_up; + oss_move_cursor_up << "\033[" << progress_bar_separation_lines_ + 1 << "F"; + 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::ostringstream oss; + oss << "\033[" << progress_bar_separation_lines_ + 1 << "B\n"; + std::cout << oss.str() << std::flush; + } + } + + void print_help_str() const + { + std::string help_str; + if (enable_progress_bar_) { + if (progress_bar_update_always_) { + help_str = "Progress bar enabled for every message."; + } else { + std::ostringstream oss; + oss << "Progress bar enabled at " << + (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz"; + help_str = oss.str(); + } + RCLCPP_INFO_STREAM(logger_, help_str); + std::string help_str2 = + "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped"; + RCLCPP_INFO_STREAM(logger_, help_str2); + } else { + help_str = "Progress bar disabled"; + RCLCPP_INFO_STREAM(logger_, help_str); + } + } + + 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_always_) { + 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_) + { + 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::ostringstream oss; + oss << + // Clear and print newlines + progress_bar_helper_clear_and_move_cursor_down_ << + // Print progress bar + "====== Playback Progress ======\n" << + "[" << std::setw(13) << 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) << "] " << + // Go up to the beginning of the blank lines + progress_bar_helper_move_cursor_up_; + std::cout << oss.str() << std::flush; + } + +private: + rclcpp::Logger logger_; + 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_; + bool progress_bar_update_always_; + rcutils_duration_value_t progress_bar_update_period_; + std::chrono::steady_clock::time_point progress_bar_last_time_updated_{}; + int32_t progress_bar_separation_lines_ = 3; + double progress_secs_from_start_ = 0.0; + double progress_current_time_secs_ = 0.0; +}; +} // namespace rosbag2_transport + +#endif // ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_IMPL_HPP_ From ec3cdee5a95e34f45d4429017529f8f926f15930 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 00:19:49 -0800 Subject: [PATCH 08/13] Replace logger and std::cout with reference to the std::ostream - Rationale: To be able to redirect progress bar output to the std::ostringstream in unit tests. Signed-off-by: Michael Orlov --- .../rosbag2_transport/player_progress_bar.hpp | 6 ++-- .../src/rosbag2_transport/player.cpp | 2 +- .../rosbag2_transport/player_progress_bar.cpp | 4 +-- .../player_progress_bar_impl.hpp | 31 +++++++------------ 4 files changed, 18 insertions(+), 25 deletions(-) diff --git a/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp b/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp index b5490762c..f8fc5bcf9 100644 --- a/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp @@ -37,8 +37,8 @@ class ROSBAG2_TRANSPORT_PUBLIC PlayerProgressBar }; /// PlayerProgressBar constructor - /// \param logger The rclcpp::Logger to be used in cases when PlayerProgressBar needs to log its - /// own error messages. + /// \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). @@ -48,7 +48,7 @@ class ROSBAG2_TRANSPORT_PUBLIC PlayerProgressBar /// \param progress_bar_separation_lines Number of separation lines to print in between the /// playback output and the progress bar. explicit PlayerProgressBar( - rclcpp::Logger logger, + 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, diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index a08aecda6..66f000ff8 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -486,7 +486,7 @@ PlayerImpl::PlayerImpl( } progress_bar_ = std::make_unique( - owner_->get_logger(), starting_time_, ending_time, + std::cout, starting_time_, ending_time, play_options.progress_bar_update_rate, play_options.progress_bar_separation_lines); diff --git a/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp index 16ef51fbf..7c3377dc3 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp @@ -21,14 +21,14 @@ namespace rosbag2_transport { PlayerProgressBar::PlayerProgressBar( - rclcpp::Logger logger, + std::ostream & output_stream, rcutils_time_point_value_t starting_time, rcutils_time_point_value_t ending_time, int32_t progress_bar_update_rate, int32_t progress_bar_separation_lines) { pimpl_ = std::make_unique( - std::move(logger), starting_time, ending_time, + output_stream, starting_time, ending_time, progress_bar_update_rate, progress_bar_separation_lines); } diff --git a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp index 626a85e7a..2ca2ed5fa 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp @@ -35,12 +35,12 @@ class PlayerProgressBarImpl { using PlayerStatus = PlayerProgressBar::PlayerStatus; PlayerProgressBarImpl( - rclcpp::Logger logger, + std::ostream & output_stream, rcutils_time_point_value_t starting_time, rcutils_time_point_value_t ending_time, int32_t progress_bar_update_rate, int32_t progress_bar_separation_lines) - : logger_(std::move(logger)), + : o_stream_(output_stream), enable_progress_bar_(progress_bar_update_rate != 0), progress_bar_update_always_(progress_bar_update_rate < 0), progress_bar_update_period_(progress_bar_update_rate != 0 ? @@ -71,30 +71,25 @@ class PlayerProgressBarImpl { if (enable_progress_bar_) { std::ostringstream oss; oss << "\033[" << progress_bar_separation_lines_ + 1 << "B\n"; - std::cout << oss.str() << std::flush; + o_stream_ << oss.str() << std::flush; } } void print_help_str() const { - std::string help_str; + std::ostringstream oss; if (enable_progress_bar_) { if (progress_bar_update_always_) { - help_str = "Progress bar enabled for every message."; + oss << "Progress bar enabled for every message.\n"; } else { - std::ostringstream oss; oss << "Progress bar enabled at " << - (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz"; - help_str = oss.str(); + (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz.\n"; } - RCLCPP_INFO_STREAM(logger_, help_str); - std::string help_str2 = - "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped"; - RCLCPP_INFO_STREAM(logger_, help_str2); + oss << "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped\n"; } else { - help_str = "Progress bar disabled"; - RCLCPP_INFO_STREAM(logger_, help_str); + oss << "Progress bar disabled.\n"; } + o_stream_ << oss.str() << std::flush; } void update_with_limited_rate( @@ -129,9 +124,7 @@ class PlayerProgressBarImpl { draw_progress_bar(-1, status); } - void draw_progress_bar( - const rcutils_time_point_value_t & timestamp, - const PlayerStatus & 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)); @@ -150,11 +143,11 @@ class PlayerProgressBarImpl { "/" << duration_secs_ << " [" << static_cast(status) << "] " << // Go up to the beginning of the blank lines progress_bar_helper_move_cursor_up_; - std::cout << oss.str() << std::flush; + o_stream_ << oss.str() << std::flush; } private: - rclcpp::Logger logger_; + 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_; From 7799aaa19098a84794c443ef9871d1e27922cdd1 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 00:25:49 -0800 Subject: [PATCH 09/13] Use std::stringstream and rdbuf() to avoid extra data copy to string Signed-off-by: Michael Orlov --- .../player_progress_bar_impl.hpp | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp index 2ca2ed5fa..063309287 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp @@ -69,27 +69,27 @@ class PlayerProgressBarImpl { { // arrange cursor position to be after the progress bar if (enable_progress_bar_) { - std::ostringstream oss; - oss << "\033[" << progress_bar_separation_lines_ + 1 << "B\n"; - o_stream_ << oss.str() << std::flush; + std::stringstream ss; + ss << "\033[" << progress_bar_separation_lines_ + 1 << "B\n"; + o_stream_ << ss.rdbuf() << std::flush; } } void print_help_str() const { - std::ostringstream oss; + std::stringstream ss; if (enable_progress_bar_) { if (progress_bar_update_always_) { - oss << "Progress bar enabled for every message.\n"; + ss << "Progress bar enabled for every message.\n"; } else { - oss << "Progress bar enabled at " << - (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz.\n"; + ss << "Progress bar enabled at " << + (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz.\n"; } - oss << "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped\n"; + ss << "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped\n"; } else { - oss << "Progress bar disabled.\n"; + ss << "Progress bar disabled.\n"; } - o_stream_ << oss.str() << std::flush; + o_stream_ << ss.rdbuf() << std::flush; } void update_with_limited_rate( @@ -130,20 +130,20 @@ class PlayerProgressBarImpl { progress_current_time_secs_ = RCUTILS_NS_TO_S(static_cast(timestamp)); progress_secs_from_start_ = progress_current_time_secs_ - starting_time_secs_; } - std::ostringstream oss; - oss << + std::stringstream ss; + ss << // Clear and print newlines progress_bar_helper_clear_and_move_cursor_down_ << // Print progress bar "====== Playback Progress ======\n" << - "[" << std::setw(13) << std::fixed << std::setprecision(9) << progress_current_time_secs_ << - "] Duration " << std::setprecision(2) << progress_secs_from_start_ << + "[" << std::setw(13) << 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) << "] " << // Go up to the beginning of the blank lines progress_bar_helper_move_cursor_up_; - o_stream_ << oss.str() << std::flush; + o_stream_ << ss.rdbuf() << std::flush; } private: From cb19562e99c696fe531d1a1dd1a515d9520c51cb Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 00:34:18 -0800 Subject: [PATCH 10/13] Change type of the `progress_bar_separation_lines` to the `uint32_t` Signed-off-by: Michael Orlov --- .../include/rosbag2_transport/play_options.hpp | 5 ++--- .../rosbag2_transport/player_progress_bar.hpp | 2 +- .../src/rosbag2_transport/play_options.cpp | 2 +- .../src/rosbag2_transport/player_progress_bar.cpp | 2 +- .../rosbag2_transport/player_progress_bar_impl.hpp | 12 ++++++------ 5 files changed, 11 insertions(+), 12 deletions(-) diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp index cc022b77a..71e68c90d 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -141,9 +141,8 @@ struct PlayOptions // 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. - int32_t progress_bar_separation_lines = 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 index f8fc5bcf9..5f802139a 100644 --- a/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp @@ -52,7 +52,7 @@ class ROSBAG2_TRANSPORT_PUBLIC PlayerProgressBar rcutils_time_point_value_t starting_time, rcutils_time_point_value_t ending_time, int32_t progress_bar_update_rate = 3, - int32_t progress_bar_separation_lines = 3); + uint32_t progress_bar_separation_lines = 3); virtual ~PlayerProgressBar(); diff --git a/rosbag2_transport/src/rosbag2_transport/play_options.cpp b/rosbag2_transport/src/rosbag2_transport/play_options.cpp index 9f386f084..72aefb57d 100644 --- a/rosbag2_transport/src/rosbag2_transport/play_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/play_options.cpp @@ -120,7 +120,7 @@ bool convert::decode( optional_assign( node, "progress_bar_update_rate", play_options.progress_bar_update_rate); - optional_assign( + optional_assign( node, "progress_bar_separation_lines", play_options.progress_bar_separation_lines); return true; diff --git a/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp index 7c3377dc3..d583720d9 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp @@ -25,7 +25,7 @@ PlayerProgressBar::PlayerProgressBar( rcutils_time_point_value_t starting_time, rcutils_time_point_value_t ending_time, int32_t progress_bar_update_rate, - int32_t progress_bar_separation_lines) + uint32_t progress_bar_separation_lines) { pimpl_ = std::make_unique( output_stream, starting_time, ending_time, diff --git a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp index 063309287..1ff2e2438 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp @@ -39,7 +39,7 @@ class PlayerProgressBarImpl { rcutils_time_point_value_t starting_time, rcutils_time_point_value_t ending_time, int32_t progress_bar_update_rate, - int32_t progress_bar_separation_lines) + uint32_t progress_bar_separation_lines) : o_stream_(output_stream), enable_progress_bar_(progress_bar_update_rate != 0), progress_bar_update_always_(progress_bar_update_rate < 0), @@ -55,7 +55,7 @@ class PlayerProgressBarImpl { progress_secs_from_start_ = starting_time_secs_; progress_current_time_secs_ = starting_time_secs_; std::ostringstream oss_clear_and_move_cursor_down; - for (int i = 0; i < progress_bar_separation_lines_; i++) { + for (uint32_t i = 0; i < progress_bar_separation_lines_; i++) { oss_clear_and_move_cursor_down << "\033[2K\n"; } oss_clear_and_move_cursor_down << "\033[2K"; @@ -83,7 +83,7 @@ class PlayerProgressBarImpl { ss << "Progress bar enabled for every message.\n"; } else { ss << "Progress bar enabled at " << - (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz.\n"; + (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz.\n"; } ss << "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped\n"; } else { @@ -136,8 +136,8 @@ class PlayerProgressBarImpl { progress_bar_helper_clear_and_move_cursor_down_ << // Print progress bar "====== Playback Progress ======\n" << - "[" << std::setw(13) << std::fixed << std::setprecision(9) << progress_current_time_secs_ << - "] Duration " << std::setprecision(2) << progress_secs_from_start_ << + "[" << std::setw(13) << 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) << "] " << @@ -157,7 +157,7 @@ class PlayerProgressBarImpl { bool progress_bar_update_always_; rcutils_duration_value_t progress_bar_update_period_; std::chrono::steady_clock::time_point progress_bar_last_time_updated_{}; - int32_t progress_bar_separation_lines_ = 3; + uint32_t progress_bar_separation_lines_ = 3; double progress_secs_from_start_ = 0.0; double progress_current_time_secs_ = 0.0; }; From a13ca0c53f8f1ba26bb20586330760d22a1c18e7 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 01:13:45 -0800 Subject: [PATCH 11/13] Simplify logic by using update period in milliseconds - The progress_bar_update_period_ms_ equal to 0 means update always. - Also use integer arithmetic for the sake of performance. Signed-off-by: Michael Orlov --- .../player_progress_bar_impl.hpp | 25 ++++++++----------- 1 file changed, 10 insertions(+), 15 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp index 1ff2e2438..aaec4ad1f 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp @@ -42,10 +42,8 @@ class PlayerProgressBarImpl { uint32_t progress_bar_separation_lines) : o_stream_(output_stream), enable_progress_bar_(progress_bar_update_rate != 0), - progress_bar_update_always_(progress_bar_update_rate < 0), - progress_bar_update_period_(progress_bar_update_rate != 0 ? - RCUTILS_S_TO_NS(1.0 / progress_bar_update_rate) : - std::numeric_limits::max()), + 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( @@ -79,11 +77,10 @@ class PlayerProgressBarImpl { { std::stringstream ss; if (enable_progress_bar_) { - if (progress_bar_update_always_) { - ss << "Progress bar enabled for every message.\n"; + 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 at " << - (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz.\n"; + ss << "Progress bar enabled for every message.\n"; } ss << "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped\n"; } else { @@ -102,10 +99,10 @@ class PlayerProgressBarImpl { // 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_always_) { + 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_) + if (std::chrono::duration_cast( + steady_time_now - progress_bar_last_time_updated_).count() < progress_bar_update_period_ms_) { return; } @@ -126,7 +123,7 @@ class PlayerProgressBarImpl { void draw_progress_bar(const rcutils_time_point_value_t & timestamp, const PlayerStatus & status) { - if (timestamp > 0) { + 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_; } @@ -152,10 +149,8 @@ class PlayerProgressBarImpl { 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_; - bool progress_bar_update_always_; - rcutils_duration_value_t progress_bar_update_period_; + 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; From f09eaeab7e8efb29d71de400f1664469c849677a Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 11:32:05 -0800 Subject: [PATCH 12/13] Regenerate pyi stub files Signed-off-by: Michael Orlov --- rosbag2_py/rosbag2_py/_transport.pyi | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbag2_py/rosbag2_py/_transport.pyi b/rosbag2_py/rosbag2_py/_transport.pyi index 010a1125b..727c469a9 100644 --- a/rosbag2_py/rosbag2_py/_transport.pyi +++ b/rosbag2_py/rosbag2_py/_transport.pyi @@ -35,7 +35,7 @@ class PlayOptions: playback_duration: float playback_until_timestamp: int progress_bar_separation_lines: int - progress_bar_update_rate: float + progress_bar_update_rate: int publish_service_requests: bool rate: float read_ahead_queue_size: int From 42e5134e76c2eed227e3797a660ec9340c9f2a77 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 12:16:58 -0800 Subject: [PATCH 13/13] Add test_player_progress_bar.cpp with TODOs Signed-off-by: Michael Orlov --- rosbag2_transport/CMakeLists.txt | 3 + .../rosbag2_transport/player_progress_bar.hpp | 1 + .../test_player_progress_bar.cpp | 92 +++++++++++++++++++ 3 files changed, 96 insertions(+) create mode 100644 rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp diff --git a/rosbag2_transport/CMakeLists.txt b/rosbag2_transport/CMakeLists.txt index 4cd4f2a4a..eefe93a5e 100644 --- a/rosbag2_transport/CMakeLists.txt +++ b/rosbag2_transport/CMakeLists.txt @@ -535,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/player_progress_bar.hpp b/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp index 5f802139a..224094d32 100644 --- a/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp @@ -18,6 +18,7 @@ #include #include +#include "rcutils/time.h" #include "rosbag2_transport/visibility_control.hpp" namespace rosbag2_transport 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..482ab9889 --- /dev/null +++ b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp @@ -0,0 +1,92 @@ +// 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 "rosbag2_transport/player_progress_bar.hpp" + +using namespace ::testing; // NOLINT +using namespace rosbag2_transport; // NOLINT + +class TestPlayerProgressBar : public Test +{ +}; + +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, 0, 0); + progress_bar->print_help_str(); + progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); + // TODO(someone): Check output in the oss + } +} + +TEST_F(TestPlayerProgressBar, print_help_str_with_zero_update_rate) { + // TODO(someone): Add content for this test +} + +TEST_F(TestPlayerProgressBar, print_help_str_with_negative_update_rate) { + // TODO(someone): Add content for this test +} + +TEST_F(TestPlayerProgressBar, print_help_str_with_positive_update_rate) { + // TODO(someone): Add content for this test +} + +TEST_F(TestPlayerProgressBar, update_status_with_disabled_progress_bar) { + // TODO(someone): Add content for this test +} + +TEST_F(TestPlayerProgressBar, update_status_with_enabled_progress_bar) { + // TODO(someone): Add content for this test +} + +TEST_F(TestPlayerProgressBar, update_status_with_separation_lines) { + // TODO(someone): Add content for this test +} + +TEST_F(TestPlayerProgressBar, update_status_without_separation_lines) { + // TODO(someone): Add content for this test +} + +TEST_F(TestPlayerProgressBar, update_with_limited_rate_respect_update_rate) { + // TODO(someone): Add content for this test +} + +TEST_F(TestPlayerProgressBar, update_with_limited_rate_update_progress) { + // TODO(someone): Add content for this test +} + +TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_negative_update_rate) { + // TODO(someone): Add content for this test +} + +TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_update_rate) { + // TODO(someone): Add content for this test +} + +TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_timestamp) { + // TODO(someone): Add content for this test +}