From a3c7c47c0d6db62a7cca17c9b5048ab83a6ceeae Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Fri, 10 Jan 2025 20:36:03 -0800 Subject: [PATCH 01/33] Squashed commits of the initial nicolaloi progress bar implementation 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 11890b508..1b4489197 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 01b41ec49..c08234c94 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 @@ -308,6 +318,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}; @@ -354,6 +390,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_; @@ -399,6 +438,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) @@ -425,7 +470,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( @@ -451,9 +501,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()}); @@ -461,9 +522,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 @@ -476,6 +542,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); @@ -486,6 +556,7 @@ PlayerImpl::PlayerImpl( } create_control_services(); add_keyboard_callbacks(); + print_progress_bar_help_str(); } PlayerImpl::~PlayerImpl() @@ -508,6 +579,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 @@ -531,6 +609,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; } } @@ -556,9 +635,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 { { @@ -571,6 +653,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]() { @@ -601,6 +686,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); @@ -638,6 +725,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; } @@ -674,6 +762,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(); @@ -694,12 +784,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() @@ -726,6 +818,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; } @@ -747,7 +840,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."); @@ -755,6 +848,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; } @@ -778,6 +872,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); } @@ -785,13 +884,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; @@ -799,6 +900,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; } @@ -1059,6 +1161,11 @@ void PlayerImpl::play_messages_from_queue() finished_play_next_ = true; play_next_result_ = message_published; 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(); @@ -1708,6 +1815,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 12588395d82180fc186b259fa8d834234a2e507c Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 11 Jan 2025 00:26:44 -0800 Subject: [PATCH 02/33] 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 c08234c94..d4c3c8a89 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -475,7 +475,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 2b75b6ff6a604d08832b2526b52d0cd60939af50 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 11 Jan 2025 01:49:04 -0800 Subject: [PATCH 03/33] 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 d4c3c8a89..fc5c48403 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 @@ -798,6 +797,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 @@ -840,7 +840,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."); @@ -872,11 +872,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); } @@ -892,7 +887,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; @@ -1161,12 +1156,10 @@ void PlayerImpl::play_messages_from_queue() finished_play_next_ = true; play_next_result_ = message_published; 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 fa4b2207a5649cc425a869efd58963adc2645a97 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 25 Jan 2025 15:54:40 -0800 Subject: [PATCH 04/33] 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 fc5c48403..5bcd1def4 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 @@ -317,32 +466,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}; @@ -389,9 +512,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_; @@ -414,6 +534,8 @@ class PlayerImpl std::shared_ptr player_service_client_manager_; + std::unique_ptr progress_bar_; + static BagMessageComparator get_bag_message_comparator(const MessageOrder & order); /// Comparator for SerializedBagMessageSharedPtr to order chronologically by recv_timestamp. @@ -437,12 +559,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) @@ -469,12 +585,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( @@ -500,20 +611,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()}); @@ -526,8 +627,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, @@ -541,10 +642,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); @@ -555,7 +658,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() @@ -578,13 +682,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 @@ -608,7 +706,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; } } @@ -634,12 +732,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 { { @@ -653,7 +749,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( @@ -685,7 +781,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) { @@ -724,7 +820,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; } @@ -761,7 +857,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 @@ -783,21 +879,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 @@ -818,7 +914,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; } @@ -848,7 +944,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; } @@ -879,11 +975,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) { @@ -895,7 +991,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; } @@ -1157,9 +1253,15 @@ void PlayerImpl::play_messages_from_queue() play_next_result_ = message_published; 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(); } @@ -1808,87 +1910,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 5f13cca898db00211dbf11cee71a995b575ac9b0 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 25 Jan 2025 16:31:34 -0800 Subject: [PATCH 05/33] 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 5bcd1def4..01771b2d5 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 fa7b943eebb47ef0f6197b2214fa0036d22856fc Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 25 Jan 2025 17:25:38 -0800 Subject: [PATCH 06/33] 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 01771b2d5..6b5ee97cf 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 c6b03799bf50c796903cc26a11998e7436637347 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 25 Jan 2025 18:40:18 -0800 Subject: [PATCH 07/33] 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 b526d783fb9346680228a44812756802859b093f Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 00:19:49 -0800 Subject: [PATCH 08/33] 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 6b5ee97cf..80bd764c4 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -488,7 +488,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 89497800335f9bef13414fe540f3947fc224c22e Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 00:25:49 -0800 Subject: [PATCH 09/33] 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 ae26cb62a0e73b40e21668d59c10a70e52f4fb6b Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 00:34:18 -0800 Subject: [PATCH 10/33] 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 3990386209e35dd436ec54ac9821bd0a97cfd00c Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 01:13:45 -0800 Subject: [PATCH 11/33] 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 cb95bf827c216fc4fa7339961fedb295492694c3 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 11:32:05 -0800 Subject: [PATCH 12/33] 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 6472ce359654427cb19a2798cde69365639e55b2 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 12:16:58 -0800 Subject: [PATCH 13/33] 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 +} From 5d8978e4031b50c2cee12c4d8f47f43b8eea0d46 Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Thu, 30 Jan 2025 00:19:04 +0000 Subject: [PATCH 14/33] Add some unit tests Signed-off-by: Nicola Loi --- .../player_progress_bar_impl.hpp | 2 +- .../test_player_progress_bar.cpp | 318 +++++++++++++++++- 2 files changed, 308 insertions(+), 12 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 aaec4ad1f..f0a2d7602 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp @@ -50,8 +50,8 @@ class PlayerProgressBarImpl { 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_; + progress_secs_from_start_ = progress_current_time_secs_ - starting_time_secs_; std::ostringstream oss_clear_and_move_cursor_down; for (uint32_t i = 0; i < progress_bar_separation_lines_; i++) { oss_clear_and_move_cursor_down << "\033[2K\n"; diff --git a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp index 482ab9889..a4fd9930f 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp @@ -16,6 +16,7 @@ #include #include +#include #include "rosbag2_transport/player_progress_bar.hpp" @@ -24,6 +25,44 @@ using namespace rosbag2_transport; // NOLINT class TestPlayerProgressBar : public Test { +public: + std::string GenerateClearAndMoveCursorDownRegexString(uint32_t num_lines) + { + std::ostringstream oss; + for (uint32_t i = 0; i < num_lines; i++) { + oss << "\x1B[2K\n"; + } + oss << "\x1B[2K"; + return CursorMoveStringEscapeRegex(oss.str()); + } + + std::string GenerateMoveCursorUpRegexString(uint32_t num_lines) + { + std::ostringstream oss; + oss << "\x1B[" << num_lines + 1 << "F"; + return CursorMoveStringEscapeRegex(oss.str()); + } + + std::vector status_list_ = { + PlayerProgressBar::PlayerStatus::DELAYED, + PlayerProgressBar::PlayerStatus::RUNNING, + PlayerProgressBar::PlayerStatus::PAUSED, + PlayerProgressBar::PlayerStatus::BURST, + PlayerProgressBar::PlayerStatus::STOPPED + }; + +private: + std::string CursorMoveStringEscapeRegex(std::string str) + { + std::ostringstream oss; + for (const char c : str) { + if (c == '[' || c == ']') { + oss << "\\"; + } + oss << c; + } + return oss.str(); + } }; TEST_F(TestPlayerProgressBar, default_ctor_dtor) { @@ -39,40 +78,233 @@ TEST_F(TestPlayerProgressBar, can_dtor_after_output) { 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 + std::ostringstream oss; + { + auto progress_bar = std::make_unique(oss, 0, 0, 0); + progress_bar->print_help_str(); + EXPECT_EQ(oss.str(), "Progress bar disabled.\n"); + } } TEST_F(TestPlayerProgressBar, print_help_str_with_negative_update_rate) { - // TODO(someone): Add content for this test + { + std::ostringstream oss; + auto progress_bar = std::make_unique(oss, 0, 0, -1); + progress_bar->print_help_str(); + EXPECT_EQ(oss.str(), + "Progress bar enabled for every message." + "\nProgress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped\n"); + } } TEST_F(TestPlayerProgressBar, print_help_str_with_positive_update_rate) { - // TODO(someone): Add content for this test + { + std::ostringstream oss; + auto progress_bar = std::make_unique(oss, 0, 0, 1); + progress_bar->print_help_str(); + EXPECT_EQ(oss.str(), + "Progress bar enabled at 1 Hz." + "\nProgress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped\n"); + } } TEST_F(TestPlayerProgressBar, update_status_with_disabled_progress_bar) { - // TODO(someone): Add content for this test + { + std::ostringstream oss; + auto progress_bar = std::make_unique(oss, 0, 0, 0); + for (const auto & status : status_list_) { + progress_bar->update(status); + EXPECT_THAT(oss.str(), IsEmpty()); + oss.clear(); + oss.str(""); + } + } } TEST_F(TestPlayerProgressBar, update_status_with_enabled_progress_bar) { - // TODO(someone): Add content for this test + { + // Test Playback Progress header + std::ostringstream oss; + auto progress_bar = std::make_unique(oss, 0, 0, 1); + progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update(PlayerProgressBar::PlayerStatus::STOPPED); + EXPECT_THAT(oss.str(), MatchesRegex( + ".*====== Playback Progress ======" + "\n.*\\[R\\]" + ".*====== Playback Progress ======" + "\n.*\\[S\\].*")); + } + + { + // Test status update with update rate 1 Hz + std::ostringstream oss; + auto progress_bar = std::make_unique(oss, 0, 0, 1); + progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update(PlayerProgressBar::PlayerStatus::STOPPED); + EXPECT_THAT(oss.str(), MatchesRegex( + ".*\\[R\\].*\n.*\\[S\\].*")); + } + + { + // Test status update with update rate 10 Hz + std::ostringstream oss; + auto progress_bar = std::make_unique(oss, 0, 0, 10); + progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update(PlayerProgressBar::PlayerStatus::STOPPED); + EXPECT_THAT(oss.str(), MatchesRegex( + ".*\\[R\\].*\n.*\\[S\\].*")); + } + + { + // Test status update on all statuses + std::ostringstream oss; + auto progress_bar = std::make_unique(oss, 0, 0, 1); + for (const auto & status : status_list_) { + progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update(status); + std::string status_str(1, static_cast(status)); + EXPECT_THAT(oss.str(), MatchesRegex( + ".*\\[R\\].*\n.*\\[" + status_str + "\\].*")); + oss.clear(); + oss.str(""); + } + } } TEST_F(TestPlayerProgressBar, update_status_with_separation_lines) { - // TODO(someone): Add content for this test + { + // Test status update with 2 separation lines + std::ostringstream oss; + uint32_t num_separation_lines = 2; + std::string expected_pre_line_separator = + GenerateClearAndMoveCursorDownRegexString(num_separation_lines); + std::string expected_post_line_separator = + GenerateMoveCursorUpRegexString(num_separation_lines); + auto progress_bar = std::make_unique(oss, 0, 0, 1, num_separation_lines); + progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); + progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); + EXPECT_THAT(oss.str(), MatchesRegex( + expected_pre_line_separator + + ".*\\[D\\].*" + expected_post_line_separator + + expected_pre_line_separator + + ".*\\[R\\].*" + expected_post_line_separator)); + } + + { + // Test status update with 5 separation lines + std::ostringstream oss; + uint32_t num_separation_lines = 5; + std::string expected_pre_line_separator = + GenerateClearAndMoveCursorDownRegexString(num_separation_lines); + std::string expected_post_line_separator = + GenerateMoveCursorUpRegexString(num_separation_lines); + auto progress_bar = std::make_unique(oss, 0, 0, 1, num_separation_lines); + progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); + progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update(PlayerProgressBar::PlayerStatus::PAUSED); + EXPECT_THAT(oss.str(), MatchesRegex( + expected_pre_line_separator + + ".*\\[D\\].*" + expected_post_line_separator + + expected_pre_line_separator + + ".*\\[R\\].*" + expected_post_line_separator + + expected_pre_line_separator + + ".*\\[P\\].*" + expected_post_line_separator)); + } + + { + // Test status update with 5 separation lines and external log messages + std::ostringstream oss; + uint32_t num_separation_lines = 5; + std::string expected_pre_line_separator = + GenerateClearAndMoveCursorDownRegexString(num_separation_lines); + std::string expected_post_line_separator = + GenerateMoveCursorUpRegexString(num_separation_lines); + auto progress_bar = std::make_unique(oss, 0, 0, 1, num_separation_lines); + progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); + oss << "log msg 1\n"; + oss << "log msg 2\n"; + oss << "log msg 3\n"; + oss << "log msg 4\n"; + progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); + oss << "log msg 5\n"; + oss << "log msg 6\n"; + progress_bar->update(PlayerProgressBar::PlayerStatus::PAUSED); + EXPECT_THAT(oss.str(), MatchesRegex( + expected_pre_line_separator + + ".*\\[D\\].*" + expected_post_line_separator + + "log msg 1\nlog msg 2\nlog msg 3\nlog msg 4\n" + + expected_pre_line_separator + + ".*\\[R\\].*" + expected_post_line_separator + + "log msg 5\nlog msg 6\n" + + expected_pre_line_separator + + ".*\\[P\\].*" + expected_post_line_separator)); + } } TEST_F(TestPlayerProgressBar, update_status_without_separation_lines) { - // TODO(someone): Add content for this test + std::ostringstream oss; + { + // Test status update without separation lines + uint32_t num_separation_lines = 0; + std::string expected_pre_line_separator = + GenerateClearAndMoveCursorDownRegexString(num_separation_lines); + std::string expected_post_line_separator = + GenerateMoveCursorUpRegexString(num_separation_lines); + auto progress_bar = std::make_unique(oss, 0, 0, 1, num_separation_lines); + progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); + EXPECT_THAT(oss.str(), MatchesRegex( + expected_pre_line_separator + + ".*\\[D\\].*" + expected_post_line_separator)); + + progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); + EXPECT_THAT(oss.str(), MatchesRegex( + expected_pre_line_separator + + ".*\\[D\\].*" + expected_post_line_separator + + expected_pre_line_separator + + ".*\\[R\\].*" + expected_post_line_separator)); + } } TEST_F(TestPlayerProgressBar, update_with_limited_rate_respect_update_rate) { - // TODO(someone): Add content for this test + rcutils_time_point_value_t starting_time = 1e18; // nanoseconds + rcutils_time_point_value_t ending_time = starting_time + 5e9; + { + // Test if update rate of the progress bar is respected + std::ostringstream oss; + int32_t update_rate = 3; + uint64_t update_period_ns = 1e9 / update_rate; + auto progress_bar = std::make_unique( + oss, starting_time, ending_time, update_rate, 0); + std::vector timestamps; + timestamps.push_back(starting_time); // 1st update progress bar + // add timestamps relative to 1st update progress bar + timestamps.push_back(timestamps.at(0) + 0.3 * update_period_ns); // skip update + timestamps.push_back(timestamps.at(0) + 0.5 * update_period_ns); // skip update + timestamps.push_back(timestamps.at(0) + 1.3 * update_period_ns); // 2nd update + // add timestamps relative to 2nd update progress bar + timestamps.push_back(timestamps.at(3) + 0.3 * update_period_ns); // skip update + timestamps.push_back(timestamps.at(3) + 1.3 * update_period_ns); // 3rd update + + progress_bar->update_with_limited_rate( + timestamps[0], PlayerProgressBar::PlayerStatus::RUNNING); + for (size_t i = 1; i < timestamps.size(); ++i) { + { + std::this_thread::sleep_for( + std::chrono::nanoseconds(timestamps[i] - timestamps[i - 1])); + } + progress_bar->update_with_limited_rate( + timestamps[i], PlayerProgressBar::PlayerStatus::RUNNING); + } + // Check if the progress bar is updated at the correct 3 timestamps + EXPECT_THAT(oss.str(), MatchesRegex( + ".*\\[1000000000\\.000000000\\] Duration 0\\.00/5\\.00" + ".*\\[1000000000\\.4333.*\\] Duration 0\\.43/5\\.00" + ".*\\[1000000000\\.8666.*\\] Duration 0\\.87/5\\.00.*")); + } } TEST_F(TestPlayerProgressBar, update_with_limited_rate_update_progress) { @@ -80,11 +312,75 @@ TEST_F(TestPlayerProgressBar, update_with_limited_rate_update_progress) { } TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_negative_update_rate) { - // TODO(someone): Add content for this test + rcutils_time_point_value_t starting_time = 1e18; // nanoseconds + rcutils_time_point_value_t ending_time = starting_time + 5e9; + int32_t update_rate = -1; + { + // Test if progress bar is updated every time + std::ostringstream oss; + auto progress_bar = std::make_unique( + oss, starting_time, ending_time, update_rate, 0); + rcutils_time_point_value_t timestamp_1 = starting_time; + rcutils_time_point_value_t timestamp_2 = starting_time + 1e6; + rcutils_time_point_value_t timestamp_3 = starting_time + 2e6; + progress_bar->update_with_limited_rate(timestamp_1, PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update_with_limited_rate(timestamp_2, PlayerProgressBar::PlayerStatus::STOPPED); + progress_bar->update_with_limited_rate(timestamp_3, PlayerProgressBar::PlayerStatus::RUNNING); + EXPECT_THAT(oss.str(), MatchesRegex( + ".*\\[1000000000\\.000000000\\] Duration 0\\.00/5\\.00" + ".*\\[1000000000\\.0009.*\\] Duration 0\\.00/5\\.00" + ".*\\[1000000000\\.0019.*\\] Duration 0\\.00/5\\.00.*")); + } + + { + // Test if progress bar is updated every time + std::ostringstream oss; + auto progress_bar = std::make_unique( + oss, starting_time, ending_time, update_rate, 0); + rcutils_time_point_value_t timestamp_1 = starting_time; + rcutils_time_point_value_t timestamp_2 = starting_time + 1e9; + rcutils_time_point_value_t timestamp_3 = starting_time + 2e9; + progress_bar->update_with_limited_rate(timestamp_1, PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update_with_limited_rate(timestamp_2, PlayerProgressBar::PlayerStatus::STOPPED); + progress_bar->update_with_limited_rate(timestamp_3, PlayerProgressBar::PlayerStatus::RUNNING); + EXPECT_THAT(oss.str(), MatchesRegex( + ".*\\[1000000000\\.000000000\\] Duration 0\\.00/5\\.00" + ".*\\[1000000001\\.000000000\\] Duration 1\\.00/5\\.00" + ".*\\[1000000002\\.000000000\\] Duration 2\\.00/5\\.00.*")); + } } TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_update_rate) { - // TODO(someone): Add content for this test + rcutils_time_point_value_t starting_time = 1e18; // nanoseconds + rcutils_time_point_value_t ending_time = starting_time + 5e9; + int32_t update_rate = 0; + { + // Test if progress bar is not updated + std::ostringstream oss; + auto progress_bar = std::make_unique( + oss, starting_time, ending_time, update_rate); + rcutils_time_point_value_t timestamp_1 = starting_time; + rcutils_time_point_value_t timestamp_2 = starting_time + 1e6; + rcutils_time_point_value_t timestamp_3 = starting_time + 2e6; + progress_bar->update_with_limited_rate(timestamp_1, PlayerProgressBar::PlayerStatus::DELAYED); + progress_bar->update_with_limited_rate(timestamp_2, PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update_with_limited_rate(timestamp_3, PlayerProgressBar::PlayerStatus::RUNNING); + EXPECT_THAT(oss.str(), IsEmpty()); + } + + { + // Test if progress bar is not updated + std::ostringstream oss; + auto progress_bar = std::make_unique( + oss, starting_time, ending_time, update_rate); + rcutils_time_point_value_t timestamp_1 = starting_time; + rcutils_time_point_value_t timestamp_2 = starting_time + 1e9; + rcutils_time_point_value_t timestamp_3 = starting_time + 2e9; + progress_bar->update_with_limited_rate(timestamp_1, PlayerProgressBar::PlayerStatus::DELAYED); + progress_bar->update_with_limited_rate(timestamp_2, PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update_with_limited_rate(timestamp_3, PlayerProgressBar::PlayerStatus::RUNNING); + EXPECT_THAT(oss.str(), IsEmpty()); + } } TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_timestamp) { From d804c6cf1a00b21eba169d2dbb60a80608176f23 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Wed, 5 Feb 2025 19:09:30 -0800 Subject: [PATCH 15/33] Cleanup mess with ANSI control codes Signed-off-by: Michael Orlov --- .../player_progress_bar_impl.hpp | 18 +++++++++--- .../test_player_progress_bar.cpp | 28 +++++++------------ 2 files changed, 24 insertions(+), 22 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 f0a2d7602..b9169c83e 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp @@ -53,13 +53,18 @@ class PlayerProgressBarImpl { progress_current_time_secs_ = starting_time_secs_; progress_secs_from_start_ = progress_current_time_secs_ - starting_time_secs_; std::ostringstream oss_clear_and_move_cursor_down; + for (uint32_t i = 0; i < progress_bar_separation_lines_; i++) { + // The ANSI control code "\033[2K" is used to clear an entire line in the terminal. + // Cleanup current line and jump down to one new line with "\n" oss_clear_and_move_cursor_down << "\033[2K\n"; } - 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"; + // Move cursor up by a specific number of lines using "Cursor Up" '\033[A' ANSI control code + oss_move_cursor_up << "\033[" << + progress_bar_separation_lines_ + progress_bar_lines_count_ << "A"; progress_bar_helper_move_cursor_up_ = oss_move_cursor_up.str(); } @@ -68,7 +73,7 @@ class PlayerProgressBarImpl { // arrange cursor position to be after the progress bar if (enable_progress_bar_) { std::stringstream ss; - ss << "\033[" << progress_bar_separation_lines_ + 1 << "B\n"; + ss << "\033[" << progress_bar_separation_lines_ + progress_bar_lines_count_ << "B"; o_stream_ << ss.rdbuf() << std::flush; } } @@ -137,7 +142,7 @@ class PlayerProgressBarImpl { "] 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) << "] " << + "/" << duration_secs_ << " [" << static_cast(status) << "] \n" << // Go up to the beginning of the blank lines progress_bar_helper_move_cursor_up_; o_stream_ << ss.rdbuf() << std::flush; @@ -155,6 +160,11 @@ class PlayerProgressBarImpl { uint32_t progress_bar_separation_lines_ = 3; double progress_secs_from_start_ = 0.0; double progress_current_time_secs_ = 0.0; + + /// progress_bar_lines_count_ - The number of lines in progress bar to be printed out. + /// ====== Playback Progress ====== + /// [0.000000000] Duration 0.00/0.00 [R] + static const size_t progress_bar_lines_count_ = 2; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp index a4fd9930f..4db64aae8 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp @@ -30,17 +30,22 @@ class TestPlayerProgressBar : public Test { std::ostringstream oss; for (uint32_t i = 0; i < num_lines; i++) { - oss << "\x1B[2K\n"; + // The ANSI control code "\033[2K" is used to clear an entire line in the terminal. + // Cleanup current line and jump down to one new line with "\n" + oss << "\033\\[2K\n"; } - oss << "\x1B[2K"; - return CursorMoveStringEscapeRegex(oss.str()); + return oss.str(); } std::string GenerateMoveCursorUpRegexString(uint32_t num_lines) { + static const size_t progress_bar_lines_count = 2; + // ====== Playback Progress ====== + // [0.000000000] Duration 0.00/0.00 [R] std::ostringstream oss; - oss << "\x1B[" << num_lines + 1 << "F"; - return CursorMoveStringEscapeRegex(oss.str()); + // Move cursor up by a specific number of lines using "Cursor Up" '\033[A' escape sequence + oss << "\033\\[" << num_lines + progress_bar_lines_count << "A"; + return oss.str(); } std::vector status_list_ = { @@ -50,19 +55,6 @@ class TestPlayerProgressBar : public Test PlayerProgressBar::PlayerStatus::BURST, PlayerProgressBar::PlayerStatus::STOPPED }; - -private: - std::string CursorMoveStringEscapeRegex(std::string str) - { - std::ostringstream oss; - for (const char c : str) { - if (c == '[' || c == ']') { - oss << "\\"; - } - oss << c; - } - return oss.str(); - } }; TEST_F(TestPlayerProgressBar, default_ctor_dtor) { From 1d4391827ec5e36c00cbd2ca55c65df14406d117 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Wed, 5 Feb 2025 19:13:34 -0800 Subject: [PATCH 16/33] Remove padding before leading seconds in timestamp value - Replace `[ 0.000000000] Duration 0.00/0.00 [R]` with `[0.000000000] Duration 0.00/0.00 [R]`. Rationale: To simplify unit tests. In real life we will never have timestamps with less than 10000 seconds. Signed-off-by: Michael Orlov --- .../src/rosbag2_transport/player_progress_bar_impl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 b9169c83e..ab0d1ad88 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp @@ -138,7 +138,7 @@ 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_ << + "[" << 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. From 3631125cfd937ff4e716a88865643c0029947f7b Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Wed, 5 Feb 2025 20:43:13 -0800 Subject: [PATCH 17/33] Remove redundant test cases and extra scope parenthesis Signed-off-by: Michael Orlov --- .../test_player_progress_bar.cpp | 176 +++++------------- 1 file changed, 47 insertions(+), 129 deletions(-) diff --git a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp index 4db64aae8..fe1b19264 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp @@ -75,45 +75,37 @@ TEST_F(TestPlayerProgressBar, can_dtor_after_output) { TEST_F(TestPlayerProgressBar, print_help_str_with_zero_update_rate) { std::ostringstream oss; - { - auto progress_bar = std::make_unique(oss, 0, 0, 0); - progress_bar->print_help_str(); - EXPECT_EQ(oss.str(), "Progress bar disabled.\n"); - } + auto progress_bar = std::make_unique(oss, 0, 0, 0); + progress_bar->print_help_str(); + EXPECT_EQ(oss.str(), "Progress bar disabled.\n"); } TEST_F(TestPlayerProgressBar, print_help_str_with_negative_update_rate) { - { - std::ostringstream oss; - auto progress_bar = std::make_unique(oss, 0, 0, -1); - progress_bar->print_help_str(); - EXPECT_EQ(oss.str(), - "Progress bar enabled for every message." - "\nProgress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped\n"); - } + std::ostringstream oss; + auto progress_bar = std::make_unique(oss, 0, 0, -1); + progress_bar->print_help_str(); + EXPECT_EQ(oss.str(), + "Progress bar enabled for every message." + "\nProgress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped\n"); } TEST_F(TestPlayerProgressBar, print_help_str_with_positive_update_rate) { - { - std::ostringstream oss; - auto progress_bar = std::make_unique(oss, 0, 0, 1); - progress_bar->print_help_str(); - EXPECT_EQ(oss.str(), - "Progress bar enabled at 1 Hz." - "\nProgress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped\n"); - } + std::ostringstream oss; + auto progress_bar = std::make_unique(oss, 0, 0, 1); + progress_bar->print_help_str(); + EXPECT_EQ(oss.str(), + "Progress bar enabled at 1 Hz." + "\nProgress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped\n"); } TEST_F(TestPlayerProgressBar, update_status_with_disabled_progress_bar) { - { - std::ostringstream oss; - auto progress_bar = std::make_unique(oss, 0, 0, 0); - for (const auto & status : status_list_) { - progress_bar->update(status); - EXPECT_THAT(oss.str(), IsEmpty()); - oss.clear(); - oss.str(""); - } + std::ostringstream oss; + auto progress_bar = std::make_unique(oss, 0, 0, 0); + for (const auto & status : status_list_) { + progress_bar->update(status); + EXPECT_THAT(oss.str(), IsEmpty()); + oss.clear(); + oss.str(""); } } @@ -131,26 +123,6 @@ TEST_F(TestPlayerProgressBar, update_status_with_enabled_progress_bar) { "\n.*\\[S\\].*")); } - { - // Test status update with update rate 1 Hz - std::ostringstream oss; - auto progress_bar = std::make_unique(oss, 0, 0, 1); - progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); - progress_bar->update(PlayerProgressBar::PlayerStatus::STOPPED); - EXPECT_THAT(oss.str(), MatchesRegex( - ".*\\[R\\].*\n.*\\[S\\].*")); - } - - { - // Test status update with update rate 10 Hz - std::ostringstream oss; - auto progress_bar = std::make_unique(oss, 0, 0, 10); - progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); - progress_bar->update(PlayerProgressBar::PlayerStatus::STOPPED); - EXPECT_THAT(oss.str(), MatchesRegex( - ".*\\[R\\].*\n.*\\[S\\].*")); - } - { // Test status update on all statuses std::ostringstream oss; @@ -186,27 +158,6 @@ TEST_F(TestPlayerProgressBar, update_status_with_separation_lines) { ".*\\[R\\].*" + expected_post_line_separator)); } - { - // Test status update with 5 separation lines - std::ostringstream oss; - uint32_t num_separation_lines = 5; - std::string expected_pre_line_separator = - GenerateClearAndMoveCursorDownRegexString(num_separation_lines); - std::string expected_post_line_separator = - GenerateMoveCursorUpRegexString(num_separation_lines); - auto progress_bar = std::make_unique(oss, 0, 0, 1, num_separation_lines); - progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); - progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); - progress_bar->update(PlayerProgressBar::PlayerStatus::PAUSED); - EXPECT_THAT(oss.str(), MatchesRegex( - expected_pre_line_separator + - ".*\\[D\\].*" + expected_post_line_separator + - expected_pre_line_separator + - ".*\\[R\\].*" + expected_post_line_separator + - expected_pre_line_separator + - ".*\\[P\\].*" + expected_post_line_separator)); - } - { // Test status update with 5 separation lines and external log messages std::ostringstream oss; @@ -307,72 +258,39 @@ TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_negative_update_rate rcutils_time_point_value_t starting_time = 1e18; // nanoseconds rcutils_time_point_value_t ending_time = starting_time + 5e9; int32_t update_rate = -1; - { - // Test if progress bar is updated every time - std::ostringstream oss; - auto progress_bar = std::make_unique( - oss, starting_time, ending_time, update_rate, 0); - rcutils_time_point_value_t timestamp_1 = starting_time; - rcutils_time_point_value_t timestamp_2 = starting_time + 1e6; - rcutils_time_point_value_t timestamp_3 = starting_time + 2e6; - progress_bar->update_with_limited_rate(timestamp_1, PlayerProgressBar::PlayerStatus::RUNNING); - progress_bar->update_with_limited_rate(timestamp_2, PlayerProgressBar::PlayerStatus::STOPPED); - progress_bar->update_with_limited_rate(timestamp_3, PlayerProgressBar::PlayerStatus::RUNNING); - EXPECT_THAT(oss.str(), MatchesRegex( - ".*\\[1000000000\\.000000000\\] Duration 0\\.00/5\\.00" - ".*\\[1000000000\\.0009.*\\] Duration 0\\.00/5\\.00" - ".*\\[1000000000\\.0019.*\\] Duration 0\\.00/5\\.00.*")); - } - { - // Test if progress bar is updated every time - std::ostringstream oss; - auto progress_bar = std::make_unique( - oss, starting_time, ending_time, update_rate, 0); - rcutils_time_point_value_t timestamp_1 = starting_time; - rcutils_time_point_value_t timestamp_2 = starting_time + 1e9; - rcutils_time_point_value_t timestamp_3 = starting_time + 2e9; - progress_bar->update_with_limited_rate(timestamp_1, PlayerProgressBar::PlayerStatus::RUNNING); - progress_bar->update_with_limited_rate(timestamp_2, PlayerProgressBar::PlayerStatus::STOPPED); - progress_bar->update_with_limited_rate(timestamp_3, PlayerProgressBar::PlayerStatus::RUNNING); - EXPECT_THAT(oss.str(), MatchesRegex( - ".*\\[1000000000\\.000000000\\] Duration 0\\.00/5\\.00" - ".*\\[1000000001\\.000000000\\] Duration 1\\.00/5\\.00" - ".*\\[1000000002\\.000000000\\] Duration 2\\.00/5\\.00.*")); - } + // Test if progress bar is updated every time + std::ostringstream oss; + auto progress_bar = std::make_unique( + oss, starting_time, ending_time, update_rate, 0); + rcutils_time_point_value_t timestamp_1 = starting_time; + rcutils_time_point_value_t timestamp_2 = starting_time + 1e6; + rcutils_time_point_value_t timestamp_3 = starting_time + 2e6; + progress_bar->update_with_limited_rate(timestamp_1, PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update_with_limited_rate(timestamp_2, PlayerProgressBar::PlayerStatus::STOPPED); + progress_bar->update_with_limited_rate(timestamp_3, PlayerProgressBar::PlayerStatus::RUNNING); + EXPECT_THAT(oss.str(), MatchesRegex( + ".*\\[1000000000\\.000000000\\] Duration 0\\.00/5\\.00" + ".*\\[1000000000\\.0009.*\\] Duration 0\\.00/5\\.00" + ".*\\[1000000000\\.0019.*\\] Duration 0\\.00/5\\.00.*")); } TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_update_rate) { rcutils_time_point_value_t starting_time = 1e18; // nanoseconds rcutils_time_point_value_t ending_time = starting_time + 5e9; int32_t update_rate = 0; - { - // Test if progress bar is not updated - std::ostringstream oss; - auto progress_bar = std::make_unique( - oss, starting_time, ending_time, update_rate); - rcutils_time_point_value_t timestamp_1 = starting_time; - rcutils_time_point_value_t timestamp_2 = starting_time + 1e6; - rcutils_time_point_value_t timestamp_3 = starting_time + 2e6; - progress_bar->update_with_limited_rate(timestamp_1, PlayerProgressBar::PlayerStatus::DELAYED); - progress_bar->update_with_limited_rate(timestamp_2, PlayerProgressBar::PlayerStatus::RUNNING); - progress_bar->update_with_limited_rate(timestamp_3, PlayerProgressBar::PlayerStatus::RUNNING); - EXPECT_THAT(oss.str(), IsEmpty()); - } - { - // Test if progress bar is not updated - std::ostringstream oss; - auto progress_bar = std::make_unique( - oss, starting_time, ending_time, update_rate); - rcutils_time_point_value_t timestamp_1 = starting_time; - rcutils_time_point_value_t timestamp_2 = starting_time + 1e9; - rcutils_time_point_value_t timestamp_3 = starting_time + 2e9; - progress_bar->update_with_limited_rate(timestamp_1, PlayerProgressBar::PlayerStatus::DELAYED); - progress_bar->update_with_limited_rate(timestamp_2, PlayerProgressBar::PlayerStatus::RUNNING); - progress_bar->update_with_limited_rate(timestamp_3, PlayerProgressBar::PlayerStatus::RUNNING); - EXPECT_THAT(oss.str(), IsEmpty()); - } + // Test if progress bar is not updated + std::ostringstream oss; + auto progress_bar = std::make_unique( + oss, starting_time, ending_time, update_rate); + rcutils_time_point_value_t timestamp_1 = starting_time; + rcutils_time_point_value_t timestamp_2 = starting_time + 1e6; + rcutils_time_point_value_t timestamp_3 = starting_time + 2e6; + progress_bar->update_with_limited_rate(timestamp_1, PlayerProgressBar::PlayerStatus::DELAYED); + progress_bar->update_with_limited_rate(timestamp_2, PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update_with_limited_rate(timestamp_3, PlayerProgressBar::PlayerStatus::RUNNING); + EXPECT_THAT(oss.str(), IsEmpty()); } TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_timestamp) { From 95e454551310ce5d0cd640a0bbd1de1a7c143687 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Wed, 5 Feb 2025 20:58:59 -0800 Subject: [PATCH 18/33] Implement "update_with_limited_rate_with_zero_timestamp" test Signed-off-by: Michael Orlov --- .../test_player_progress_bar.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp index fe1b19264..7695a2eab 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp @@ -294,5 +294,18 @@ TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_update_rate) { } TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_timestamp) { - // TODO(someone): Add content for this test + rcutils_time_point_value_t starting_time = RCUTILS_S_TO_NS(1); + rcutils_time_point_value_t ending_time = starting_time + RCUTILS_S_TO_NS(5); + std::ostringstream oss; + auto progress_bar = + std::make_unique(oss, starting_time, ending_time, 1, 0); + + progress_bar->update_with_limited_rate(0, PlayerProgressBar::PlayerStatus::RUNNING); + + EXPECT_THAT(oss.str(), + MatchesRegex( + ".*====== Playback Progress ======\n" + "\\[0\\.000000000\\] Duration -1\\.00/5\\.00.*" + ) + ); } From 74db33a3846a73a6a3f58ff7961a950db0ff7ce5 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Wed, 5 Feb 2025 21:11:28 -0800 Subject: [PATCH 19/33] Sweep cleanup in test_player_progress_bar.cpp - Rewrite tests to adhere ROS 2 style and standards Signed-off-by: Michael Orlov --- .../test_player_progress_bar.cpp | 177 ++++++++++-------- 1 file changed, 95 insertions(+), 82 deletions(-) diff --git a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp index 7695a2eab..df3151d1a 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp @@ -67,10 +67,16 @@ TEST_F(TestPlayerProgressBar, default_ctor_dtor) { TEST_F(TestPlayerProgressBar, can_dtor_after_output) { std::ostringstream oss; { - auto progress_bar = std::make_unique(oss, 0, 0); - progress_bar->print_help_str(); + auto progress_bar = std::make_unique( + oss, RCUTILS_S_TO_NS(1000000000), RCUTILS_S_TO_NS(2000000000), 1, 0); progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); } + EXPECT_THAT(oss.str(), + MatchesRegex( + "====== Playback Progress ======\n" + "\\[1000000000.000000000\\] Duration 0\\.00/1000000000\\.00 \\[D\\].*" + ) + ); } TEST_F(TestPlayerProgressBar, print_help_str_with_zero_update_rate) { @@ -111,16 +117,19 @@ TEST_F(TestPlayerProgressBar, update_status_with_disabled_progress_bar) { TEST_F(TestPlayerProgressBar, update_status_with_enabled_progress_bar) { { - // Test Playback Progress header + // Test Playback Progress header and status update with update rate 1 Hz std::ostringstream oss; auto progress_bar = std::make_unique(oss, 0, 0, 1); progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); progress_bar->update(PlayerProgressBar::PlayerStatus::STOPPED); - EXPECT_THAT(oss.str(), MatchesRegex( - ".*====== Playback Progress ======" - "\n.*\\[R\\]" - ".*====== Playback Progress ======" - "\n.*\\[S\\].*")); + EXPECT_THAT(oss.str(), + MatchesRegex( + ".*====== Playback Progress ======\n" + "\\[0\\.000000000\\] Duration 0\\.00/0\\.00 \\[R\\].*" + ".*====== Playback Progress ======\n" + "\\[0\\.000000000\\] Duration 0\\.00/0\\.00 \\[S\\].*" + ) + ); } { @@ -146,16 +155,15 @@ TEST_F(TestPlayerProgressBar, update_status_with_separation_lines) { uint32_t num_separation_lines = 2; std::string expected_pre_line_separator = GenerateClearAndMoveCursorDownRegexString(num_separation_lines); - std::string expected_post_line_separator = - GenerateMoveCursorUpRegexString(num_separation_lines); + std::string cursor_up_regex_string = GenerateMoveCursorUpRegexString(num_separation_lines); auto progress_bar = std::make_unique(oss, 0, 0, 1, num_separation_lines); progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); EXPECT_THAT(oss.str(), MatchesRegex( expected_pre_line_separator + - ".*\\[D\\].*" + expected_post_line_separator + + ".*\\[D\\].*" + cursor_up_regex_string + expected_pre_line_separator + - ".*\\[R\\].*" + expected_post_line_separator)); + ".*\\[R\\].*" + cursor_up_regex_string)); } { @@ -164,8 +172,7 @@ TEST_F(TestPlayerProgressBar, update_status_with_separation_lines) { uint32_t num_separation_lines = 5; std::string expected_pre_line_separator = GenerateClearAndMoveCursorDownRegexString(num_separation_lines); - std::string expected_post_line_separator = - GenerateMoveCursorUpRegexString(num_separation_lines); + std::string cursor_up_regex_string = GenerateMoveCursorUpRegexString(num_separation_lines); auto progress_bar = std::make_unique(oss, 0, 0, 1, num_separation_lines); progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); oss << "log msg 1\n"; @@ -178,94 +185,100 @@ TEST_F(TestPlayerProgressBar, update_status_with_separation_lines) { progress_bar->update(PlayerProgressBar::PlayerStatus::PAUSED); EXPECT_THAT(oss.str(), MatchesRegex( expected_pre_line_separator + - ".*\\[D\\].*" + expected_post_line_separator + + ".*\\[D\\].*" + cursor_up_regex_string + "log msg 1\nlog msg 2\nlog msg 3\nlog msg 4\n" + expected_pre_line_separator + - ".*\\[R\\].*" + expected_post_line_separator + + ".*\\[R\\].*" + cursor_up_regex_string + "log msg 5\nlog msg 6\n" + expected_pre_line_separator + - ".*\\[P\\].*" + expected_post_line_separator)); + ".*\\[P\\].*" + cursor_up_regex_string)); } } TEST_F(TestPlayerProgressBar, update_status_without_separation_lines) { std::ostringstream oss; - { - // Test status update without separation lines - uint32_t num_separation_lines = 0; - std::string expected_pre_line_separator = - GenerateClearAndMoveCursorDownRegexString(num_separation_lines); - std::string expected_post_line_separator = - GenerateMoveCursorUpRegexString(num_separation_lines); - auto progress_bar = std::make_unique(oss, 0, 0, 1, num_separation_lines); - progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); - EXPECT_THAT(oss.str(), MatchesRegex( - expected_pre_line_separator + - ".*\\[D\\].*" + expected_post_line_separator)); + // Test status update without separation lines + const uint32_t num_separation_lines = 0; + std::string cursor_up_regex_string = GenerateMoveCursorUpRegexString(num_separation_lines); + auto progress_bar = std::make_unique(oss, 0, 0, 1, num_separation_lines); + progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); + EXPECT_THAT(oss.str(), MatchesRegex( + ".*\\[D\\].*" + cursor_up_regex_string)); - progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); - EXPECT_THAT(oss.str(), MatchesRegex( - expected_pre_line_separator + - ".*\\[D\\].*" + expected_post_line_separator + - expected_pre_line_separator + - ".*\\[R\\].*" + expected_post_line_separator)); - } + progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); + EXPECT_THAT(oss.str(), MatchesRegex( + ".*\\[D\\].*" + cursor_up_regex_string + + ".*\\[R\\].*" + cursor_up_regex_string)); } TEST_F(TestPlayerProgressBar, update_with_limited_rate_respect_update_rate) { - rcutils_time_point_value_t starting_time = 1e18; // nanoseconds - rcutils_time_point_value_t ending_time = starting_time + 5e9; - { - // Test if update rate of the progress bar is respected - std::ostringstream oss; - int32_t update_rate = 3; - uint64_t update_period_ns = 1e9 / update_rate; - auto progress_bar = std::make_unique( - oss, starting_time, ending_time, update_rate, 0); - std::vector timestamps; - timestamps.push_back(starting_time); // 1st update progress bar - // add timestamps relative to 1st update progress bar - timestamps.push_back(timestamps.at(0) + 0.3 * update_period_ns); // skip update - timestamps.push_back(timestamps.at(0) + 0.5 * update_period_ns); // skip update - timestamps.push_back(timestamps.at(0) + 1.3 * update_period_ns); // 2nd update - // add timestamps relative to 2nd update progress bar - timestamps.push_back(timestamps.at(3) + 0.3 * update_period_ns); // skip update - timestamps.push_back(timestamps.at(3) + 1.3 * update_period_ns); // 3rd update + rcutils_time_point_value_t starting_time = RCUTILS_S_TO_NS(1); + rcutils_time_point_value_t ending_time = starting_time + RCUTILS_S_TO_NS(5); + + // Test if update rate of the progress bar is respected + std::ostringstream oss; + const size_t update_rate = 4; + const rcutils_duration_value_t update_period_ns = RCUTILS_S_TO_NS(1) / update_rate; + auto progress_bar = + std::make_unique(oss, starting_time, ending_time, update_rate, 0); + + std::vector timestamps; + timestamps.push_back(starting_time); // 1st update of the progress bar + + // Add timestamps relative to the 1st update of the progress bar + timestamps.push_back( + starting_time + static_cast(0.3 * update_period_ns)); // skip update + timestamps.push_back( + starting_time + static_cast(0.5 * update_period_ns)); // skip update + + rcutils_time_point_value_t second_update_timestamp = starting_time + + static_cast(1.3 * update_period_ns); // more than one period + + timestamps.push_back(second_update_timestamp); // 2nd update + + // Add timestamps relative to the 2nd update of the progress bar + timestamps.push_back(second_update_timestamp + + static_cast(0.3 * update_period_ns)); // skip update + + timestamps.push_back(second_update_timestamp + + static_cast(0.5 * update_period_ns)); // skip update + + timestamps.push_back(second_update_timestamp + + static_cast(1.5 * update_period_ns)); // 3rd update + + progress_bar->update_with_limited_rate(timestamps[0], PlayerProgressBar::PlayerStatus::RUNNING); + for (size_t i = 1; i < timestamps.size(); i++) { + std::this_thread::sleep_for(std::chrono::nanoseconds(timestamps[i] - timestamps[i - 1])); progress_bar->update_with_limited_rate( - timestamps[0], PlayerProgressBar::PlayerStatus::RUNNING); - for (size_t i = 1; i < timestamps.size(); ++i) { - { - std::this_thread::sleep_for( - std::chrono::nanoseconds(timestamps[i] - timestamps[i - 1])); - } - progress_bar->update_with_limited_rate( - timestamps[i], PlayerProgressBar::PlayerStatus::RUNNING); - } - // Check if the progress bar is updated at the correct 3 timestamps - EXPECT_THAT(oss.str(), MatchesRegex( - ".*\\[1000000000\\.000000000\\] Duration 0\\.00/5\\.00" - ".*\\[1000000000\\.4333.*\\] Duration 0\\.43/5\\.00" - ".*\\[1000000000\\.8666.*\\] Duration 0\\.87/5\\.00.*")); + timestamps[i], PlayerProgressBar::PlayerStatus::RUNNING); } -} -TEST_F(TestPlayerProgressBar, update_with_limited_rate_update_progress) { - // TODO(someone): Add content for this test + // Check if the progress bar is updated at the correct 3 timestamps + EXPECT_THAT(oss.str(), + MatchesRegex( + ".*====== Playback Progress ======\n" + "\\[1\\.000000000\\] Duration 0\\.00/5\\.00" + ".*====== Playback Progress ======\n" + "\\[1\\.325000000\\] Duration 0\\.32/5\\.00" + ".*====== Playback Progress ======\n" + "\\[1\\.700000000\\] Duration 0\\.70/5\\.00.*" + ) + ); } TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_negative_update_rate) { - rcutils_time_point_value_t starting_time = 1e18; // nanoseconds - rcutils_time_point_value_t ending_time = starting_time + 5e9; - int32_t update_rate = -1; + rcutils_time_point_value_t starting_time = RCUTILS_S_TO_NS(1000000000); + rcutils_time_point_value_t ending_time = starting_time + RCUTILS_S_TO_NS(5); + const int32_t update_rate = -1; // Test if progress bar is updated every time std::ostringstream oss; - auto progress_bar = std::make_unique( - oss, starting_time, ending_time, update_rate, 0); + auto progress_bar = + std::make_unique(oss, starting_time, ending_time, update_rate, 0); rcutils_time_point_value_t timestamp_1 = starting_time; - rcutils_time_point_value_t timestamp_2 = starting_time + 1e6; - rcutils_time_point_value_t timestamp_3 = starting_time + 2e6; + rcutils_time_point_value_t timestamp_2 = starting_time + RCUTILS_MS_TO_NS(1); + rcutils_time_point_value_t timestamp_3 = starting_time + RCUTILS_MS_TO_NS(2); progress_bar->update_with_limited_rate(timestamp_1, PlayerProgressBar::PlayerStatus::RUNNING); progress_bar->update_with_limited_rate(timestamp_2, PlayerProgressBar::PlayerStatus::STOPPED); progress_bar->update_with_limited_rate(timestamp_3, PlayerProgressBar::PlayerStatus::RUNNING); @@ -276,8 +289,8 @@ TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_negative_update_rate } TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_update_rate) { - rcutils_time_point_value_t starting_time = 1e18; // nanoseconds - rcutils_time_point_value_t ending_time = starting_time + 5e9; + rcutils_time_point_value_t starting_time = RCUTILS_S_TO_NS(1000000000); + rcutils_time_point_value_t ending_time = starting_time + RCUTILS_S_TO_NS(5); int32_t update_rate = 0; // Test if progress bar is not updated @@ -285,8 +298,8 @@ TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_update_rate) { auto progress_bar = std::make_unique( oss, starting_time, ending_time, update_rate); rcutils_time_point_value_t timestamp_1 = starting_time; - rcutils_time_point_value_t timestamp_2 = starting_time + 1e6; - rcutils_time_point_value_t timestamp_3 = starting_time + 2e6; + rcutils_time_point_value_t timestamp_2 = starting_time + RCUTILS_MS_TO_NS(1); + rcutils_time_point_value_t timestamp_3 = starting_time + RCUTILS_MS_TO_NS(2); progress_bar->update_with_limited_rate(timestamp_1, PlayerProgressBar::PlayerStatus::DELAYED); progress_bar->update_with_limited_rate(timestamp_2, PlayerProgressBar::PlayerStatus::RUNNING); progress_bar->update_with_limited_rate(timestamp_3, PlayerProgressBar::PlayerStatus::RUNNING); From a6d02d21b6233d90916d8f57e898da9fe870f796 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Thu, 6 Feb 2025 16:49:36 -0800 Subject: [PATCH 20/33] Use get_message_order_timestamp(..) with update_with_limited_rate(..) Signed-off-by: Michael Orlov --- rosbag2_transport/src/rosbag2_transport/player.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 80bd764c4..2eb840536 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -1099,13 +1099,8 @@ void PlayerImpl::play_messages_from_queue() } // Updating progress bar in this code section protected // by the mutex skip_message_in_main_play_loop_mutex_. - if (play_options_.message_order == MessageOrder::RECEIVED_TIMESTAMP) { - progress_bar_->update_with_limited_rate( - message_ptr->recv_timestamp, PlayerStatus::RUNNING); - } else if (play_options_.message_order == MessageOrder::SENT_TIMESTAMP) { - progress_bar_->update_with_limited_rate( - message_ptr->send_timestamp, PlayerStatus::RUNNING); - } + progress_bar_->update_with_limited_rate( + get_message_order_timestamp(message_ptr), PlayerStatus::RUNNING); } message_ptr = take_next_message_from_queue(); } From 4d3180c1028cd78282a6d9aeeb185e4d96979f50 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Tue, 18 Feb 2025 20:03:20 -0800 Subject: [PATCH 21/33] Disable progress bar by default on rosbag2_transport layer - Also remove printing `Progress bar disabled.` if it is disabled. - Rationale: To not overcomplicate output from other unit tests. - Notice: The progress bar still enabled by default for `ros2 bag play` CLI command. Signed-off-by: Michael Orlov --- .../include/rosbag2_transport/play_options.hpp | 7 +++++-- .../include/rosbag2_transport/player_progress_bar.hpp | 2 +- .../src/rosbag2_transport/player_progress_bar_impl.hpp | 2 -- .../test/rosbag2_transport/test_player_progress_bar.cpp | 2 +- 4 files changed, 7 insertions(+), 6 deletions(-) diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp index 71e68c90d..d925ba040 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -138,8 +138,11 @@ struct PlayOptions // read_ahead_queue_size value to buffer (and order) more messages. MessageOrder message_order = MessageOrder::RECEIVED_TIMESTAMP; - // Progress bar update rate in times per second (Hz) - int32_t progress_bar_update_rate = 3; + /// 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. + int32_t progress_bar_update_rate = 0; // Number of separation lines to print in between the playback output and the progress bar. uint32_t progress_bar_separation_lines = 3; diff --git a/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp b/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp index 224094d32..55a62eacf 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 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, + int32_t progress_bar_update_rate = 0, uint32_t progress_bar_separation_lines = 3); virtual ~PlayerProgressBar(); 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 ab0d1ad88..279293291 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp @@ -88,8 +88,6 @@ class PlayerProgressBarImpl { ss << "Progress bar enabled for every message.\n"; } ss << "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped\n"; - } else { - ss << "Progress bar disabled.\n"; } o_stream_ << ss.rdbuf() << std::flush; } diff --git a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp index df3151d1a..c81b84a05 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp @@ -83,7 +83,7 @@ TEST_F(TestPlayerProgressBar, print_help_str_with_zero_update_rate) { std::ostringstream oss; auto progress_bar = std::make_unique(oss, 0, 0, 0); progress_bar->print_help_str(); - EXPECT_EQ(oss.str(), "Progress bar disabled.\n"); + EXPECT_EQ(oss.str(), ""); } TEST_F(TestPlayerProgressBar, print_help_str_with_negative_update_rate) { From 9554b72bd19a847b7018c959f71c8b6edcc683df Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Tue, 18 Feb 2025 20:31:51 -0800 Subject: [PATCH 22/33] Add line cleanup ANSI codes before printing progress bar info - Rationale: To clean up possible leftovers from previous output in cases when separation lines was not enough to clean space for progress bar, over when playback start over in the loop after the end. - Also got rid of trailing spaces after printing status. Signed-off-by: Michael Orlov --- .../player_progress_bar_impl.hpp | 9 ++++----- .../test_player_progress_bar.cpp | 18 +++++++++--------- 2 files changed, 13 insertions(+), 14 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 279293291..0542dee53 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp @@ -134,13 +134,12 @@ class PlayerProgressBarImpl { ss << // Clear and print newlines progress_bar_helper_clear_and_move_cursor_down_ << - // Print progress bar - "====== Playback Progress ======\n" << + // Print progress bar + // Use "\033[2K" ANSI control code to clear an entire line in the terminal before output. + "\033[2K====== Playback Progress ======\n\033[2K" << "[" << std::fixed << std::setprecision(9) << progress_current_time_secs_ << "] Duration " << std::setprecision(2) << progress_secs_from_start_ << - // Spaces at the end are used to clear any previous progress bar in case the new one is - // shorter, which can happen when the playback starts a new loop. - "/" << duration_secs_ << " [" << static_cast(status) << "] \n" << + "/" << duration_secs_ << " [" << static_cast(status) << "]\n" << // Go up to the beginning of the blank lines progress_bar_helper_move_cursor_up_; o_stream_ << ss.rdbuf() << std::flush; diff --git a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp index c81b84a05..bf760aaa2 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp @@ -73,8 +73,8 @@ TEST_F(TestPlayerProgressBar, can_dtor_after_output) { } EXPECT_THAT(oss.str(), MatchesRegex( - "====== Playback Progress ======\n" - "\\[1000000000.000000000\\] Duration 0\\.00/1000000000\\.00 \\[D\\].*" + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + "\\[1000000000.000000000\\] Duration 0\\.00/1000000000\\.00 \\[D\\]\n.*" ) ); } @@ -124,10 +124,10 @@ TEST_F(TestPlayerProgressBar, update_status_with_enabled_progress_bar) { progress_bar->update(PlayerProgressBar::PlayerStatus::STOPPED); EXPECT_THAT(oss.str(), MatchesRegex( - ".*====== Playback Progress ======\n" + ".*\033\\[2K====== Playback Progress ======\n\033\\[2K" "\\[0\\.000000000\\] Duration 0\\.00/0\\.00 \\[R\\].*" - ".*====== Playback Progress ======\n" - "\\[0\\.000000000\\] Duration 0\\.00/0\\.00 \\[S\\].*" + ".*\033\\[2K====== Playback Progress ======\n\033\\[2K" + "\\[0\\.000000000\\] Duration 0\\.00/0\\.00 \\[S\\]\n.*" ) ); } @@ -257,11 +257,11 @@ TEST_F(TestPlayerProgressBar, update_with_limited_rate_respect_update_rate) { // Check if the progress bar is updated at the correct 3 timestamps EXPECT_THAT(oss.str(), MatchesRegex( - ".*====== Playback Progress ======\n" + ".*\033\\[2K====== Playback Progress ======\n\033\\[2K" "\\[1\\.000000000\\] Duration 0\\.00/5\\.00" - ".*====== Playback Progress ======\n" + ".*\033\\[2K====== Playback Progress ======\n\033\\[2K" "\\[1\\.325000000\\] Duration 0\\.32/5\\.00" - ".*====== Playback Progress ======\n" + ".*\033\\[2K====== Playback Progress ======\n\033\\[2K" "\\[1\\.700000000\\] Duration 0\\.70/5\\.00.*" ) ); @@ -317,7 +317,7 @@ TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_timestamp) { EXPECT_THAT(oss.str(), MatchesRegex( - ".*====== Playback Progress ======\n" + ".*\033\\[2K====== Playback Progress ======\n\033\\[2K" "\\[0\\.000000000\\] Duration -1\\.00/5\\.00.*" ) ); From 9a0f4f8f62d94016fccabd5cfd8bdae328e5d047 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Tue, 18 Feb 2025 22:58:38 -0800 Subject: [PATCH 23/33] Adjust API to properly handle progress bar update with play_next() call Signed-off-by: Michael Orlov --- .../rosbag2_transport/player_progress_bar.hpp | 14 ++++++++++---- .../src/rosbag2_transport/player.cpp | 18 ++++++++++++++++-- .../rosbag2_transport/player_progress_bar.cpp | 16 ++++++++++++---- .../player_progress_bar_impl.hpp | 19 +++++++++++++------ .../test_player_progress_bar.cpp | 18 +++++++++--------- 5 files changed, 60 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 55a62eacf..003fafab8 100644 --- a/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp @@ -67,11 +67,11 @@ class ROSBAG2_TRANSPORT_PUBLIC PlayerProgressBar /// 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. + /// \param timestamp Timestamp of the last published message. void update_with_limited_rate( - const rcutils_time_point_value_t & timestamp, - const PlayerStatus & status); + const PlayerStatus & status, + const rcutils_time_point_value_t & timestamp); /// \brief Updates progress bar with the specified player status, irrespective to the update rate /// set by the user. @@ -79,7 +79,13 @@ class ROSBAG2_TRANSPORT_PUBLIC PlayerProgressBar /// 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); + /// \param timestamp Timestamp of the last published message. If timestamp is less than 0 the + /// last published timestamp will be taken from the inner progress bar cache. + void update(const PlayerStatus & status, const rcutils_time_point_value_t & timestamp = -1); + + /// \brief Getter function for current player status. + /// \return Current player status. + PlayerStatus get_player_status(); private: std::unique_ptr pimpl_; diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 2eb840536..d45fb0ce4 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -1099,8 +1099,22 @@ void PlayerImpl::play_messages_from_queue() } // Updating progress bar in this code section protected // by the mutex skip_message_in_main_play_loop_mutex_. - progress_bar_->update_with_limited_rate( - get_message_order_timestamp(message_ptr), PlayerStatus::RUNNING); + const auto current_player_status = progress_bar_->get_player_status(); + switch (current_player_status) { + case PlayerStatus::PAUSED: + // Update progress bar without delays for each explicit play_next() call + progress_bar_->update(PlayerStatus::PAUSED, get_message_order_timestamp(message_ptr)); + break; + case PlayerStatus::BURST: + // Limit progress bar update in burst mode + progress_bar_->update_with_limited_rate( + PlayerStatus::BURST, get_message_order_timestamp(message_ptr)); + break; + default: + progress_bar_->update_with_limited_rate( + PlayerStatus::RUNNING, get_message_order_timestamp(message_ptr)); + break; + } } message_ptr = take_next_message_from_queue(); } diff --git a/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp index d583720d9..5d9fd68c4 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp @@ -40,14 +40,22 @@ void PlayerProgressBar::print_help_str() const } void PlayerProgressBar::update_with_limited_rate( - const rcutils_time_point_value_t & timestamp, const PlayerStatus & status) + const PlayerStatus & status, + const rcutils_time_point_value_t & timestamp) { - pimpl_->update_with_limited_rate(timestamp, status); + pimpl_->update_with_limited_rate(status, timestamp); } -void PlayerProgressBar::update(const PlayerStatus & status) +void PlayerProgressBar::update( + const PlayerStatus & status, + const rcutils_time_point_value_t & timestamp) { - pimpl_->update(status); + pimpl_->update(status, timestamp); +} + +PlayerProgressBar::PlayerStatus PlayerProgressBar::get_player_status() +{ + return pimpl_->current_player_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 index 0542dee53..8ab526157 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp @@ -93,9 +93,10 @@ class PlayerProgressBarImpl { } void update_with_limited_rate( - const rcutils_time_point_value_t & timestamp, - const PlayerStatus & status) + const PlayerStatus & status, + const rcutils_time_point_value_t & timestamp) { + current_player_status_ = status; if (!enable_progress_bar_) { return; } @@ -112,20 +113,24 @@ class PlayerProgressBarImpl { progress_bar_last_time_updated_ = steady_time_now; } - draw_progress_bar(timestamp, status); + draw_progress_bar(status, timestamp); } - void update(const PlayerStatus & status) + void update(const PlayerStatus & status, const rcutils_time_point_value_t & timestamp = -1) { + current_player_status_ = status; if (!enable_progress_bar_) { return; } // Update progress bar irrespective of the update rate set by the user. - draw_progress_bar(-1, status); + draw_progress_bar(status, timestamp); } - void draw_progress_bar(const rcutils_time_point_value_t & timestamp, const PlayerStatus & status) + void draw_progress_bar( + const PlayerStatus & status, + const rcutils_time_point_value_t & timestamp = -1) { + current_player_status_ = 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_; @@ -145,6 +150,8 @@ class PlayerProgressBarImpl { o_stream_ << ss.rdbuf() << std::flush; } + PlayerStatus current_player_status_{PlayerStatus::STOPPED}; + private: std::ostream & o_stream_; double starting_time_secs_ = 0.0; diff --git a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp index bf760aaa2..a604fe5db 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp @@ -246,12 +246,12 @@ TEST_F(TestPlayerProgressBar, update_with_limited_rate_respect_update_rate) { timestamps.push_back(second_update_timestamp + static_cast(1.5 * update_period_ns)); // 3rd update - progress_bar->update_with_limited_rate(timestamps[0], PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update_with_limited_rate(PlayerProgressBar::PlayerStatus::RUNNING, timestamps[0]); for (size_t i = 1; i < timestamps.size(); i++) { std::this_thread::sleep_for(std::chrono::nanoseconds(timestamps[i] - timestamps[i - 1])); progress_bar->update_with_limited_rate( - timestamps[i], PlayerProgressBar::PlayerStatus::RUNNING); + PlayerProgressBar::PlayerStatus::RUNNING, timestamps[i]); } // Check if the progress bar is updated at the correct 3 timestamps @@ -279,9 +279,9 @@ TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_negative_update_rate rcutils_time_point_value_t timestamp_1 = starting_time; rcutils_time_point_value_t timestamp_2 = starting_time + RCUTILS_MS_TO_NS(1); rcutils_time_point_value_t timestamp_3 = starting_time + RCUTILS_MS_TO_NS(2); - progress_bar->update_with_limited_rate(timestamp_1, PlayerProgressBar::PlayerStatus::RUNNING); - progress_bar->update_with_limited_rate(timestamp_2, PlayerProgressBar::PlayerStatus::STOPPED); - progress_bar->update_with_limited_rate(timestamp_3, PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update_with_limited_rate(PlayerProgressBar::PlayerStatus::RUNNING, timestamp_1); + progress_bar->update_with_limited_rate(PlayerProgressBar::PlayerStatus::STOPPED, timestamp_2); + progress_bar->update_with_limited_rate(PlayerProgressBar::PlayerStatus::RUNNING, timestamp_3); EXPECT_THAT(oss.str(), MatchesRegex( ".*\\[1000000000\\.000000000\\] Duration 0\\.00/5\\.00" ".*\\[1000000000\\.0009.*\\] Duration 0\\.00/5\\.00" @@ -300,9 +300,9 @@ TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_update_rate) { rcutils_time_point_value_t timestamp_1 = starting_time; rcutils_time_point_value_t timestamp_2 = starting_time + RCUTILS_MS_TO_NS(1); rcutils_time_point_value_t timestamp_3 = starting_time + RCUTILS_MS_TO_NS(2); - progress_bar->update_with_limited_rate(timestamp_1, PlayerProgressBar::PlayerStatus::DELAYED); - progress_bar->update_with_limited_rate(timestamp_2, PlayerProgressBar::PlayerStatus::RUNNING); - progress_bar->update_with_limited_rate(timestamp_3, PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update_with_limited_rate(PlayerProgressBar::PlayerStatus::DELAYED, timestamp_1); + progress_bar->update_with_limited_rate(PlayerProgressBar::PlayerStatus::RUNNING, timestamp_2); + progress_bar->update_with_limited_rate(PlayerProgressBar::PlayerStatus::RUNNING, timestamp_3); EXPECT_THAT(oss.str(), IsEmpty()); } @@ -313,7 +313,7 @@ TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_timestamp) { auto progress_bar = std::make_unique(oss, starting_time, ending_time, 1, 0); - progress_bar->update_with_limited_rate(0, PlayerProgressBar::PlayerStatus::RUNNING); + progress_bar->update_with_limited_rate(PlayerProgressBar::PlayerStatus::RUNNING, 0); EXPECT_THAT(oss.str(), MatchesRegex( From 5e3cabe29c0cb0d2151ab4acec14fbd9fd0ab762 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Wed, 19 Feb 2025 10:38:03 -0800 Subject: [PATCH 24/33] Use std::atomic for current_player_status_ Signed-off-by: Michael Orlov --- .../src/rosbag2_transport/player_progress_bar.cpp | 2 +- .../src/rosbag2_transport/player_progress_bar_impl.hpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp index 5d9fd68c4..3364077fa 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp @@ -55,7 +55,7 @@ void PlayerProgressBar::update( PlayerProgressBar::PlayerStatus PlayerProgressBar::get_player_status() { - return pimpl_->current_player_status_; + return pimpl_->current_player_status_.load(); } } // 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 index 8ab526157..d45b44c02 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp @@ -16,6 +16,7 @@ #define ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_IMPL_HPP_ #include +#include #include #include #include @@ -96,7 +97,7 @@ class PlayerProgressBarImpl { const PlayerStatus & status, const rcutils_time_point_value_t & timestamp) { - current_player_status_ = status; + current_player_status_.store(status); if (!enable_progress_bar_) { return; } @@ -118,7 +119,7 @@ class PlayerProgressBarImpl { void update(const PlayerStatus & status, const rcutils_time_point_value_t & timestamp = -1) { - current_player_status_ = status; + current_player_status_.store(status); if (!enable_progress_bar_) { return; } @@ -130,7 +131,6 @@ class PlayerProgressBarImpl { const PlayerStatus & status, const rcutils_time_point_value_t & timestamp = -1) { - current_player_status_ = 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_; @@ -150,7 +150,7 @@ class PlayerProgressBarImpl { o_stream_ << ss.rdbuf() << std::flush; } - PlayerStatus current_player_status_{PlayerStatus::STOPPED}; + std::atomic current_player_status_{PlayerStatus::STOPPED}; private: std::ostream & o_stream_; From 47c1cf0ea41db11c32302d78040e8ec7d883d7d1 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Wed, 19 Feb 2025 10:52:25 -0800 Subject: [PATCH 25/33] Remove line clear ANSI code from separation lines - Rationale: By clearing separation lines we may occasionally erase important log messages. The separation lines is intended to be as a "buffer" in case if logging happened during drawing the progress bar. In most cases the separation lines will be empty by default but when not we want to preserve those logging information. Signed-off-by: Michael Orlov --- .../rosbag2_transport/player_progress_bar_impl.hpp | 13 ++++++------- .../rosbag2_transport/test_player_progress_bar.cpp | 11 +++++------ 2 files changed, 11 insertions(+), 13 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 d45b44c02..9277e9557 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp @@ -53,15 +53,14 @@ class PlayerProgressBarImpl { std::max(static_cast(ending_time - starting_time), 0.0)); progress_current_time_secs_ = starting_time_secs_; progress_secs_from_start_ = progress_current_time_secs_ - starting_time_secs_; - std::ostringstream oss_clear_and_move_cursor_down; + std::ostringstream oss_move_cursor_down; for (uint32_t i = 0; i < progress_bar_separation_lines_; i++) { - // The ANSI control code "\033[2K" is used to clear an entire line in the terminal. - // Cleanup current line and jump down to one new line with "\n" - oss_clear_and_move_cursor_down << "\033[2K\n"; + // Jump down to one new line with "\n" + oss_move_cursor_down << "\n"; } - progress_bar_helper_clear_and_move_cursor_down_ = oss_clear_and_move_cursor_down.str(); + progress_bar_helper_move_cursor_down_ = oss_move_cursor_down.str(); std::ostringstream oss_move_cursor_up; // Move cursor up by a specific number of lines using "Cursor Up" '\033[A' ANSI control code oss_move_cursor_up << "\033[" << @@ -138,7 +137,7 @@ class PlayerProgressBarImpl { std::stringstream ss; ss << // Clear and print newlines - progress_bar_helper_clear_and_move_cursor_down_ << + progress_bar_helper_move_cursor_down_ << // Print progress bar // Use "\033[2K" ANSI control code to clear an entire line in the terminal before output. "\033[2K====== Playback Progress ======\n\033[2K" << @@ -156,7 +155,7 @@ class PlayerProgressBarImpl { std::ostream & o_stream_; double starting_time_secs_ = 0.0; double duration_secs_ = 0.0; - std::string progress_bar_helper_clear_and_move_cursor_down_; + std::string progress_bar_helper_move_cursor_down_; std::string progress_bar_helper_move_cursor_up_; bool enable_progress_bar_; uint16_t progress_bar_update_period_ms_; diff --git a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp index a604fe5db..13b9782e2 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp @@ -26,13 +26,12 @@ using namespace rosbag2_transport; // NOLINT class TestPlayerProgressBar : public Test { public: - std::string GenerateClearAndMoveCursorDownRegexString(uint32_t num_lines) + std::string GenerateMoveCursorDownRegexString(uint32_t num_lines) { std::ostringstream oss; for (uint32_t i = 0; i < num_lines; i++) { - // The ANSI control code "\033[2K" is used to clear an entire line in the terminal. - // Cleanup current line and jump down to one new line with "\n" - oss << "\033\\[2K\n"; + // Jump down to one new line with "\n" + oss << "\n"; } return oss.str(); } @@ -154,7 +153,7 @@ TEST_F(TestPlayerProgressBar, update_status_with_separation_lines) { std::ostringstream oss; uint32_t num_separation_lines = 2; std::string expected_pre_line_separator = - GenerateClearAndMoveCursorDownRegexString(num_separation_lines); + GenerateMoveCursorDownRegexString(num_separation_lines); std::string cursor_up_regex_string = GenerateMoveCursorUpRegexString(num_separation_lines); auto progress_bar = std::make_unique(oss, 0, 0, 1, num_separation_lines); progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); @@ -171,7 +170,7 @@ TEST_F(TestPlayerProgressBar, update_status_with_separation_lines) { std::ostringstream oss; uint32_t num_separation_lines = 5; std::string expected_pre_line_separator = - GenerateClearAndMoveCursorDownRegexString(num_separation_lines); + GenerateMoveCursorDownRegexString(num_separation_lines); std::string cursor_up_regex_string = GenerateMoveCursorUpRegexString(num_separation_lines); auto progress_bar = std::make_unique(oss, 0, 0, 1, num_separation_lines); progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); From 858da22f4779ab1244a6e9c94b1cc0ce9e3dc677 Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Fri, 21 Feb 2025 23:10:48 +0000 Subject: [PATCH 26/33] update README and docs Signed-off-by: Nicola Loi --- README.md | 4 +++- docs/design/rosbag2_record_replay_service.md | 10 ++++++++-- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 16499038f..cfc842c74 100644 --- a/README.md +++ b/README.md @@ -136,7 +136,7 @@ $ ros2 bag play ``` The bag argument can be a directory containing `metadata.yaml` and one or more storage files, or to a single storage file such as `.mcap` or `.db3`. -The Player will automatically detect which storage implementation to use for playing. +The Player will automatically detect which storage implementation to use for playing. A progress bar to track the playback progress will be displayed in the terminal by default. To play back multiple bags: @@ -164,6 +164,8 @@ Options: The reference to use for bag message chronological ordering. Choices: reception timestamp (`received`), publication timestamp (`sent`). Default: reception timestamp. +* `--progress-bar [ProgressBarRate]` + Print a progress bar of the playback player at a specific rate in Hz. Negative values mark an update for every published message, while a zero value disables the progress bar. Default to 3 Hz. For more options, run with `--help`. diff --git a/docs/design/rosbag2_record_replay_service.md b/docs/design/rosbag2_record_replay_service.md index 981329dee..c4110a7ff 100644 --- a/docs/design/rosbag2_record_replay_service.md +++ b/docs/design/rosbag2_record_replay_service.md @@ -150,9 +150,15 @@ Added or changed parameters: Rename from `--exclude`. Exclude topics and services containing provided regular expression. -- `--progress-bar [ProgressBarFrequency]` +- `--progress-bar [ProgressBarRate]` - 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. + Print a progress bar of the playback player at a specific rate in Hz. + Negative values mark an update for every published message, while a zero value disables the progress bar. Default to 3 Hz. + +- `--progress-bar-separation-lines [ProgressBarSeparationLines]` + + Number of lines to separate the progress bar from the rest of the playback player output. + It prevents mixing external log message with the progress bar string. Default to 2. `-e REGEX, --regex REGEX` affects both topics and services. From dda2c4ea23c29bfd2c1eaabbf7ee0e4b1a46c781 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 22 Feb 2025 18:47:18 -0800 Subject: [PATCH 27/33] Revert "Remove line clear ANSI code from separation lines" Signed-off-by: Michael Orlov --- .../src/rosbag2_transport/player_progress_bar_impl.hpp | 9 +++++---- .../test/rosbag2_transport/test_player_progress_bar.cpp | 5 +++-- 2 files changed, 8 insertions(+), 6 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 9277e9557..5d08fc149 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp @@ -53,14 +53,15 @@ class PlayerProgressBarImpl { std::max(static_cast(ending_time - starting_time), 0.0)); progress_current_time_secs_ = starting_time_secs_; progress_secs_from_start_ = progress_current_time_secs_ - starting_time_secs_; - std::ostringstream oss_move_cursor_down; + std::ostringstream oss_clear_and_move_cursor_down; for (uint32_t i = 0; i < progress_bar_separation_lines_; i++) { - // Jump down to one new line with "\n" - oss_move_cursor_down << "\n"; + // The ANSI control code "\033[2K" is used to clear an entire line in the terminal. + // Cleanup current line and jump down to one new line with "\n" + oss_clear_and_move_cursor_down << "\033[2K\n"; } - progress_bar_helper_move_cursor_down_ = oss_move_cursor_down.str(); + progress_bar_helper_move_cursor_down_ = oss_clear_and_move_cursor_down.str(); std::ostringstream oss_move_cursor_up; // Move cursor up by a specific number of lines using "Cursor Up" '\033[A' ANSI control code oss_move_cursor_up << "\033[" << diff --git a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp index 13b9782e2..8adced3b9 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp @@ -30,8 +30,9 @@ class TestPlayerProgressBar : public Test { std::ostringstream oss; for (uint32_t i = 0; i < num_lines; i++) { - // Jump down to one new line with "\n" - oss << "\n"; + // The ANSI control code "\033[2K" is used to clear an entire line in the terminal. + // Cleanup current line and jump down to one new line with "\n" + oss << "\033\\[2K\n"; } return oss.str(); } From 1d18d096654ed1789d820b513394665e5a11f62d Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 22 Feb 2025 19:47:38 -0800 Subject: [PATCH 28/33] Correct wording in README.md and `ros2 bag play` options Signed-off-by: Michael Orlov --- README.md | 18 +++++++++++++----- docs/design/rosbag2_record_replay_service.md | 10 ---------- ros2bag/ros2bag/verb/play.py | 16 ++++++++-------- 3 files changed, 21 insertions(+), 23 deletions(-) diff --git a/README.md b/README.md index cfc842c74..bbbe2d8cf 100644 --- a/README.md +++ b/README.md @@ -135,8 +135,10 @@ When you have a recorded bag, you can use Rosbag2 to play it back: $ ros2 bag play ``` -The bag argument can be a directory containing `metadata.yaml` and one or more storage files, or to a single storage file such as `.mcap` or `.db3`. -The Player will automatically detect which storage implementation to use for playing. A progress bar to track the playback progress will be displayed in the terminal by default. +The bag argument can be a directory containing `metadata.yaml` and one or more storage files, or +to a single storage file such as `.mcap` or `.db3`. +The Player will automatically detect which storage implementation to use for playing. +A progress bar to track the playback progress will be displayed in the terminal by default. To play back multiple bags: @@ -144,7 +146,8 @@ To play back multiple bags: $ ros2 bag play -i -i ``` -Messages from all provided bags will be played in order, based on their original recording reception timestamps. +Messages from all provided bags will be played in order, based on their original recording +reception timestamps. Options: @@ -164,8 +167,13 @@ Options: The reference to use for bag message chronological ordering. Choices: reception timestamp (`received`), publication timestamp (`sent`). Default: reception timestamp. -* `--progress-bar [ProgressBarRate]` - Print a progress bar of the playback player at a specific rate in Hz. Negative values mark an update for every published message, while a zero value disables the progress bar. Default to 3 Hz. +* `--progress-bar [ProgressBarUpdateRate]` + Print a progress bar for the playback with a specified maximum update rate in times per second + (Hz). Negative values mark an update for every published message, while a zero value disables + the progress bar. Default is 3 Hz. +* `--progress-bar-separation-lines [ProgressBarSeparationLines]` + The number of lines to separate the progress bar from the rest of the playback player output. + It prevents mixing external log messages with the progress bar string. Default to 2. For more options, run with `--help`. diff --git a/docs/design/rosbag2_record_replay_service.md b/docs/design/rosbag2_record_replay_service.md index c4110a7ff..48ce2a524 100644 --- a/docs/design/rosbag2_record_replay_service.md +++ b/docs/design/rosbag2_record_replay_service.md @@ -150,16 +150,6 @@ Added or changed parameters: Rename from `--exclude`. Exclude topics and services containing provided regular expression. -- `--progress-bar [ProgressBarRate]` - - Print a progress bar of the playback player at a specific rate in Hz. - Negative values mark an update for every published message, while a zero value disables the progress bar. Default to 3 Hz. - -- `--progress-bar-separation-lines [ProgressBarSeparationLines]` - - Number of lines to separate the progress bar from the rest of the playback player output. - It prevents mixing external log message with the progress bar string. Default to 2. - `-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 8d3d3863e..fc263b864 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -182,22 +182,22 @@ def add_arguments(self, parser, cli_name): # noqa: D102 choices=['debug', 'info', 'warn', 'error', 'fatal'], help='Logging level.') parser.add_argument( - '--progress-bar', type=int, default=3, - help='Print a progress bar for the playback at a specific rate in times per second. ' - 'Default is %(default)d. ' + '--progress-bar', type=int, metavar='Hz', default=3, + help='Print a progress bar for the playback with a specified maximum update rate in ' + 'times per second (Hz). ' + 'Default is %(default)d Hz. ' 'Negative values mark an update for every published message, while ' - ' a zero value disables the progress bar.', - metavar='PROGRESS_BAR_UPDATE_RATE') + ' a zero value disables the progress bar.') 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 ' + metavar='NUM_SEPARATION_LINES', + help=' Then number of lines to separate the progress bar from the rest of the ' 'playback player output. Negative values are invalid. Default is %(default)d. ' 'This option makes sense only if the progress bar is enabled. ' 'Large values are useful if external log messages from other nodes are shorter ' 'than the progress bar string and are printed at a rate higher than the ' 'progress bar update rate. In these cases, a larger value will avoid ' - 'the external log message to be mixed with the progress bar string.', - metavar='NUM_SEPARATION_LINES') + 'the external log message to be mixed with the progress bar string.') def get_playback_until_from_arg_group(self, playback_until_sec, playback_until_nsec) -> int: nano_scale = 1000 * 1000 * 1000 From deed06af60d7f1bf27bf8699cce4b61173302101 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 22 Feb 2025 20:15:30 -0800 Subject: [PATCH 29/33] Rename progress-bar CLI option to the progress-bar-update-rate - Also use 'Progress bar group' to group progress bar specific CLI options. Signed-off-by: Michael Orlov --- README.md | 4 ++-- ros2bag/ros2bag/verb/play.py | 7 ++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index bbbe2d8cf..a48e5a584 100644 --- a/README.md +++ b/README.md @@ -167,11 +167,11 @@ Options: The reference to use for bag message chronological ordering. Choices: reception timestamp (`received`), publication timestamp (`sent`). Default: reception timestamp. -* `--progress-bar [ProgressBarUpdateRate]` +* `--progress-bar-update-rate [Hz]`: Print a progress bar for the playback with a specified maximum update rate in times per second (Hz). Negative values mark an update for every published message, while a zero value disables the progress bar. Default is 3 Hz. -* `--progress-bar-separation-lines [ProgressBarSeparationLines]` +* `--progress-bar-separation-lines`: The number of lines to separate the progress bar from the rest of the playback player output. It prevents mixing external log messages with the progress bar string. Default to 2. diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index fc263b864..ee915776a 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -181,14 +181,15 @@ def add_arguments(self, parser, cli_name): # noqa: D102 '--log-level', type=str, default='info', choices=['debug', 'info', 'warn', 'error', 'fatal'], help='Logging level.') - parser.add_argument( - '--progress-bar', type=int, metavar='Hz', default=3, + progress_bar_group = parser.add_argument_group('Progress bar', 'Settings for progress bar') + progress_bar_group.add_argument( + '--progress-bar-update-rate', type=int, metavar='Hz', default=3, help='Print a progress bar for the playback with a specified maximum update rate in ' 'times per second (Hz). ' 'Default is %(default)d Hz. ' 'Negative values mark an update for every published message, while ' ' a zero value disables the progress bar.') - parser.add_argument( + progress_bar_group.add_argument( '--progress-bar-separation-lines', type=check_not_negative_int, default=2, metavar='NUM_SEPARATION_LINES', help=' Then number of lines to separate the progress bar from the rest of the ' From ec5dbfdb9f9ae83d5c94ff91802df9586d978087 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 22 Feb 2025 21:06:40 -0800 Subject: [PATCH 30/33] Rename progress-bar CLI option to the progress-bar-update-rate - Also use 'Progress bar group' to group progress bar specific CLI options. Signed-off-by: Michael Orlov --- ros2bag/ros2bag/verb/play.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index ee915776a..2ef53bc38 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -310,7 +310,7 @@ 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_update_rate = args.progress_bar_update_rate play_options.progress_bar_separation_lines = args.progress_bar_separation_lines player = Player(args.log_level) From 35052150b0a7c39e935bd0694c3c5986d2f74308 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 22 Feb 2025 21:39:00 -0800 Subject: [PATCH 31/33] Add progress bar options to the get_play_options_from_node_params parser Signed-off-by: Michael Orlov --- .../rosbag2_transport/config_options_from_node_params.cpp | 7 +++++++ rosbag2_transport/test/resources/player_node_params.yaml | 2 ++ .../test/rosbag2_transport/test_composable_player.cpp | 2 ++ 3 files changed, 11 insertions(+) diff --git a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp index 3a29a418e..d67cf7704 100644 --- a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp +++ b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp @@ -252,6 +252,13 @@ PlayOptions get_play_options_from_node_params(rclcpp::Node & node) message_order.c_str()); } + play_options.progress_bar_update_rate = param_utils::declare_integer_node_params( + node, "play.progress_bar_update_rate", std::numeric_limits::min(), + std::numeric_limits::max(), 0); + + play_options.progress_bar_separation_lines = param_utils::declare_integer_node_params( + node, "play.progress_bar_separation_lines", 0, std::numeric_limits::max(), 3); + return play_options; } diff --git a/rosbag2_transport/test/resources/player_node_params.yaml b/rosbag2_transport/test/resources/player_node_params.yaml index f471d6cb9..8bc1eb698 100644 --- a/rosbag2_transport/test/resources/player_node_params.yaml +++ b/rosbag2_transport/test/resources/player_node_params.yaml @@ -40,6 +40,8 @@ player_params_node: publish_service_requests: false service_requests_source: "CLIENT_INTROSPECTION" # SERVICE_INTROSPECTION or CLIENT_INTROSPECTION message_order: "SENT_TIMESTAMP" # RECEIVED_TIMESTAMP or SENT_TIMESTAMP + progress_bar_update_rate: 2 # times per second (Hz) + progress_bar_separation_lines: 3 storage: uri: "path/to/some_bag" diff --git a/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp b/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp index c9d7fd710..07940fac3 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp @@ -178,6 +178,8 @@ TEST_P(ComposablePlayerTests, player_can_parse_parameters_from_file) { play_options.service_requests_source, rosbag2_transport::ServiceRequestsSource::CLIENT_INTROSPECTION); EXPECT_EQ(play_options.message_order, rosbag2_transport::MessageOrder::SENT_TIMESTAMP); + EXPECT_EQ(play_options.progress_bar_update_rate, 2); + EXPECT_EQ(play_options.progress_bar_separation_lines, 3); ASSERT_EQ(1, storage_options.size()); EXPECT_EQ(storage_options[0].uri, uri_str); From 6839d0674080e3ddc436701d33d770113134a24d Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 23 Feb 2025 22:56:07 -0800 Subject: [PATCH 32/33] Address test failures on Windows MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - On Windows RE2 is not available, Google Test falls back to a very simplified regex engine where `.` for “any character except newline”. Therefore, need explicitly specify every newline character in regex. Signed-off-by: Michael Orlov --- .../test_player_progress_bar.cpp | 60 +++++++++++-------- 1 file changed, 36 insertions(+), 24 deletions(-) diff --git a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp index 8adced3b9..7e36a522e 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_player_progress_bar.cpp @@ -74,7 +74,7 @@ TEST_F(TestPlayerProgressBar, can_dtor_after_output) { EXPECT_THAT(oss.str(), MatchesRegex( "\033\\[2K====== Playback Progress ======\n\033\\[2K" - "\\[1000000000.000000000\\] Duration 0\\.00/1000000000\\.00 \\[D\\]\n.*" + "\\[1000000000\\.000000000\\] Duration 0\\.00/1000000000\\.00 \\[D\\]\n.*" ) ); } @@ -119,13 +119,13 @@ TEST_F(TestPlayerProgressBar, update_status_with_enabled_progress_bar) { { // Test Playback Progress header and status update with update rate 1 Hz std::ostringstream oss; - auto progress_bar = std::make_unique(oss, 0, 0, 1); + auto progress_bar = std::make_unique(oss, 0, 0, 1, 0); progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); progress_bar->update(PlayerProgressBar::PlayerStatus::STOPPED); EXPECT_THAT(oss.str(), MatchesRegex( ".*\033\\[2K====== Playback Progress ======\n\033\\[2K" - "\\[0\\.000000000\\] Duration 0\\.00/0\\.00 \\[R\\].*" + "\\[0\\.000000000\\] Duration 0\\.00/0\\.00 \\[R\\]\n.*" ".*\033\\[2K====== Playback Progress ======\n\033\\[2K" "\\[0\\.000000000\\] Duration 0\\.00/0\\.00 \\[S\\]\n.*" ) @@ -135,13 +135,14 @@ TEST_F(TestPlayerProgressBar, update_status_with_enabled_progress_bar) { { // Test status update on all statuses std::ostringstream oss; - auto progress_bar = std::make_unique(oss, 0, 0, 1); + auto progress_bar = std::make_unique(oss, 0, 0, 1, 0); for (const auto & status : status_list_) { progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); progress_bar->update(status); std::string status_str(1, static_cast(status)); EXPECT_THAT(oss.str(), MatchesRegex( - ".*\\[R\\].*\n.*\\[" + status_str + "\\].*")); + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + ".*\\[R\\]\n.*\n.*\\[" + status_str + "\\]\n.*")); oss.clear(); oss.str(""); } @@ -161,9 +162,11 @@ TEST_F(TestPlayerProgressBar, update_status_with_separation_lines) { progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); EXPECT_THAT(oss.str(), MatchesRegex( expected_pre_line_separator + - ".*\\[D\\].*" + cursor_up_regex_string + + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + ".*\\[D\\]\n.*" + cursor_up_regex_string + expected_pre_line_separator + - ".*\\[R\\].*" + cursor_up_regex_string)); + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + ".*\\[R\\]\n.*" + cursor_up_regex_string)); } { @@ -185,13 +188,16 @@ TEST_F(TestPlayerProgressBar, update_status_with_separation_lines) { progress_bar->update(PlayerProgressBar::PlayerStatus::PAUSED); EXPECT_THAT(oss.str(), MatchesRegex( expected_pre_line_separator + - ".*\\[D\\].*" + cursor_up_regex_string + + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + ".*\\[D\\]\n.*" + cursor_up_regex_string + "log msg 1\nlog msg 2\nlog msg 3\nlog msg 4\n" + expected_pre_line_separator + - ".*\\[R\\].*" + cursor_up_regex_string + + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + ".*\\[R\\]\n.*" + cursor_up_regex_string + "log msg 5\nlog msg 6\n" + expected_pre_line_separator + - ".*\\[P\\].*" + cursor_up_regex_string)); + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + ".*\\[P\\]\n.*" + cursor_up_regex_string)); } } @@ -203,12 +209,15 @@ TEST_F(TestPlayerProgressBar, update_status_without_separation_lines) { auto progress_bar = std::make_unique(oss, 0, 0, 1, num_separation_lines); progress_bar->update(PlayerProgressBar::PlayerStatus::DELAYED); EXPECT_THAT(oss.str(), MatchesRegex( - ".*\\[D\\].*" + cursor_up_regex_string)); + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + ".*\\[D\\]\n.*" + cursor_up_regex_string)); progress_bar->update(PlayerProgressBar::PlayerStatus::RUNNING); EXPECT_THAT(oss.str(), MatchesRegex( - ".*\\[D\\].*" + cursor_up_regex_string + - ".*\\[R\\].*" + cursor_up_regex_string)); + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + ".*\\[D\\]\n.*" + cursor_up_regex_string + + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + ".*\\[R\\]\n.*" + cursor_up_regex_string)); } TEST_F(TestPlayerProgressBar, update_with_limited_rate_respect_update_rate) { @@ -257,12 +266,12 @@ TEST_F(TestPlayerProgressBar, update_with_limited_rate_respect_update_rate) { // Check if the progress bar is updated at the correct 3 timestamps EXPECT_THAT(oss.str(), MatchesRegex( - ".*\033\\[2K====== Playback Progress ======\n\033\\[2K" - "\\[1\\.000000000\\] Duration 0\\.00/5\\.00" - ".*\033\\[2K====== Playback Progress ======\n\033\\[2K" - "\\[1\\.325000000\\] Duration 0\\.32/5\\.00" - ".*\033\\[2K====== Playback Progress ======\n\033\\[2K" - "\\[1\\.700000000\\] Duration 0\\.70/5\\.00.*" + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + "\\[1\\.000000000\\] Duration 0\\.00/5\\.00 \\[R\\]\n.*" + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + "\\[1\\.325000000\\] Duration 0\\.32/5\\.00 \\[R\\]\n.*" + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + "\\[1\\.700000000\\] Duration 0\\.70/5\\.00 \\[R\\]\n.*" ) ); } @@ -283,9 +292,12 @@ TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_negative_update_rate progress_bar->update_with_limited_rate(PlayerProgressBar::PlayerStatus::STOPPED, timestamp_2); progress_bar->update_with_limited_rate(PlayerProgressBar::PlayerStatus::RUNNING, timestamp_3); EXPECT_THAT(oss.str(), MatchesRegex( - ".*\\[1000000000\\.000000000\\] Duration 0\\.00/5\\.00" - ".*\\[1000000000\\.0009.*\\] Duration 0\\.00/5\\.00" - ".*\\[1000000000\\.0019.*\\] Duration 0\\.00/5\\.00.*")); + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + "\\[1000000000\\.000000000\\] Duration 0\\.00/5\\.00 \\[R\\]\n.*" + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + "\\[1000000000\\.0009.*\\] Duration 0\\.00/5\\.00 \\[S\\]\n.*" + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + "\\[1000000000\\.0019.*\\] Duration 0\\.00/5\\.00 \\[R\\]\n.*")); } TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_update_rate) { @@ -317,8 +329,8 @@ TEST_F(TestPlayerProgressBar, update_with_limited_rate_with_zero_timestamp) { EXPECT_THAT(oss.str(), MatchesRegex( - ".*\033\\[2K====== Playback Progress ======\n\033\\[2K" - "\\[0\\.000000000\\] Duration -1\\.00/5\\.00.*" + "\033\\[2K====== Playback Progress ======\n\033\\[2K" + "\\[0\\.000000000\\] Duration -1\\.00/5\\.00 \\[R\\]\n.*" ) ); } From 30976c84a5652ae4044936da52229fea0efe2091 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 23 Feb 2025 23:35:05 -0800 Subject: [PATCH 33/33] Enable progress bar by default for composable player node Signed-off-by: Michael Orlov --- .../src/rosbag2_transport/config_options_from_node_params.cpp | 4 ++-- rosbag2_transport/test/resources/player_node_params.yaml | 4 ++-- .../test/rosbag2_transport/test_composable_player.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp index d67cf7704..388365588 100644 --- a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp +++ b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp @@ -254,10 +254,10 @@ PlayOptions get_play_options_from_node_params(rclcpp::Node & node) play_options.progress_bar_update_rate = param_utils::declare_integer_node_params( node, "play.progress_bar_update_rate", std::numeric_limits::min(), - std::numeric_limits::max(), 0); + std::numeric_limits::max(), 3); play_options.progress_bar_separation_lines = param_utils::declare_integer_node_params( - node, "play.progress_bar_separation_lines", 0, std::numeric_limits::max(), 3); + node, "play.progress_bar_separation_lines", 0, std::numeric_limits::max(), 2); return play_options; } diff --git a/rosbag2_transport/test/resources/player_node_params.yaml b/rosbag2_transport/test/resources/player_node_params.yaml index 8bc1eb698..78886a23f 100644 --- a/rosbag2_transport/test/resources/player_node_params.yaml +++ b/rosbag2_transport/test/resources/player_node_params.yaml @@ -40,8 +40,8 @@ player_params_node: publish_service_requests: false service_requests_source: "CLIENT_INTROSPECTION" # SERVICE_INTROSPECTION or CLIENT_INTROSPECTION message_order: "SENT_TIMESTAMP" # RECEIVED_TIMESTAMP or SENT_TIMESTAMP - progress_bar_update_rate: 2 # times per second (Hz) - progress_bar_separation_lines: 3 + progress_bar_update_rate: -1 # times per second (Hz) + progress_bar_separation_lines: 0 storage: uri: "path/to/some_bag" diff --git a/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp b/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp index 07940fac3..83676cb6b 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp @@ -178,8 +178,8 @@ TEST_P(ComposablePlayerTests, player_can_parse_parameters_from_file) { play_options.service_requests_source, rosbag2_transport::ServiceRequestsSource::CLIENT_INTROSPECTION); EXPECT_EQ(play_options.message_order, rosbag2_transport::MessageOrder::SENT_TIMESTAMP); - EXPECT_EQ(play_options.progress_bar_update_rate, 2); - EXPECT_EQ(play_options.progress_bar_separation_lines, 3); + EXPECT_EQ(play_options.progress_bar_update_rate, -1); + EXPECT_EQ(play_options.progress_bar_separation_lines, 0); ASSERT_EQ(1, storage_options.size()); EXPECT_EQ(storage_options[0].uri, uri_str);