From 616df0d3d93f8ea777c668cdc7a34342419f7be3 Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Tue, 15 Oct 2024 22:34:34 +0200 Subject: [PATCH 01/33] Start adding progress-bar for ros2 bag play Signed-off-by: Nicola Loi --- ros2bag/ros2bag/verb/play.py | 4 + rosbag2_py/rosbag2_py/_transport.pyi | 1 + rosbag2_py/src/rosbag2_py/_transport.cpp | 2 + .../rosbag2_transport/play_options.hpp | 3 + .../rosbag2_transport/player_progress_bar.hpp | 90 +++++++++++++++++++ .../src/rosbag2_transport/player.cpp | 26 ++++++ 6 files changed, 126 insertions(+) create mode 100644 rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index 363d43d59..e033654e8 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -168,6 +168,9 @@ 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', action='store_true', default=False, + help='Print a progress bar of the playback player.') def get_playback_until_from_arg_group(self, playback_until_sec, playback_until_nsec) -> int: nano_scale = 1000 * 1000 * 1000 @@ -243,6 +246,7 @@ def main(self, *, args): # noqa: D102 play_options.service_requests_source = ServiceRequestsSource.SERVICE_INTROSPECTION else: play_options.service_requests_source = ServiceRequestsSource.CLIENT_INTROSPECTION + play_options.disable_progress_bar = not args.progress_bar player = Player(args.log_level) try: diff --git a/rosbag2_py/rosbag2_py/_transport.pyi b/rosbag2_py/rosbag2_py/_transport.pyi index fdd8f0780..b6b66f288 100644 --- a/rosbag2_py/rosbag2_py/_transport.pyi +++ b/rosbag2_py/rosbag2_py/_transport.pyi @@ -10,6 +10,7 @@ class PlayOptions: delay: float disable_keyboard_controls: bool disable_loan_message: bool + disable_progress_bar: bool exclude_regex_to_filter: str exclude_service_events_to_filter: List[str] exclude_topics_to_filter: List[str] diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 9d15e82b1..9b2883b06 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -544,6 +544,8 @@ PYBIND11_MODULE(_transport, m) { "playback_until_timestamp", &PlayOptions::getPlaybackUntilTimestamp, &PlayOptions::setPlaybackUntilTimestamp) + .def_readwrite( + "disable_progress_bar", &PlayOptions::disable_progress_bar) .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 e5029c923..b20f6c909 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -123,6 +123,9 @@ struct PlayOptions // The source of the service request ServiceRequestsSource service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; + + // Disable to print a progress bar of the playback player + bool disable_progress_bar = true; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp b/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp new file mode 100644 index 000000000..15fb1f557 --- /dev/null +++ b/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp @@ -0,0 +1,90 @@ +// Copyright 2024 Nicola Loi. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_HPP_ +#define ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_HPP_ + +#include +#include +#include + +#include "rcl/types.h" +#include "rclcpp/rclcpp.hpp" + +namespace rosbag2_transport +{ + +class PlayerProgressBar final +{ +public: + explicit PlayerProgressBar( + bool disable_progress_bar, + rcutils_time_point_value_t bag_starting_time, + rcutils_duration_value_t bag_duration) + : disable_progress_bar_(disable_progress_bar), + bag_starting_time_secs_(RCUTILS_NS_TO_S(static_cast(bag_starting_time))), + bag_duration_secs_(RCUTILS_NS_TO_S(static_cast(bag_duration))) + { + } + ~PlayerProgressBar() + { + if (!disable_progress_bar_) { + printf("\n"); + fflush(stdout); + } + } + inline void print_running_status(const rcutils_time_point_value_t current_time) const + { + if (!disable_progress_bar_) { + print_status(current_time, 'R'); + } + } + inline void print_paused_status(const rcutils_time_point_value_t current_time) const + { + if (!disable_progress_bar_) { + print_status(current_time, 'P'); + } + } + inline void print_delayed_status(const rcutils_time_point_value_t current_time) const + { + if (!disable_progress_bar_) { + print_status(current_time, 'D'); + } + } + std::string get_help_str() const + { + if (!disable_progress_bar_) { + return "Bag Time and Duration [?]: ? = R running, P paused, D delayed"; + } else { + return "Bag Time and Duration progress bar disabled."; + } + } + +private: + inline void print_status(const rcutils_time_point_value_t current_time, const char status) const + { + double current_time_secs = RCUTILS_NS_TO_S(static_cast(current_time)); + double bag_progress_secs = current_time_secs - bag_starting_time_secs_; + printf(" Bag Time %13.6f Duration %.6f/%.6f [%c] \r", + current_time_secs, bag_progress_secs, bag_duration_secs_, status); + fflush(stdout); + } + bool disable_progress_bar_; + const double bag_starting_time_secs_; + const double bag_duration_secs_; +}; + +} // 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 74a911f1d..e26d27174 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -41,6 +41,7 @@ #include "rosbag2_transport/config_options_from_node_params.hpp" #include "rosbag2_transport/player_service_client.hpp" #include "rosbag2_transport/reader_writer_factory.hpp" +#include "rosbag2_transport/player_progress_bar.hpp" #include "logging.hpp" @@ -349,6 +350,9 @@ class PlayerImpl std::vector keyboard_callbacks_; std::shared_ptr player_service_client_manager_; + + // progress bar + std::unique_ptr progress_bar_; }; PlayerImpl::PlayerImpl( @@ -393,6 +397,7 @@ PlayerImpl::PlayerImpl( // keep reader open until player is destroyed reader_->open(storage_options_, {"", rmw_get_serialization_format()}); auto metadata = reader_->get_metadata(); + rcutils_duration_value_t bag_duration = metadata.duration.count(); starting_time_ = std::chrono::duration_cast( metadata.starting_time.time_since_epoch()).count(); // If a non-default (positive) starting time offset is provided in PlayOptions, @@ -405,10 +410,13 @@ PlayerImpl::PlayerImpl( ". Negative start offset ignored."); } else { starting_time_ += play_options_.start_offset; + bag_duration -= play_options_.start_offset; } clock_ = std::make_unique( starting_time_, std::chrono::steady_clock::now, std::chrono::milliseconds{100}, play_options_.start_paused); + progress_bar_ = std::make_unique(play_options_.disable_progress_bar, + starting_time_, bag_duration); set_rate(play_options_.rate); topic_qos_profile_overrides_ = play_options_.topic_qos_profile_overrides; prepare_publishers(); @@ -416,6 +424,7 @@ PlayerImpl::PlayerImpl( } create_control_services(); add_keyboard_callbacks(); + RCLCPP_INFO_STREAM(owner_->get_logger(), progress_bar_->get_help_str()); } PlayerImpl::~PlayerImpl() @@ -486,6 +495,7 @@ bool PlayerImpl::play() do { if (delay > rclcpp::Duration(0, 0)) { RCLCPP_INFO_STREAM(owner_->get_logger(), "Sleep " << delay.nanoseconds() << " ns"); + progress_bar_->print_delayed_status(clock_->now()); std::chrono::nanoseconds delay_duration(delay.nanoseconds()); std::this_thread::sleep_for(delay_duration); } @@ -602,18 +612,25 @@ void PlayerImpl::stop() playback_thread_.join(); } } + if (clock_->is_paused()) { + progress_bar_->print_paused_status(clock_->now()); + } else { + progress_bar_->print_running_status(clock_->now()); + } } void PlayerImpl::pause() { clock_->pause(); RCLCPP_INFO_STREAM(owner_->get_logger(), "Pausing play."); + progress_bar_->print_paused_status(clock_->now()); } void PlayerImpl::resume() { clock_->resume(); RCLCPP_INFO_STREAM(owner_->get_logger(), "Resuming play."); + progress_bar_->print_running_status(clock_->now()); } void PlayerImpl::toggle_paused() @@ -640,6 +657,11 @@ bool PlayerImpl::set_rate(double rate) } else { RCLCPP_WARN_STREAM(owner_->get_logger(), "Failed to set rate to invalid value " << rate); } + if (clock_->is_paused()) { + progress_bar_->print_paused_status(clock_->now()); + } else { + progress_bar_->print_running_status(clock_->now()); + } return ok; } @@ -707,6 +729,7 @@ bool PlayerImpl::play_next() next_message_published = publish_message(message_ptr); clock_->jump(message_ptr->recv_timestamp); + progress_bar_->print_paused_status(clock_->now()); message_queue_.pop(); message_ptr = peek_next_message_from_queue(); } @@ -717,6 +740,7 @@ size_t PlayerImpl::burst(const size_t num_messages) { if (!clock_->is_paused()) { RCLCPP_WARN_STREAM(owner_->get_logger(), "Burst can only be used when in the paused state."); + progress_bar_->print_running_status(clock_->now()); return 0; } @@ -731,6 +755,7 @@ size_t PlayerImpl::burst(const size_t num_messages) } RCLCPP_INFO_STREAM(owner_->get_logger(), "Burst " << messages_played << " messages."); + progress_bar_->print_running_status(clock_->now()); return messages_played; } @@ -956,6 +981,7 @@ void PlayerImpl::play_messages_from_queue() continue; } publish_message(message_ptr); + progress_bar_->print_running_status(clock_->now()); } message_queue_.pop(); message_ptr = peek_next_message_from_queue(); From 2cb732d2b54942130b85c00b90fe717f949ec5dd Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Wed, 16 Oct 2024 00:21:08 +0200 Subject: [PATCH 02/33] clean #include Signed-off-by: Nicola Loi --- .../include/rosbag2_transport/player_progress_bar.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp b/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp index 15fb1f557..3f8717ba9 100644 --- a/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp @@ -15,8 +15,6 @@ #ifndef ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_HPP_ #define ROSBAG2_TRANSPORT__PLAYER_PROGRESS_BAR_HPP_ -#include -#include #include #include "rcl/types.h" From 4ba1a8c0a9bfb02eac79320a9b05820cb27654d6 Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Mon, 21 Oct 2024 00:49:08 +0200 Subject: [PATCH 03/33] remove new progress bar class, integrate functionality in PlayerImpl Signed-off-by: Nicola Loi --- .../rosbag2_transport/player_progress_bar.hpp | 88 ------------------- .../src/rosbag2_transport/player.cpp | 82 +++++++++++++---- 2 files changed, 65 insertions(+), 105 deletions(-) delete mode 100644 rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp diff --git a/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp b/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp deleted file mode 100644 index 3f8717ba9..000000000 --- a/rosbag2_transport/include/rosbag2_transport/player_progress_bar.hpp +++ /dev/null @@ -1,88 +0,0 @@ -// Copyright 2024 Nicola Loi. -// -// 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 "rcl/types.h" -#include "rclcpp/rclcpp.hpp" - -namespace rosbag2_transport -{ - -class PlayerProgressBar final -{ -public: - explicit PlayerProgressBar( - bool disable_progress_bar, - rcutils_time_point_value_t bag_starting_time, - rcutils_duration_value_t bag_duration) - : disable_progress_bar_(disable_progress_bar), - bag_starting_time_secs_(RCUTILS_NS_TO_S(static_cast(bag_starting_time))), - bag_duration_secs_(RCUTILS_NS_TO_S(static_cast(bag_duration))) - { - } - ~PlayerProgressBar() - { - if (!disable_progress_bar_) { - printf("\n"); - fflush(stdout); - } - } - inline void print_running_status(const rcutils_time_point_value_t current_time) const - { - if (!disable_progress_bar_) { - print_status(current_time, 'R'); - } - } - inline void print_paused_status(const rcutils_time_point_value_t current_time) const - { - if (!disable_progress_bar_) { - print_status(current_time, 'P'); - } - } - inline void print_delayed_status(const rcutils_time_point_value_t current_time) const - { - if (!disable_progress_bar_) { - print_status(current_time, 'D'); - } - } - std::string get_help_str() const - { - if (!disable_progress_bar_) { - return "Bag Time and Duration [?]: ? = R running, P paused, D delayed"; - } else { - return "Bag Time and Duration progress bar disabled."; - } - } - -private: - inline void print_status(const rcutils_time_point_value_t current_time, const char status) const - { - double current_time_secs = RCUTILS_NS_TO_S(static_cast(current_time)); - double bag_progress_secs = current_time_secs - bag_starting_time_secs_; - printf(" Bag Time %13.6f Duration %.6f/%.6f [%c] \r", - current_time_secs, bag_progress_secs, bag_duration_secs_, status); - fflush(stdout); - } - bool disable_progress_bar_; - const double bag_starting_time_secs_; - const double bag_duration_secs_; -}; - -} // 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 e26d27174..3d255757a 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -41,7 +41,6 @@ #include "rosbag2_transport/config_options_from_node_params.hpp" #include "rosbag2_transport/player_service_client.hpp" #include "rosbag2_transport/reader_writer_factory.hpp" -#include "rosbag2_transport/player_progress_bar.hpp" #include "logging.hpp" @@ -299,6 +298,11 @@ class PlayerImpl void configure_play_until_timestamp(); bool shall_stop_at_timestamp(const rcutils_time_point_value_t & msg_timestamp) const; + void print_status(const char status) const; + void print_running_status() const; + void print_paused_status() const; + void print_delayed_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}; @@ -329,6 +333,8 @@ class PlayerImpl std::condition_variable playback_finished_cv_; rcutils_time_point_value_t starting_time_; + double starting_time_secs_; + double duration_secs_; // control services rclcpp::Service::SharedPtr srv_pause_; @@ -352,7 +358,7 @@ class PlayerImpl std::shared_ptr player_service_client_manager_; // progress bar - std::unique_ptr progress_bar_; + const bool disable_progress_bar_; }; PlayerImpl::PlayerImpl( @@ -365,7 +371,8 @@ PlayerImpl::PlayerImpl( storage_options_(storage_options), play_options_(play_options), keyboard_handler_(std::move(keyboard_handler)), - player_service_client_manager_(std::make_shared()) + player_service_client_manager_(std::make_shared()), + disable_progress_bar_(play_options.disable_progress_bar) { for (auto & topic : play_options_.topics_to_filter) { topic = rclcpp::expand_topic_or_service_name( @@ -412,11 +419,11 @@ PlayerImpl::PlayerImpl( starting_time_ += play_options_.start_offset; bag_duration -= play_options_.start_offset; } + starting_time_secs_ = RCUTILS_NS_TO_S(static_cast(starting_time_)); + duration_secs_ = RCUTILS_NS_TO_S(static_cast(bag_duration)); clock_ = std::make_unique( starting_time_, std::chrono::steady_clock::now, std::chrono::milliseconds{100}, play_options_.start_paused); - progress_bar_ = std::make_unique(play_options_.disable_progress_bar, - starting_time_, bag_duration); set_rate(play_options_.rate); topic_qos_profile_overrides_ = play_options_.topic_qos_profile_overrides; prepare_publishers(); @@ -424,7 +431,13 @@ PlayerImpl::PlayerImpl( } create_control_services(); add_keyboard_callbacks(); - RCLCPP_INFO_STREAM(owner_->get_logger(), progress_bar_->get_help_str()); + std::string progress_bar_help_str; + if (!disable_progress_bar_) { + progress_bar_help_str = "Bag Time and Duration [?]: ? = R running, P paused, D delayed"; + } else { + progress_bar_help_str = "Bag Time and Duration progress bar disabled."; + } + RCLCPP_INFO_STREAM(owner_->get_logger(), progress_bar_help_str); } PlayerImpl::~PlayerImpl() @@ -445,6 +458,11 @@ PlayerImpl::~PlayerImpl() if (reader_) { reader_->close(); } + // print new line to keep on screen the latest progress bar + if (!disable_progress_bar_) { + printf("\n"); + fflush(stdout); + } } const std::chrono::milliseconds @@ -495,7 +513,7 @@ bool PlayerImpl::play() do { if (delay > rclcpp::Duration(0, 0)) { RCLCPP_INFO_STREAM(owner_->get_logger(), "Sleep " << delay.nanoseconds() << " ns"); - progress_bar_->print_delayed_status(clock_->now()); + print_delayed_status(); std::chrono::nanoseconds delay_duration(delay.nanoseconds()); std::this_thread::sleep_for(delay_duration); } @@ -613,9 +631,9 @@ void PlayerImpl::stop() } } if (clock_->is_paused()) { - progress_bar_->print_paused_status(clock_->now()); + print_paused_status(); } else { - progress_bar_->print_running_status(clock_->now()); + print_running_status(); } } @@ -623,14 +641,14 @@ void PlayerImpl::pause() { clock_->pause(); RCLCPP_INFO_STREAM(owner_->get_logger(), "Pausing play."); - progress_bar_->print_paused_status(clock_->now()); + print_paused_status(); } void PlayerImpl::resume() { clock_->resume(); RCLCPP_INFO_STREAM(owner_->get_logger(), "Resuming play."); - progress_bar_->print_running_status(clock_->now()); + print_running_status(); } void PlayerImpl::toggle_paused() @@ -658,9 +676,9 @@ bool PlayerImpl::set_rate(double rate) RCLCPP_WARN_STREAM(owner_->get_logger(), "Failed to set rate to invalid value " << rate); } if (clock_->is_paused()) { - progress_bar_->print_paused_status(clock_->now()); + print_paused_status(); } else { - progress_bar_->print_running_status(clock_->now()); + print_running_status(); } return ok; } @@ -729,7 +747,7 @@ bool PlayerImpl::play_next() next_message_published = publish_message(message_ptr); clock_->jump(message_ptr->recv_timestamp); - progress_bar_->print_paused_status(clock_->now()); + print_paused_status(); message_queue_.pop(); message_ptr = peek_next_message_from_queue(); } @@ -740,7 +758,7 @@ size_t PlayerImpl::burst(const size_t num_messages) { if (!clock_->is_paused()) { RCLCPP_WARN_STREAM(owner_->get_logger(), "Burst can only be used when in the paused state."); - progress_bar_->print_running_status(clock_->now()); + print_running_status(); return 0; } @@ -755,7 +773,7 @@ size_t PlayerImpl::burst(const size_t num_messages) } RCLCPP_INFO_STREAM(owner_->get_logger(), "Burst " << messages_played << " messages."); - progress_bar_->print_running_status(clock_->now()); + print_running_status(); return messages_played; } @@ -981,7 +999,7 @@ void PlayerImpl::play_messages_from_queue() continue; } publish_message(message_ptr); - progress_bar_->print_running_status(clock_->now()); + print_running_status(); } message_queue_.pop(); message_ptr = peek_next_message_from_queue(); @@ -1553,6 +1571,36 @@ const rosbag2_transport::PlayOptions & PlayerImpl::get_play_options() return play_options_; } +inline void PlayerImpl::print_status(const char status) const +{ + double current_time_secs = RCUTILS_NS_TO_S(static_cast(clock_->now())); + double progress_secs = current_time_secs - starting_time_secs_; + printf(" Bag Time %13.6f Duration %.6f/%.6f [%c] \r", + current_time_secs, progress_secs, duration_secs_, status); + fflush(stdout); +} + +inline void PlayerImpl::print_running_status() const +{ + if (!disable_progress_bar_) { + print_status('R'); + } +} + +inline void PlayerImpl::print_paused_status() const +{ + if (!disable_progress_bar_) { + print_status('P'); + } +} + +inline void PlayerImpl::print_delayed_status() const +{ + if (!disable_progress_bar_) { + print_status('D'); + } +} + /////////////////////////////// // Player public interface From a00bdc43eb8a3ff7bc8e54e2b2cba7516eb59425 Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Thu, 24 Oct 2024 00:06:25 +0200 Subject: [PATCH 04/33] convert to template Signed-off-by: Nicola Loi --- .../src/rosbag2_transport/player.cpp | 44 +++++++++++-------- 1 file changed, 26 insertions(+), 18 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 3d255757a..e62b448f5 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -298,7 +298,9 @@ class PlayerImpl void configure_play_until_timestamp(); bool shall_stop_at_timestamp(const rcutils_time_point_value_t & msg_timestamp) const; - void print_status(const char status) const; + template + void print_status() const; + void check_and_print_status() const; void print_running_status() const; void print_paused_status() const; void print_delayed_status() const; @@ -630,11 +632,7 @@ void PlayerImpl::stop() playback_thread_.join(); } } - if (clock_->is_paused()) { - print_paused_status(); - } else { - print_running_status(); - } + check_and_print_status(); } void PlayerImpl::pause() @@ -675,11 +673,7 @@ bool PlayerImpl::set_rate(double rate) } else { RCLCPP_WARN_STREAM(owner_->get_logger(), "Failed to set rate to invalid value " << rate); } - if (clock_->is_paused()) { - print_paused_status(); - } else { - print_running_status(); - } + check_and_print_status(); return ok; } @@ -1571,8 +1565,11 @@ const rosbag2_transport::PlayOptions & PlayerImpl::get_play_options() return play_options_; } -inline void PlayerImpl::print_status(const char status) const +template +void PlayerImpl::print_status() const { + static_assert(status == 'R' || status == 'P' || status == 'D', + "Invalid status character"); double current_time_secs = RCUTILS_NS_TO_S(static_cast(clock_->now())); double progress_secs = current_time_secs - starting_time_secs_; printf(" Bag Time %13.6f Duration %.6f/%.6f [%c] \r", @@ -1580,24 +1577,35 @@ inline void PlayerImpl::print_status(const char status) const fflush(stdout); } -inline void PlayerImpl::print_running_status() const +void PlayerImpl::check_and_print_status() const +{ + if (!disable_progress_bar_) { + if (clock_->is_paused()) { + print_status<'P'>(); + } else { + print_status<'R'>(); + } + } +} + +void PlayerImpl::print_running_status() const { if (!disable_progress_bar_) { - print_status('R'); + print_status<'R'>(); } } -inline void PlayerImpl::print_paused_status() const +void PlayerImpl::print_paused_status() const { if (!disable_progress_bar_) { - print_status('P'); + print_status<'P'>(); } } -inline void PlayerImpl::print_delayed_status() const +void PlayerImpl::print_delayed_status() const { if (!disable_progress_bar_) { - print_status('D'); + print_status<'D'>(); } } From 43fa0f89ef2d51a786953088dbdea18a2ce8b39e Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Mon, 28 Oct 2024 23:00:06 +0000 Subject: [PATCH 05/33] Code review Signed-off-by: Nicola Loi --- ros2bag/ros2bag/verb/play.py | 10 +- rosbag2_py/rosbag2_py/_transport.pyi | 2 +- rosbag2_py/src/rosbag2_py/_transport.cpp | 3 +- .../rosbag2_transport/play_options.hpp | 4 +- .../src/rosbag2_transport/player.cpp | 155 +++++++++++------- 5 files changed, 104 insertions(+), 70 deletions(-) diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index e033654e8..1047f6eb3 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -169,8 +169,12 @@ def add_arguments(self, parser, cli_name): # noqa: D102 choices=['debug', 'info', 'warn', 'error', 'fatal'], help='Logging level.') parser.add_argument( - '--progress-bar', action='store_true', default=False, - help='Print a progress bar of the playback player.') + '--progress-bar', type=float, default=10.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') def get_playback_until_from_arg_group(self, playback_until_sec, playback_until_nsec) -> int: nano_scale = 1000 * 1000 * 1000 @@ -246,7 +250,7 @@ def main(self, *, args): # noqa: D102 play_options.service_requests_source = ServiceRequestsSource.SERVICE_INTROSPECTION else: play_options.service_requests_source = ServiceRequestsSource.CLIENT_INTROSPECTION - play_options.disable_progress_bar = not args.progress_bar + play_options.progress_bar_print_frequency = args.progress_bar player = Player(args.log_level) try: diff --git a/rosbag2_py/rosbag2_py/_transport.pyi b/rosbag2_py/rosbag2_py/_transport.pyi index b6b66f288..79462dde8 100644 --- a/rosbag2_py/rosbag2_py/_transport.pyi +++ b/rosbag2_py/rosbag2_py/_transport.pyi @@ -10,7 +10,6 @@ class PlayOptions: delay: float disable_keyboard_controls: bool disable_loan_message: bool - disable_progress_bar: bool exclude_regex_to_filter: str exclude_service_events_to_filter: List[str] exclude_topics_to_filter: List[str] @@ -18,6 +17,7 @@ class PlayOptions: node_prefix: str playback_duration: float playback_until_timestamp: int + progress_bar_print_frequency: 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 9b2883b06..0e1788c7c 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -544,8 +544,7 @@ PYBIND11_MODULE(_transport, m) { "playback_until_timestamp", &PlayOptions::getPlaybackUntilTimestamp, &PlayOptions::setPlaybackUntilTimestamp) - .def_readwrite( - "disable_progress_bar", &PlayOptions::disable_progress_bar) + .def_readwrite("progress_bar_print_frequency", &PlayOptions::progress_bar_print_frequency) .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 b20f6c909..cf2ad2212 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -124,8 +124,8 @@ struct PlayOptions // The source of the service request ServiceRequestsSource service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; - // Disable to print a progress bar of the playback player - bool disable_progress_bar = true; + // Rate in Hz at which to print progress bar. + double progress_bar_print_frequency = 10.0; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index e62b448f5..e76475adb 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -80,6 +80,14 @@ rclcpp::QoS publisher_qos_for_topic( namespace rosbag2_transport { + +enum class PlayerStatus : char +{ + RUNNING = 'R', + PAUSED = 'P', + DELAYED = 'D', +}; + constexpr Player::callback_handle_t Player::invalid_callback_handle; class PlayerImpl @@ -123,12 +131,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 @@ -298,12 +307,8 @@ class PlayerImpl void configure_play_until_timestamp(); bool shall_stop_at_timestamp(const rcutils_time_point_value_t & msg_timestamp) const; - template - void print_status() const; - void check_and_print_status() const; - void print_running_status() const; - void print_paused_status() const; - void print_delayed_status() const; + void print_progress_bar_help_str(double progress_bar_print_frequency) const; + void print_progress_bar_status(const PlayerStatus & status, bool force_update = false); static constexpr double read_ahead_lower_bound_percentage_ = 0.9; static const std::chrono::milliseconds queue_read_wait_period_; @@ -360,7 +365,11 @@ class PlayerImpl std::shared_ptr player_service_client_manager_; // progress bar - const bool disable_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_printed_{}; + rcutils_time_point_value_t recv_timestamp_last_published_; }; PlayerImpl::PlayerImpl( @@ -374,7 +383,11 @@ PlayerImpl::PlayerImpl( play_options_(play_options), keyboard_handler_(std::move(keyboard_handler)), player_service_client_manager_(std::make_shared()), - disable_progress_bar_(play_options.disable_progress_bar) + enable_progress_bar_(play_options.progress_bar_print_frequency != 0), + progress_bar_update_always_(play_options.progress_bar_print_frequency < 0), + progress_bar_update_period_(play_options.progress_bar_print_frequency != 0 ? + RCUTILS_S_TO_NS(1.0 / play_options.progress_bar_print_frequency) : + std::numeric_limits::max()) { for (auto & topic : play_options_.topics_to_filter) { topic = rclcpp::expand_topic_or_service_name( @@ -423,6 +436,7 @@ PlayerImpl::PlayerImpl( } starting_time_secs_ = RCUTILS_NS_TO_S(static_cast(starting_time_)); duration_secs_ = RCUTILS_NS_TO_S(static_cast(bag_duration)); + recv_timestamp_last_published_ = starting_time_; clock_ = std::make_unique( starting_time_, std::chrono::steady_clock::now, std::chrono::milliseconds{100}, play_options_.start_paused); @@ -433,13 +447,7 @@ PlayerImpl::PlayerImpl( } create_control_services(); add_keyboard_callbacks(); - std::string progress_bar_help_str; - if (!disable_progress_bar_) { - progress_bar_help_str = "Bag Time and Duration [?]: ? = R running, P paused, D delayed"; - } else { - progress_bar_help_str = "Bag Time and Duration progress bar disabled."; - } - RCLCPP_INFO_STREAM(owner_->get_logger(), progress_bar_help_str); + print_progress_bar_help_str(play_options.progress_bar_print_frequency); } PlayerImpl::~PlayerImpl() @@ -460,10 +468,9 @@ PlayerImpl::~PlayerImpl() if (reader_) { reader_->close(); } - // print new line to keep on screen the latest progress bar - if (!disable_progress_bar_) { - printf("\n"); - fflush(stdout); + // print new line to keep on screen the latest progress bar (which had a carriage return) + if (enable_progress_bar_) { + std::cout << std::endl; } } @@ -515,9 +522,12 @@ bool PlayerImpl::play() do { if (delay > rclcpp::Duration(0, 0)) { RCLCPP_INFO_STREAM(owner_->get_logger(), "Sleep " << delay.nanoseconds() << " ns"); - print_delayed_status(); + print_progress_bar_status(PlayerStatus::DELAYED); std::chrono::nanoseconds delay_duration(delay.nanoseconds()); std::this_thread::sleep_for(delay_duration); + } else { + print_progress_bar_status(clock_->is_paused() ? + PlayerStatus::PAUSED : PlayerStatus::RUNNING, true); } { std::lock_guard lk(reader_mutex_); @@ -582,6 +592,8 @@ bool PlayerImpl::play() is_in_playback_ = false; playback_finished_cv_.notify_all(); } + print_progress_bar_status(clock_->is_paused() ? + PlayerStatus::PAUSED : PlayerStatus::RUNNING, true); }); return true; } @@ -632,21 +644,22 @@ void PlayerImpl::stop() playback_thread_.join(); } } - check_and_print_status(); + print_progress_bar_status(clock_->is_paused() ? + PlayerStatus::PAUSED : PlayerStatus::RUNNING, true); } void PlayerImpl::pause() { clock_->pause(); RCLCPP_INFO_STREAM(owner_->get_logger(), "Pausing play."); - print_paused_status(); + print_progress_bar_status(PlayerStatus::PAUSED); } void PlayerImpl::resume() { clock_->resume(); RCLCPP_INFO_STREAM(owner_->get_logger(), "Resuming play."); - print_running_status(); + print_progress_bar_status(PlayerStatus::RUNNING, true); } void PlayerImpl::toggle_paused() @@ -673,7 +686,8 @@ bool PlayerImpl::set_rate(double rate) } else { RCLCPP_WARN_STREAM(owner_->get_logger(), "Failed to set rate to invalid value " << rate); } - check_and_print_status(); + print_progress_bar_status(clock_->is_paused() ? + PlayerStatus::PAUSED : PlayerStatus::RUNNING, true); return ok; } @@ -706,10 +720,11 @@ rosbag2_storage::SerializedBagMessageSharedPtr PlayerImpl::peek_next_message_fro return nullptr; } -bool PlayerImpl::play_next() +bool PlayerImpl::play_next(bool is_burst_mode) { if (!clock_->is_paused()) { RCLCPP_WARN_STREAM(owner_->get_logger(), "Called play next, but not in paused state."); + print_progress_bar_status(PlayerStatus::RUNNING, true); return false; } @@ -722,6 +737,7 @@ bool PlayerImpl::play_next() // resume() or stop() from another thread while we were waiting on mutex. if (!clock_->is_paused()) { RCLCPP_WARN_STREAM(owner_->get_logger(), "Called play next, but not in paused state."); + print_progress_bar_status(PlayerStatus::RUNNING, true); return false; } skip_message_in_main_play_loop_ = true; @@ -741,7 +757,11 @@ bool PlayerImpl::play_next() next_message_published = publish_message(message_ptr); clock_->jump(message_ptr->recv_timestamp); - print_paused_status(); + // Update the progress bar if we were not called in burst mode to avoid delays. + if (!is_burst_mode) { + recv_timestamp_last_published_ = message_ptr->recv_timestamp; + print_progress_bar_status(PlayerStatus::PAUSED); + } message_queue_.pop(); message_ptr = peek_next_message_from_queue(); } @@ -752,14 +772,14 @@ 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."); - print_running_status(); + print_progress_bar_status(PlayerStatus::RUNNING, true); return 0; } 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; @@ -767,7 +787,8 @@ size_t PlayerImpl::burst(const size_t num_messages) } RCLCPP_INFO_STREAM(owner_->get_logger(), "Burst " << messages_played << " messages."); - print_running_status(); + print_progress_bar_status(clock_->is_paused() ? + PlayerStatus::PAUSED : PlayerStatus::RUNNING, true); return messages_played; } @@ -993,7 +1014,8 @@ void PlayerImpl::play_messages_from_queue() continue; } publish_message(message_ptr); - print_running_status(); + recv_timestamp_last_published_ = message_ptr->recv_timestamp; + print_progress_bar_status(PlayerStatus::RUNNING); } message_queue_.pop(); message_ptr = peek_next_message_from_queue(); @@ -1565,48 +1587,57 @@ const rosbag2_transport::PlayOptions & PlayerImpl::get_play_options() return play_options_; } -template -void PlayerImpl::print_status() const -{ - static_assert(status == 'R' || status == 'P' || status == 'D', - "Invalid status character"); - double current_time_secs = RCUTILS_NS_TO_S(static_cast(clock_->now())); - double progress_secs = current_time_secs - starting_time_secs_; - printf(" Bag Time %13.6f Duration %.6f/%.6f [%c] \r", - current_time_secs, progress_secs, duration_secs_, status); - fflush(stdout); -} - -void PlayerImpl::check_and_print_status() const +void PlayerImpl::print_progress_bar_help_str(double progress_bar_print_frequency) const { - if (!disable_progress_bar_) { - if (clock_->is_paused()) { - print_status<'P'>(); + std::string progress_bar_help_str; + if (enable_progress_bar_) { + if (progress_bar_update_period_ < 0) { + progress_bar_help_str = "Progress bar enabled with updates for every message."; } else { - print_status<'R'>(); + std::ostringstream oss; + oss << "Progress bar enabled with updates at " + << std::fixed << std::setprecision(1) << progress_bar_print_frequency << " Hz."; + progress_bar_help_str = oss.str(); } + progress_bar_help_str += " [?]: [R]unning, [P]aused, [D]elayed."; + } else { + progress_bar_help_str = "Progress bar disabled."; } + RCLCPP_INFO_STREAM(owner_->get_logger(), progress_bar_help_str); } -void PlayerImpl::print_running_status() const +void PlayerImpl::print_progress_bar_status(const PlayerStatus & status, bool force_update) { - if (!disable_progress_bar_) { - print_status<'R'>(); + if (!enable_progress_bar_) { + return; } -} -void PlayerImpl::print_paused_status() const -{ - if (!disable_progress_bar_) { - print_status<'P'>(); - } -} + const std::chrono::steady_clock::time_point steady_time_now = std::chrono::steady_clock::now(); -void PlayerImpl::print_delayed_status() const -{ - if (!disable_progress_bar_) { - print_status<'D'>(); + // Check if we should force an update of the progress bar, otherwise check time since last update. + // Force an update if: + // - the user-defined update period is negative; + // - or this function was called with a status other than RUNNING (since we want to update the + // progress bar when we pause or delay the playback, otherwise it will be stuck + // to the last update if not enough time has passed since it); + // - or this function was called with force_update set to true. + if (!(progress_bar_update_always_ || status != PlayerStatus::RUNNING || force_update)) { + if (std::chrono::duration_cast( + steady_time_now - progress_bar_last_time_printed_).count() < progress_bar_update_period_) + { + return; + } } + + const double current_time_secs = RCUTILS_NS_TO_S( + static_cast(recv_timestamp_last_published_)); + const double progress_secs = current_time_secs - starting_time_secs_; + // Print progress bar ending with carriage return '/r' so it will be overwritten by next print + std::cout << " Bag Time " << std::setw(13) << std::fixed << std::setprecision(6) << + current_time_secs << " Duration " << std::fixed << std::setprecision(6) << + progress_secs << "/" << duration_secs_ << " [" << static_cast(status) << + "]\r" << std::flush; + progress_bar_last_time_printed_ = steady_time_now; } /////////////////////////////// From 9cdf5adcb514036e2f131f04ce7add1e0890a9e0 Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Sun, 10 Nov 2024 22:59:27 +0100 Subject: [PATCH 06/33] update default rate Signed-off-by: Nicola Loi --- ros2bag/ros2bag/verb/play.py | 2 +- rosbag2_transport/include/rosbag2_transport/play_options.hpp | 2 +- rosbag2_transport/src/rosbag2_transport/play_options.cpp | 5 +++++ 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index 1047f6eb3..7fbcb4dcb 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -169,7 +169,7 @@ 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=10.0, + '--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 ' diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp index cf2ad2212..975792112 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -125,7 +125,7 @@ struct PlayOptions ServiceRequestsSource service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; // Rate in Hz at which to print progress bar. - double progress_bar_print_frequency = 10.0; + double progress_bar_print_frequency = 3.0; }; } // 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..c38683f51 100644 --- a/rosbag2_transport/src/rosbag2_transport/play_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/play_options.cpp @@ -59,6 +59,8 @@ Node convert::encode( node["disable_loan_message"] = play_options.disable_loan_message; + node["progress_bar_print_frequency"] = play_options.progress_bar_print_frequency; + return node; } @@ -114,6 +116,9 @@ bool convert::decode( optional_assign(node, "disable_loan_message", play_options.disable_loan_message); + optional_assign( + node, "progress_bar_print_frequency", play_options.progress_bar_print_frequency); + return true; } From bac4e9fcc79147f271ec0f57bd1e03cd4953021e Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Sun, 17 Nov 2024 21:05:43 +0100 Subject: [PATCH 07/33] update to avoid synchronization issues Signed-off-by: Nicola Loi --- docs/design/rosbag2_record_replay_service.md | 4 + .../src/rosbag2_transport/player.cpp | 140 +++++++++++------- 2 files changed, 90 insertions(+), 54 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/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index e76475adb..bd76c2a0c 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -307,8 +307,20 @@ class PlayerImpl void configure_play_until_timestamp(); bool shall_stop_at_timestamp(const rcutils_time_point_value_t & msg_timestamp) const; - void print_progress_bar_help_str(double progress_bar_print_frequency) const; - void print_progress_bar_status(const PlayerStatus & status, bool force_update = false); + std::atomic_bool is_delayed_status_{false}; + + void print_progress_bar_help_str() const; + // Call update_progress_bar_after_message_published function only if the function 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_after_message_published( + const rcutils_time_point_value_t & timestamp, + const PlayerStatus & status); + 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_; @@ -340,6 +352,7 @@ class PlayerImpl std::condition_variable playback_finished_cv_; rcutils_time_point_value_t starting_time_; + rcutils_time_point_value_t ending_time_; double starting_time_secs_; double duration_secs_; @@ -368,8 +381,7 @@ class PlayerImpl 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_printed_{}; - rcutils_time_point_value_t recv_timestamp_last_published_; + std::chrono::steady_clock::time_point progress_bar_last_time_updated_{}; }; PlayerImpl::PlayerImpl( @@ -422,6 +434,7 @@ PlayerImpl::PlayerImpl( rcutils_duration_value_t bag_duration = metadata.duration.count(); starting_time_ = std::chrono::duration_cast( metadata.starting_time.time_since_epoch()).count(); + ending_time_ = starting_time_ + 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 if (play_options_.start_offset < 0) { @@ -434,9 +447,9 @@ PlayerImpl::PlayerImpl( starting_time_ += play_options_.start_offset; bag_duration -= play_options_.start_offset; } - starting_time_secs_ = RCUTILS_NS_TO_S(static_cast(starting_time_)); - duration_secs_ = RCUTILS_NS_TO_S(static_cast(bag_duration)); - recv_timestamp_last_published_ = starting_time_; + 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(bag_duration), 0.0)); clock_ = std::make_unique( starting_time_, std::chrono::steady_clock::now, std::chrono::milliseconds{100}, play_options_.start_paused); @@ -447,7 +460,7 @@ PlayerImpl::PlayerImpl( } create_control_services(); add_keyboard_callbacks(); - print_progress_bar_help_str(play_options.progress_bar_print_frequency); + print_progress_bar_help_str(); } PlayerImpl::~PlayerImpl() @@ -468,6 +481,7 @@ PlayerImpl::~PlayerImpl() if (reader_) { reader_->close(); } + update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); // print new line to keep on screen the latest progress bar (which had a carriage return) if (enable_progress_bar_) { std::cout << std::endl; @@ -521,19 +535,19 @@ bool PlayerImpl::play() try { do { if (delay > rclcpp::Duration(0, 0)) { + is_delayed_status_ = true; RCLCPP_INFO_STREAM(owner_->get_logger(), "Sleep " << delay.nanoseconds() << " ns"); - print_progress_bar_status(PlayerStatus::DELAYED); + update_progress_bar(PlayerStatus::DELAYED); std::chrono::nanoseconds delay_duration(delay.nanoseconds()); std::this_thread::sleep_for(delay_duration); - } else { - print_progress_bar_status(clock_->is_paused() ? - PlayerStatus::PAUSED : PlayerStatus::RUNNING, true); + is_delayed_status_ = false; } { std::lock_guard lk(reader_mutex_); reader_->seek(starting_time_); clock_->jump(starting_time_); } + update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); load_storage_content_ = true; storage_loading_future_ = std::async( std::launch::async, [this]() { @@ -592,8 +606,7 @@ bool PlayerImpl::play() is_in_playback_ = false; playback_finished_cv_.notify_all(); } - print_progress_bar_status(clock_->is_paused() ? - PlayerStatus::PAUSED : PlayerStatus::RUNNING, true); + update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); }); return true; } @@ -644,22 +657,21 @@ void PlayerImpl::stop() playback_thread_.join(); } } - print_progress_bar_status(clock_->is_paused() ? - PlayerStatus::PAUSED : PlayerStatus::RUNNING, true); + update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); } void PlayerImpl::pause() { clock_->pause(); RCLCPP_INFO_STREAM(owner_->get_logger(), "Pausing play."); - print_progress_bar_status(PlayerStatus::PAUSED); + update_progress_bar(PlayerStatus::PAUSED); } void PlayerImpl::resume() { clock_->resume(); RCLCPP_INFO_STREAM(owner_->get_logger(), "Resuming play."); - print_progress_bar_status(PlayerStatus::RUNNING, true); + update_progress_bar(PlayerStatus::RUNNING); } void PlayerImpl::toggle_paused() @@ -686,8 +698,7 @@ bool PlayerImpl::set_rate(double rate) } else { RCLCPP_WARN_STREAM(owner_->get_logger(), "Failed to set rate to invalid value " << rate); } - print_progress_bar_status(clock_->is_paused() ? - PlayerStatus::PAUSED : PlayerStatus::RUNNING, true); + update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); return ok; } @@ -724,7 +735,7 @@ bool PlayerImpl::play_next(bool is_burst_mode) { if (!clock_->is_paused()) { RCLCPP_WARN_STREAM(owner_->get_logger(), "Called play next, but not in paused state."); - print_progress_bar_status(PlayerStatus::RUNNING, true); + update_progress_bar(PlayerStatus::RUNNING); return false; } @@ -737,7 +748,7 @@ bool PlayerImpl::play_next(bool is_burst_mode) // resume() or stop() from another thread while we were waiting on mutex. if (!clock_->is_paused()) { RCLCPP_WARN_STREAM(owner_->get_logger(), "Called play next, but not in paused state."); - print_progress_bar_status(PlayerStatus::RUNNING, true); + update_progress_bar(PlayerStatus::RUNNING); return false; } skip_message_in_main_play_loop_ = true; @@ -755,13 +766,18 @@ bool PlayerImpl::play_next(bool is_burst_mode) message_ptr != nullptr && !shall_stop_at_timestamp(message_ptr->recv_timestamp)) { next_message_published = publish_message(message_ptr); - clock_->jump(message_ptr->recv_timestamp); - // Update the progress bar if we were not called in burst mode to avoid delays. + // Update the progress bar only if we were not called in burst mode to avoid delays. if (!is_burst_mode) { - recv_timestamp_last_published_ = message_ptr->recv_timestamp; - print_progress_bar_status(PlayerStatus::PAUSED); + // update_progress_bar_after_message_published in this code section is protected + // by the mutex skip_message_in_main_play_loop_mutex_, and the other + // function call in this file is protected by the same mutex. + update_progress_bar_after_message_published( + message_ptr->recv_timestamp, PlayerStatus::PAUSED); } + + clock_->jump(message_ptr->recv_timestamp); + message_queue_.pop(); message_ptr = peek_next_message_from_queue(); } @@ -772,7 +788,7 @@ 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."); - print_progress_bar_status(PlayerStatus::RUNNING, true); + update_progress_bar(PlayerStatus::RUNNING); return 0; } @@ -787,8 +803,7 @@ size_t PlayerImpl::burst(const size_t num_messages) } RCLCPP_INFO_STREAM(owner_->get_logger(), "Burst " << messages_played << " messages."); - print_progress_bar_status(clock_->is_paused() ? - PlayerStatus::PAUSED : PlayerStatus::RUNNING, true); + update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); return messages_played; } @@ -1014,8 +1029,11 @@ void PlayerImpl::play_messages_from_queue() continue; } publish_message(message_ptr); - recv_timestamp_last_published_ = message_ptr->recv_timestamp; - print_progress_bar_status(PlayerStatus::RUNNING); + // update_progress_bar_after_message_published in this code section is protected + // by the mutex skip_message_in_main_play_loop_mutex_, and the other + // function call in this file is protected by the same mutex. + update_progress_bar_after_message_published( + message_ptr->recv_timestamp, PlayerStatus::RUNNING); } message_queue_.pop(); message_ptr = peek_next_message_from_queue(); @@ -1587,16 +1605,16 @@ const rosbag2_transport::PlayOptions & PlayerImpl::get_play_options() return play_options_; } -void PlayerImpl::print_progress_bar_help_str(double progress_bar_print_frequency) const +void PlayerImpl::print_progress_bar_help_str() const { std::string progress_bar_help_str; if (enable_progress_bar_) { - if (progress_bar_update_period_ < 0) { + if (progress_bar_update_always_) { progress_bar_help_str = "Progress bar enabled with updates for every message."; } else { std::ostringstream oss; - oss << "Progress bar enabled with updates at " - << std::fixed << std::setprecision(1) << progress_bar_print_frequency << " Hz."; + oss << "Progress bar enabled with updates at " << + play_options_.progress_bar_print_frequency << " Hz."; progress_bar_help_str = oss.str(); } progress_bar_help_str += " [?]: [R]unning, [P]aused, [D]elayed."; @@ -1606,38 +1624,52 @@ void PlayerImpl::print_progress_bar_help_str(double progress_bar_print_frequency RCLCPP_INFO_STREAM(owner_->get_logger(), progress_bar_help_str); } -void PlayerImpl::print_progress_bar_status(const PlayerStatus & status, bool force_update) +void PlayerImpl::update_progress_bar_after_message_published( + const rcutils_time_point_value_t & timestamp, + const PlayerStatus & status) { if (!enable_progress_bar_) { return; } - const std::chrono::steady_clock::time_point steady_time_now = std::chrono::steady_clock::now(); - - // Check if we should force an update of the progress bar, otherwise check time since last update. - // Force an update if: - // - the user-defined update period is negative; - // - or this function was called with a status other than RUNNING (since we want to update the - // progress bar when we pause or delay the playback, otherwise it will be stuck - // to the last update if not enough time has passed since it); - // - or this function was called with force_update set to true. - if (!(progress_bar_update_always_ || status != PlayerStatus::RUNNING || force_update)) { + 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_printed_).count() < progress_bar_update_period_) + steady_time_now - progress_bar_last_time_updated_).count() < progress_bar_update_period_) { return; } + progress_bar_last_time_updated_ = steady_time_now; } - const double current_time_secs = RCUTILS_NS_TO_S( - static_cast(recv_timestamp_last_published_)); + cout_progress_bar(timestamp, status); +} + +void PlayerImpl::update_progress_bar(const PlayerStatus & status) const +{ + if (!enable_progress_bar_) { + return; + } + + 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_; // Print progress bar ending with carriage return '/r' so it will be overwritten by next print - std::cout << " Bag Time " << std::setw(13) << std::fixed << std::setprecision(6) << - current_time_secs << " Duration " << std::fixed << std::setprecision(6) << - progress_secs << "/" << duration_secs_ << " [" << static_cast(status) << - "]\r" << std::flush; - progress_bar_last_time_printed_ = steady_time_now; + std::ostringstream oss; + oss << " Bag Time " << std::setw(13) << std::fixed << std::setprecision(6) << + current_time_secs << " Duration " << progress_secs << "/" << duration_secs_ << + " [" << static_cast(status) << "]\r"; + std::cout << oss.str() << std::flush; } /////////////////////////////// From 0c99597fe6bb77030ac0ec32ef1860e9456b7104 Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Mon, 18 Nov 2024 21:53:16 +0100 Subject: [PATCH 08/33] add burst status, reduce cout setprecision Signed-off-by: Nicola Loi --- .../src/rosbag2_transport/player.cpp | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index bd76c2a0c..344196ee3 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -85,6 +85,7 @@ enum class PlayerStatus : char { RUNNING = 'R', PAUSED = 'P', + BURST = 'B', DELAYED = 'D', }; @@ -657,7 +658,6 @@ void PlayerImpl::stop() playback_thread_.join(); } } - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); } void PlayerImpl::pause() @@ -792,6 +792,7 @@ size_t PlayerImpl::burst(const size_t num_messages) return 0; } + update_progress_bar(PlayerStatus::BURST); uint64_t messages_played = 0; for (auto ii = 0u; ii < num_messages || num_messages == 0; ++ii) { @@ -1607,21 +1608,23 @@ const rosbag2_transport::PlayOptions & PlayerImpl::get_play_options() void PlayerImpl::print_progress_bar_help_str() const { - std::string progress_bar_help_str; + std::string help_str; if (enable_progress_bar_) { if (progress_bar_update_always_) { - progress_bar_help_str = "Progress bar enabled with updates for every message."; + help_str = "Progress bar enabled for every message."; } else { std::ostringstream oss; - oss << "Progress bar enabled with updates at " << + oss << "Progress bar enabled at " << play_options_.progress_bar_print_frequency << " Hz."; - progress_bar_help_str = oss.str(); + help_str = oss.str(); } - progress_bar_help_str += " [?]: [R]unning, [P]aused, [D]elayed."; + 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 { - progress_bar_help_str = "Progress bar disabled."; + help_str = "Progress bar disabled"; + RCLCPP_INFO_STREAM(owner_->get_logger(), help_str); } - RCLCPP_INFO_STREAM(owner_->get_logger(), progress_bar_help_str); } void PlayerImpl::update_progress_bar_after_message_published( @@ -1667,8 +1670,8 @@ void PlayerImpl::cout_progress_bar( // Print progress bar ending with carriage return '/r' so it will be overwritten by next print std::ostringstream oss; oss << " Bag Time " << std::setw(13) << std::fixed << std::setprecision(6) << - current_time_secs << " Duration " << progress_secs << "/" << duration_secs_ << - " [" << static_cast(status) << "]\r"; + current_time_secs << " Duration " << std::setprecision(3) << progress_secs << + "/" << duration_secs_ << " [" << static_cast(status) << "]\r"; std::cout << oss.str() << std::flush; } From 6051e6cfc3310d3f6622233586d6157c2343ae99 Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Mon, 2 Dec 2024 23:29:10 +0000 Subject: [PATCH 09/33] some updates after merge with rolling Signed-off-by: Nicola Loi --- .../src/rosbag2_transport/player.cpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 01d0ece32..d490b0710 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -410,7 +410,7 @@ class PlayerImpl return l->recv_timestamp > r->recv_timestamp; } } bag_message_chronological_recv_timestamp_comparator; - + // progress bar const bool enable_progress_bar_; const bool progress_bar_update_always_; @@ -463,6 +463,7 @@ PlayerImpl::PlayerImpl( 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()}); @@ -470,9 +471,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 @@ -484,11 +490,11 @@ PlayerImpl::PlayerImpl( ". Negative start offset ignored."); } else { starting_time_ += play_options_.start_offset; - bag_duration -= 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(bag_duration), 0.0)); + 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); @@ -550,6 +556,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; } } @@ -805,6 +812,11 @@ 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); } From 0ef3f260ef310b23c3d508722588dc149040dc35 Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Wed, 4 Dec 2024 22:35:10 +0000 Subject: [PATCH 10/33] comments Signed-off-by: Nicola Loi --- .../src/rosbag2_transport/player.cpp | 30 ++++++++++++++----- 1 file changed, 22 insertions(+), 8 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index d490b0710..fc2ecb42a 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -317,13 +317,19 @@ class PlayerImpl std::atomic_bool is_delayed_status_{false}; void print_progress_bar_help_str() const; - // Call update_progress_bar_after_message_published function only if the function cannot run + // Update progress bar taking into account the frequency set by the user. + // The function should be called for regular progress bar updates, for example + // after the recurrent message publishing. + // Call update_progress_bar_check_frequency function only if the function 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_after_message_published( + void update_progress_bar_check_frequency( const rcutils_time_point_value_t & timestamp, const PlayerStatus & status); + // Update progress bar irrespective of the frequency 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, @@ -628,6 +634,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); @@ -702,6 +710,8 @@ void PlayerImpl::stop() cancel_wait_for_next_message_ = true; } + update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + if (clock_->is_paused()) { clock_->resume(); // Temporary resume clock to force wakeup in clock_->sleep_until(time) clock_->pause(); // Return in pause mode to preserve original state of the player @@ -1098,10 +1108,10 @@ void PlayerImpl::play_messages_from_queue() finished_play_next_cv_.notify_all(); } } else { - // update_progress_bar_after_message_published in this code section is protected + // update_progress_bar_check_frequency in this code section is protected // by the mutex skip_message_in_main_play_loop_mutex_. - update_progress_bar_after_message_published( - message_ptr->recv_timestamp, PlayerStatus::RUNNING); + update_progress_bar_check_frequency( + message_ptr->recv_timestamp, PlayerStatus::RUNNING); } } message_ptr = take_next_message_from_queue(); @@ -1728,7 +1738,7 @@ void PlayerImpl::print_progress_bar_help_str() const } else { std::ostringstream oss; oss << "Progress bar enabled at " << - play_options_.progress_bar_print_frequency << " Hz."; + play_options_.progress_bar_print_frequency << " Hz"; help_str = oss.str(); } RCLCPP_INFO_STREAM(owner_->get_logger(), help_str); @@ -1740,7 +1750,7 @@ void PlayerImpl::print_progress_bar_help_str() const } } -void PlayerImpl::update_progress_bar_after_message_published( +void PlayerImpl::update_progress_bar_check_frequency( const rcutils_time_point_value_t & timestamp, const PlayerStatus & status) { @@ -1748,6 +1758,8 @@ void PlayerImpl::update_progress_bar_after_message_published( return; } + // If we are not updating the progress bar for every call, check if we should update it now + // based on the frequency 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( @@ -1767,6 +1779,8 @@ void PlayerImpl::update_progress_bar(const PlayerStatus & status) const return; } + // Update progress bar irrespective of the frequency 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 { @@ -1784,7 +1798,7 @@ void PlayerImpl::cout_progress_bar( std::ostringstream oss; oss << " Bag Time " << std::setw(13) << std::fixed << std::setprecision(6) << current_time_secs << " Duration " << std::setprecision(3) << progress_secs << - "/" << duration_secs_ << " [" << static_cast(status) << "]\r"; + "/" << duration_secs_ << " [" << static_cast(status) << "] \r"; std::cout << oss.str() << std::flush; } From 47dff3d1bfa4ccfb18354084c28993f96763ae5f Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Thu, 5 Dec 2024 22:06:34 +0000 Subject: [PATCH 11/33] Progress bar with nanosecond precision and shorter length Signed-off-by: Nicola Loi --- rosbag2_transport/src/rosbag2_transport/player.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index fc2ecb42a..17f093190 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -579,6 +579,8 @@ bool PlayerImpl::play() } RCLCPP_INFO_STREAM(owner_->get_logger(), "Playback until timestamp: " << play_until_timestamp_); + RCLCPP_INFO_STREAM(owner_->get_logger(), "Duration of the rosbag: " << std::fixed << + std::setprecision(9) << duration_secs_ << " s"); // May need to join the previous thread if we are calling play() a second time if (playback_thread_.joinable()) { @@ -1742,8 +1744,10 @@ void PlayerImpl::print_progress_bar_help_str() const 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"; + std::string help_str2 = "Progress bar t: time, d: duration"; RCLCPP_INFO_STREAM(owner_->get_logger(), help_str2); + std::string help_str3 = "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed"; + RCLCPP_INFO_STREAM(owner_->get_logger(), help_str3); } else { help_str = "Progress bar disabled"; RCLCPP_INFO_STREAM(owner_->get_logger(), help_str); @@ -1795,10 +1799,12 @@ void PlayerImpl::cout_progress_bar( const double current_time_secs = RCUTILS_NS_TO_S(static_cast(timestamp)); const double progress_secs = current_time_secs - starting_time_secs_; // Print progress bar ending with carriage return '/r' so it will be overwritten by next print + // Spaces before '\r' are to clear any previous progress bar in case the new one is shorter, + // which can happen when the playback starts a new loop. std::ostringstream oss; - oss << " Bag Time " << std::setw(13) << std::fixed << std::setprecision(6) << - current_time_secs << " Duration " << std::setprecision(3) << progress_secs << - "/" << duration_secs_ << " [" << static_cast(status) << "] \r"; + oss << " Bag t: " << std::setw(13) << std::fixed << std::setprecision(9) << + current_time_secs << " d: " << std::setprecision(9) << progress_secs << + " [" << static_cast(status) << "] \r"; std::cout << oss.str() << std::flush; } From 19df032cb2fb786e2cacfd1882cbdd63a820884b Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Fri, 6 Dec 2024 22:28:51 +0100 Subject: [PATCH 12/33] change progress bar string, change 'frequency' terms to 'update rate' Signed-off-by: Nicola Loi --- ros2bag/ros2bag/verb/play.py | 2 +- rosbag2_py/rosbag2_py/_transport.pyi | 2 +- rosbag2_py/src/rosbag2_py/_transport.cpp | 2 +- .../rosbag2_transport/play_options.hpp | 2 +- .../src/rosbag2_transport/play_options.cpp | 4 +- .../src/rosbag2_transport/player.cpp | 40 +++++++++---------- 6 files changed, 24 insertions(+), 28 deletions(-) diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index 96771d61d..31536d969 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -282,7 +282,7 @@ def main(self, *, args): # noqa: D102 play_options.service_requests_source = ServiceRequestsSource.SERVICE_INTROSPECTION else: play_options.service_requests_source = ServiceRequestsSource.CLIENT_INTROSPECTION - play_options.progress_bar_print_frequency = args.progress_bar + play_options.progress_bar_update_rate = args.progress_bar player = Player(args.log_level) try: diff --git a/rosbag2_py/rosbag2_py/_transport.pyi b/rosbag2_py/rosbag2_py/_transport.pyi index 05e2a707e..a1488d4b3 100644 --- a/rosbag2_py/rosbag2_py/_transport.pyi +++ b/rosbag2_py/rosbag2_py/_transport.pyi @@ -17,7 +17,7 @@ class PlayOptions: node_prefix: str playback_duration: float playback_until_timestamp: int - progress_bar_print_frequency: float + 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 945c043e7..f9a3b8c4b 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -556,7 +556,7 @@ PYBIND11_MODULE(_transport, m) { "playback_until_timestamp", &PlayOptions::getPlaybackUntilTimestamp, &PlayOptions::setPlaybackUntilTimestamp) - .def_readwrite("progress_bar_print_frequency", &PlayOptions::progress_bar_print_frequency) + .def_readwrite("progress_bar_update_rate", &PlayOptions::progress_bar_update_rate) .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 975792112..f8bf67b0b 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -125,7 +125,7 @@ struct PlayOptions ServiceRequestsSource service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; // Rate in Hz at which to print progress bar. - double progress_bar_print_frequency = 3.0; + double progress_bar_update_rate = 3.0; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/src/rosbag2_transport/play_options.cpp b/rosbag2_transport/src/rosbag2_transport/play_options.cpp index c38683f51..10e608855 100644 --- a/rosbag2_transport/src/rosbag2_transport/play_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/play_options.cpp @@ -59,7 +59,7 @@ Node convert::encode( node["disable_loan_message"] = play_options.disable_loan_message; - node["progress_bar_print_frequency"] = play_options.progress_bar_print_frequency; + node["progress_bar_update_rate"] = play_options.progress_bar_update_rate; return node; } @@ -117,7 +117,7 @@ bool convert::decode( optional_assign(node, "disable_loan_message", play_options.disable_loan_message); optional_assign( - node, "progress_bar_print_frequency", play_options.progress_bar_print_frequency); + node, "progress_bar_update_rate", play_options.progress_bar_update_rate); return true; } diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 17f093190..2903c31c3 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -317,17 +317,17 @@ class PlayerImpl std::atomic_bool is_delayed_status_{false}; void print_progress_bar_help_str() const; - // Update progress bar taking into account the frequency set by the user. + // Update progress bar 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 message publishing. - // Call update_progress_bar_check_frequency function only if the function cannot run + // Call update_progress_bar_check_rate function only if the function 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_frequency( + void update_progress_bar_check_rate( const rcutils_time_point_value_t & timestamp, const PlayerStatus & status); - // Update progress bar irrespective of the frequency set by the user. + // Update progress bar 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; @@ -435,10 +435,10 @@ PlayerImpl::PlayerImpl( message_queue_(bag_message_chronological_recv_timestamp_comparator), keyboard_handler_(std::move(keyboard_handler)), player_service_client_manager_(std::make_shared()), - enable_progress_bar_(play_options.progress_bar_print_frequency != 0), - progress_bar_update_always_(play_options.progress_bar_print_frequency < 0), - progress_bar_update_period_(play_options.progress_bar_print_frequency != 0 ? - RCUTILS_S_TO_NS(1.0 / play_options.progress_bar_print_frequency) : + 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) { @@ -579,8 +579,6 @@ bool PlayerImpl::play() } RCLCPP_INFO_STREAM(owner_->get_logger(), "Playback until timestamp: " << play_until_timestamp_); - RCLCPP_INFO_STREAM(owner_->get_logger(), "Duration of the rosbag: " << std::fixed << - std::setprecision(9) << duration_secs_ << " s"); // May need to join the previous thread if we are calling play() a second time if (playback_thread_.joinable()) { @@ -1110,9 +1108,9 @@ void PlayerImpl::play_messages_from_queue() finished_play_next_cv_.notify_all(); } } else { - // update_progress_bar_check_frequency in this code section is protected + // 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_frequency( + update_progress_bar_check_rate( message_ptr->recv_timestamp, PlayerStatus::RUNNING); } } @@ -1740,21 +1738,19 @@ void PlayerImpl::print_progress_bar_help_str() const } else { std::ostringstream oss; oss << "Progress bar enabled at " << - play_options_.progress_bar_print_frequency << " Hz"; + 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 t: time, d: duration"; + std::string help_str2 = "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed"; RCLCPP_INFO_STREAM(owner_->get_logger(), help_str2); - std::string help_str3 = "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed"; - RCLCPP_INFO_STREAM(owner_->get_logger(), help_str3); } else { help_str = "Progress bar disabled"; RCLCPP_INFO_STREAM(owner_->get_logger(), help_str); } } -void PlayerImpl::update_progress_bar_check_frequency( +void PlayerImpl::update_progress_bar_check_rate( const rcutils_time_point_value_t & timestamp, const PlayerStatus & status) { @@ -1763,7 +1759,7 @@ void PlayerImpl::update_progress_bar_check_frequency( } // If we are not updating the progress bar for every call, check if we should update it now - // based on the frequency set by the user + // 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( @@ -1783,7 +1779,7 @@ void PlayerImpl::update_progress_bar(const PlayerStatus & status) const return; } - // Update progress bar irrespective of the frequency set by the user. + // 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); @@ -1802,9 +1798,9 @@ void PlayerImpl::cout_progress_bar( // Spaces before '\r' are to clear any previous progress bar in case the new one is shorter, // which can happen when the playback starts a new loop. std::ostringstream oss; - oss << " Bag t: " << std::setw(13) << std::fixed << std::setprecision(9) << - current_time_secs << " d: " << std::setprecision(9) << progress_secs << - " [" << static_cast(status) << "] \r"; + oss << "[" << std::setw(13) << std::fixed << std::setprecision(9) << current_time_secs << + "] Duration " << std::setprecision(2) << progress_secs << + "/" << duration_secs_ << " [" << static_cast(status) << "] \r"; std::cout << oss.str() << std::flush; } From fb464b027075bd981200886f59a7781362f50b75 Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Fri, 6 Dec 2024 22:40:45 +0100 Subject: [PATCH 13/33] comments Signed-off-by: Nicola Loi --- rosbag2_transport/src/rosbag2_transport/player.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 2903c31c3..05d91ee83 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -319,10 +319,10 @@ class PlayerImpl void print_progress_bar_help_str() const; // Update progress bar 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 message publishing. - // Call update_progress_bar_check_rate function only if the function 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 + // 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, From 8625164069aeb46df0f278c289df3ffd7b322512 Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Sun, 8 Dec 2024 17:14:26 +0100 Subject: [PATCH 14/33] add custom space between playback output and progress bar Signed-off-by: Nicola Loi --- ros2bag/ros2bag/verb/play.py | 11 +++++ rosbag2_py/rosbag2_py/_transport.pyi | 1 + rosbag2_py/src/rosbag2_py/_transport.cpp | 1 + .../rosbag2_transport/play_options.hpp | 4 ++ .../src/rosbag2_transport/play_options.cpp | 4 ++ .../src/rosbag2_transport/player.cpp | 44 ++++++++++++++----- 6 files changed, 54 insertions(+), 11 deletions(-) diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index 8635b9642..79ed25611 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -188,6 +188,16 @@ def add_arguments(self, parser, cli_name): # noqa: D102 '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 @@ -300,6 +310,7 @@ def main(self, *, args): # noqa: D102 }.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: diff --git a/rosbag2_py/rosbag2_py/_transport.pyi b/rosbag2_py/rosbag2_py/_transport.pyi index 4d0aefcbd..4cba0de59 100644 --- a/rosbag2_py/rosbag2_py/_transport.pyi +++ b/rosbag2_py/rosbag2_py/_transport.pyi @@ -35,6 +35,7 @@ class PlayOptions: playback_duration: float playback_until_timestamp: int progress_bar_update_rate: float + progress_bar_separation_lines: int publish_service_requests: bool rate: float read_ahead_queue_size: int diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 7242137b2..929d7e15f 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -557,6 +557,7 @@ PYBIND11_MODULE(_transport, m) { &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 8146e3abd..c4e3f9be0 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -140,6 +140,10 @@ struct PlayOptions // 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 10e608855..4fd8d1207 100644 --- a/rosbag2_transport/src/rosbag2_transport/play_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/play_options.cpp @@ -60,6 +60,7 @@ 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; } @@ -119,6 +120,9 @@ bool convert::decode( 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 14415320c..b7369a847 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -318,8 +318,12 @@ class PlayerImpl 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 taking into account the update rate set by the user. + // 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 @@ -329,7 +333,8 @@ class PlayerImpl void update_progress_bar_check_rate( const rcutils_time_point_value_t & timestamp, const PlayerStatus & status); - // Update progress bar irrespective of the update rate set by the user. + // 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; @@ -494,9 +499,18 @@ 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_) { @@ -564,9 +578,11 @@ PlayerImpl::~PlayerImpl() } } update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); - // print new line to keep on screen the latest progress bar (which had a carriage return) + // arrange cursor position to be after the progress bar if (enable_progress_bar_) { - std::cout << std::endl; + std::ostringstream oss; + oss << "\033[" << play_options_.progress_bar_separation_lines + 1 << "B\n"; + std::cout << oss.str() << std::flush; } } @@ -637,7 +653,7 @@ bool PlayerImpl::play() } update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); - + load_storage_content_ = true; storage_loading_future_ = std::async( std::launch::async, [this]() { @@ -1841,13 +1857,19 @@ void PlayerImpl::cout_progress_bar( { const double current_time_secs = RCUTILS_NS_TO_S(static_cast(timestamp)); const double progress_secs = current_time_secs - starting_time_secs_; - // Print progress bar ending with carriage return '/r' so it will be overwritten by next print - // Spaces before '\r' are to clear any previous progress bar in case the new one is shorter, - // which can happen when the playback starts a new loop. std::ostringstream oss; - oss << "[" << std::setw(13) << std::fixed << std::setprecision(9) << current_time_secs << + 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 << - "/" << duration_secs_ << " [" << static_cast(status) << "] \r"; + // 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; } From 913de31ae38e9c9c525cba98a7a38065098cb3e7 Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Sun, 8 Dec 2024 18:27:06 +0100 Subject: [PATCH 15/33] stubgen Signed-off-by: Nicola Loi --- 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 4cba0de59..010a1125b 100644 --- a/rosbag2_py/rosbag2_py/_transport.pyi +++ b/rosbag2_py/rosbag2_py/_transport.pyi @@ -34,8 +34,8 @@ class PlayOptions: node_prefix: str playback_duration: float playback_until_timestamp: int - progress_bar_update_rate: float progress_bar_separation_lines: int + progress_bar_update_rate: float publish_service_requests: bool rate: float read_ahead_queue_size: int From 5ed19944673db40f9b5cb85187be5dc36c81cdde Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 10 Dec 2024 13:54:15 -0500 Subject: [PATCH 16/33] Add test_xmllint.py to python packages. (#1879) * Add test_xmllint.py to python packages. This ensures we'll detect any invalid XML in e.g. the package.xml Signed-off-by: Chris Lalancette * Add in test_depend on ament_xmllint. Signed-off-by: Chris Lalancette --------- Signed-off-by: Chris Lalancette --- ros2bag/package.xml | 1 + ros2bag/test/test_xmllint.py | 23 +++++++++++++++++++ .../rosbag2_examples_py/package.xml | 1 + .../rosbag2_examples_py/test/test_xmllint.py | 23 +++++++++++++++++++ 4 files changed, 48 insertions(+) create mode 100644 ros2bag/test/test_xmllint.py create mode 100644 rosbag2_examples/rosbag2_examples_py/test/test_xmllint.py diff --git a/ros2bag/package.xml b/ros2bag/package.xml index c1d89c424..7fd74d31f 100644 --- a/ros2bag/package.xml +++ b/ros2bag/package.xml @@ -22,6 +22,7 @@ ament_copyright ament_flake8 ament_pep257 + ament_xmllint launch_testing launch_testing_ros python3-pytest diff --git a/ros2bag/test/test_xmllint.py b/ros2bag/test/test_xmllint.py new file mode 100644 index 000000000..08bf7fd78 --- /dev/null +++ b/ros2bag/test/test_xmllint.py @@ -0,0 +1,23 @@ +# Copyright 2019 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. + +from ament_xmllint.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.xmllint +def test_xmllint() -> None: + rc = main(argv=[]) + assert rc == 0, 'Found errors' diff --git a/rosbag2_examples/rosbag2_examples_py/package.xml b/rosbag2_examples/rosbag2_examples_py/package.xml index b4a9a4333..f0a5ed6e7 100644 --- a/rosbag2_examples/rosbag2_examples_py/package.xml +++ b/rosbag2_examples/rosbag2_examples_py/package.xml @@ -15,6 +15,7 @@ ament_copyright ament_flake8 ament_pep257 + ament_xmllint python3-pytest diff --git a/rosbag2_examples/rosbag2_examples_py/test/test_xmllint.py b/rosbag2_examples/rosbag2_examples_py/test/test_xmllint.py new file mode 100644 index 000000000..08bf7fd78 --- /dev/null +++ b/rosbag2_examples/rosbag2_examples_py/test/test_xmllint.py @@ -0,0 +1,23 @@ +# Copyright 2019 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. + +from ament_xmllint.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.xmllint +def test_xmllint() -> None: + rc = main(argv=[]) + assert rc == 0, 'Found errors' From ab7cd938ae60af6e69cc2857d96f3add6377e770 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Thu, 12 Dec 2024 07:03:22 -0800 Subject: [PATCH 17/33] Add more logging info to storage and reader/writer open operations (#1881) Signed-off-by: Michael Orlov --- .../rosbag2_cpp/readers/sequential_reader.cpp | 7 +++- .../rosbag2_cpp/writers/sequential_writer.cpp | 4 ++ .../test_storage_without_metadata_file.cpp | 1 + .../impl/storage_factory_impl.hpp | 39 ++++++++++++++++--- 4 files changed, 45 insertions(+), 6 deletions(-) diff --git a/rosbag2_cpp/src/rosbag2_cpp/readers/sequential_reader.cpp b/rosbag2_cpp/src/rosbag2_cpp/readers/sequential_reader.cpp index 8c8634bd6..301a4d2f0 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/readers/sequential_reader.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/readers/sequential_reader.cpp @@ -87,6 +87,10 @@ void SequentialReader::open( const rosbag2_storage::StorageOptions & storage_options, const ConverterOptions & converter_options) { + if (storage_options.uri.empty()) { + throw std::runtime_error("Can't open rosbag2_cpp::SequentialReader. The input URI is empty"); + } + storage_options_ = storage_options; base_folder_ = storage_options.uri; @@ -108,7 +112,8 @@ void SequentialReader::open( } else { storage_ = storage_factory_->open_read_only(storage_options_); if (!storage_) { - throw std::runtime_error{"No storage could be initialized from the inputs."}; + throw std::runtime_error("No storage could be initialized for the input URI: " + + storage_options.uri); } if (!set_read_order(read_order_)) { ROSBAG2_CPP_LOG_WARN( diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index 2d3838856..ea6d2fa57 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -103,6 +103,10 @@ void SequentialWriter::open( if (is_open_) { return; // The writer already opened } + if (storage_options.uri.empty()) { + throw std::runtime_error("Can't open rosbag2_cpp::SequentialWriter. The input URI is empty"); + } + base_folder_ = storage_options.uri; storage_options_ = storage_options; diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_storage_without_metadata_file.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_storage_without_metadata_file.cpp index 8b8ace9d3..967b8a65f 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_storage_without_metadata_file.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_storage_without_metadata_file.cpp @@ -74,6 +74,7 @@ TEST_F(StorageWithoutMetadataFileTest, open_uses_storage_id_from_storage_options EXPECT_CALL(*metadata_io, metadata_file_exists).Times(1).WillOnce(Return(false)); rosbag2_storage::StorageOptions storage_options; + storage_options.uri = "foo.bar"; storage_options.storage_id = kStorageId; auto sequential_reader = std::make_unique( diff --git a/rosbag2_storage/src/rosbag2_storage/impl/storage_factory_impl.hpp b/rosbag2_storage/src/rosbag2_storage/impl/storage_factory_impl.hpp index 85c469468..a73a24aab 100644 --- a/rosbag2_storage/src/rosbag2_storage/impl/storage_factory_impl.hpp +++ b/rosbag2_storage/src/rosbag2_storage/impl/storage_factory_impl.hpp @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include "pluginlib/class_loader.hpp" @@ -160,19 +162,38 @@ class StorageFactoryImpl virtual ~StorageFactoryImpl() = default; + std::unordered_set get_registered_plugin_names() + { + std::vector read_only_plugins = read_only_class_loader_->getDeclaredClasses(); + std::vector read_write_plugins = read_write_class_loader_->getDeclaredClasses(); + + // Merge read-only and read-write plugins + std::unordered_set combined_plugins( + read_only_plugins.begin(), read_only_plugins.end()); + combined_plugins.insert(read_write_plugins.begin(), read_write_plugins.end()); + return combined_plugins; + } + std::shared_ptr open_read_write(const StorageOptions & storage_options) { - auto instance = - get_interface_instance(read_write_class_loader_, storage_options); + auto instance = get_interface_instance(read_write_class_loader_, storage_options); if (instance == nullptr) { if (storage_options.storage_id.empty()) { ROSBAG2_STORAGE_LOG_ERROR_STREAM( - "No storage id specified, and no plugin found that could open URI"); + "No storage id specified, and no plugin found that could open URI: '" << + storage_options.uri << "'"); } else { ROSBAG2_STORAGE_LOG_ERROR_STREAM( "Could not load/open plugin with storage id '" << storage_options.storage_id << "'"); } + auto available_plugins = read_write_class_loader_->getDeclaredClasses(); + std::stringstream ss; + ss << "Available read-write storage plugins: "; + for (const auto & storage_plugin : available_plugins) { + ss << "'" << storage_plugin << "', "; + } + ROSBAG2_STORAGE_LOG_INFO("%s", ss.str().c_str()); } return instance; @@ -193,11 +214,19 @@ class StorageFactoryImpl if (instance == nullptr) { if (storage_options.storage_id.empty()) { ROSBAG2_STORAGE_LOG_ERROR_STREAM( - "No storage id specified, and no plugin found that could open URI"); + "No storage id specified, and no plugin found that could open URI: '" << + storage_options.uri << "'"); } else { ROSBAG2_STORAGE_LOG_ERROR_STREAM( - "Could not load/open plugin with storage id '" << storage_options.storage_id << "'"); + "Could not load/open plugin with storage id: '" << storage_options.storage_id << "'"); + } + auto available_plugins = get_registered_plugin_names(); + std::stringstream ss; + ss << "Available storage plugins: "; + for (const auto & storage_plugin : available_plugins) { + ss << "'" << storage_plugin << "', "; } + ROSBAG2_STORAGE_LOG_INFO("%s", ss.str().c_str()); } return instance; From 79ae2d0c21a6a677be9e595e74bac1e6ddc67513 Mon Sep 17 00:00:00 2001 From: yadunund Date: Sat, 28 Dec 2024 11:01:30 -0800 Subject: [PATCH 18/33] Increase timeout to 180s for test_rosbag2_record_end_to_end (#1889) Signed-off-by: Yadunund --- rosbag2_tests/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbag2_tests/CMakeLists.txt b/rosbag2_tests/CMakeLists.txt index 70673fde2..f8e067748 100644 --- a/rosbag2_tests/CMakeLists.txt +++ b/rosbag2_tests/CMakeLists.txt @@ -45,7 +45,7 @@ if(BUILD_TESTING) ament_add_gmock(test_rosbag2_record_end_to_end test/rosbag2_tests/test_rosbag2_record_end_to_end.cpp WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} - TIMEOUT 120) + TIMEOUT 180) if(TARGET test_rosbag2_record_end_to_end) target_link_libraries(test_rosbag2_record_end_to_end rclcpp::rclcpp From a7aa5f0894a8e52c9916cd340895d1ecfb37deb3 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 29 Dec 2024 18:44:53 -0800 Subject: [PATCH 19/33] Bugfix. Event publisher not starting for second run after stop (#1888) Signed-off-by: Michael Orlov --- rosbag2_transport/src/rosbag2_transport/recorder.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index d5d1f0b0e..62a69732b 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -328,7 +328,11 @@ void RecorderImpl::record() node->create_publisher("events/write_split", 1); // Start the thread that will publish events - event_publisher_thread_ = std::thread(&RecorderImpl::event_publisher_thread_main, this); + { + std::lock_guard lock(event_publisher_thread_mutex_); + event_publisher_thread_should_exit_ = false; + event_publisher_thread_ = std::thread(&RecorderImpl::event_publisher_thread_main, this); + } rosbag2_cpp::bag_events::WriterEventCallbacks callbacks; callbacks.write_split_callback = From 1f99a37e048b312c620942468a52060df523adb4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=98ystein=20Sture?= Date: Wed, 22 Jan 2025 03:11:31 +0100 Subject: [PATCH 20/33] Bugfix: Recorder discovery does not restart after being stopped (#1894) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Invert start discovery condition Repeated calls to start_discovery should not start additional topics discovery tasks without stop_discovery calls Signed-off-by: Øystein Sture * Rename stop_discovery to discovery_running and change logic accordingly Signed-off-by: Øystein Sture --------- Signed-off-by: Øystein Sture --- .../src/rosbag2_transport/recorder.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 62a69732b..94703768f 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -143,7 +143,7 @@ class RecorderImpl std::mutex start_stop_transition_mutex_; std::mutex discovery_mutex_; - std::atomic stop_discovery_ = false; + std::atomic discovery_running_ = false; std::atomic_uchar paused_ = 0; std::atomic in_recording_ = false; std::shared_ptr keyboard_handler_; @@ -440,7 +440,7 @@ bool RecorderImpl::is_paused() void RecorderImpl::start_discovery() { std::lock_guard state_lock(discovery_mutex_); - if (stop_discovery_.exchange(false)) { + if (discovery_running_.exchange(true)) { RCLCPP_DEBUG(node->get_logger(), "Recorder topic discovery is already running."); } else { discovery_future_ = @@ -451,10 +451,7 @@ void RecorderImpl::start_discovery() void RecorderImpl::stop_discovery() { std::lock_guard state_lock(discovery_mutex_); - if (stop_discovery_.exchange(true)) { - RCLCPP_DEBUG( - node->get_logger(), "Recorder topic discovery has already been stopped or not running."); - } else { + if (discovery_running_.exchange(false)) { if (discovery_future_.valid()) { auto status = discovery_future_.wait_for(2 * record_options_.topic_polling_interval); if (status != std::future_status::ready) { @@ -465,6 +462,9 @@ void RecorderImpl::stop_discovery() (status == std::future_status::timeout ? "timeout" : "deferred")); } } + } else { + RCLCPP_DEBUG( + node->get_logger(), "Recorder topic discovery has already been stopped or not running."); } } @@ -475,7 +475,7 @@ void RecorderImpl::topics_discovery() RCLCPP_INFO( node->get_logger(), "use_sim_time set, waiting for /clock before starting recording..."); - while (rclcpp::ok() && stop_discovery_ == false) { + while (rclcpp::ok() && discovery_running_) { if (node->get_clock()->wait_until_started(record_options_.topic_polling_interval)) { break; } @@ -484,7 +484,7 @@ void RecorderImpl::topics_discovery() RCLCPP_INFO(node->get_logger(), "Sim time /clock found, starting recording."); } } - while (rclcpp::ok() && stop_discovery_ == false) { + while (rclcpp::ok() && discovery_running_) { try { auto topics_to_subscribe = get_requested_or_available_topics(); for (const auto & topic_and_type : topics_to_subscribe) { From 6e75a9df98fe0386c33752d8ff651333245d08ce Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 11 Jan 2025 00:26:44 -0800 Subject: [PATCH 21/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 b7369a847..b7febdfd7 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -473,7 +473,7 @@ PlayerImpl::PlayerImpl( progress_bar_update_always_(play_options.progress_bar_update_rate < 0), progress_bar_update_period_(play_options.progress_bar_update_rate != 0 ? RCUTILS_S_TO_NS(1.0 / play_options.progress_bar_update_rate) : - std::numeric_limits::max()) + std::numeric_limits::max()) { for (auto & topic : play_options_.topics_to_filter) { topic = rclcpp::expand_topic_or_service_name( From ec11778f92fab5789d9d3f0bb231b28679525f9f Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 11 Jan 2025 01:49:04 -0800 Subject: [PATCH 22/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 b7febdfd7..484814c83 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -132,13 +132,12 @@ class PlayerImpl virtual bool set_rate(double); /// \brief Playing next message from queue when in pause. - /// \param in_burst_mode If true, it will not call the progress bar update to avoid delays. /// \details This is blocking call and it will wait until next available message will be /// published or rclcpp context shut down. /// \note If internal player queue is starving and storage has not been completely loaded, /// this method will wait until new element will be pushed to the queue. /// \return true if player in pause mode and successfully played next message, otherwise false. - virtual bool play_next(bool in_burst_mode = false); + virtual bool play_next(); /// \brief Burst the next \p num_messages messages from the queue when paused. /// \param num_messages The number of messages to burst from the queue. Specifying zero means no @@ -796,6 +795,7 @@ void PlayerImpl::toggle_paused() { // Note: Use upper level public API from owner class to facilitate unit tests owner_->is_paused() ? owner_->resume() : owner_->pause(); + update_progress_bar(owner_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); } bool PlayerImpl::is_paused() const @@ -838,7 +838,7 @@ rosbag2_storage::SerializedBagMessageSharedPtr PlayerImpl::take_next_message_fro return message_queue_.take().value_or(nullptr); } -bool PlayerImpl::play_next(bool is_burst_mode) +bool PlayerImpl::play_next() { if (!is_in_playback_) { RCLCPP_WARN_STREAM(owner_->get_logger(), "Called play next, but player is not playing."); @@ -870,11 +870,6 @@ bool PlayerImpl::play_next(bool is_burst_mode) finished_play_next_ = false; finished_play_next_cv_.wait(lk, [this] {return finished_play_next_.load();}); play_next_ = false; - - if (!is_burst_mode) { - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); - } - return play_next_result_.exchange(false); } @@ -890,7 +885,7 @@ size_t PlayerImpl::burst(const size_t num_messages) uint64_t messages_played = 0; for (auto ii = 0u; ii < num_messages || num_messages == 0; ++ii) { - if (play_next(true)) { + if (play_next()) { ++messages_played; } else { break; @@ -1155,12 +1150,10 @@ void PlayerImpl::play_messages_from_queue() play_next_result_ = true; finished_play_next_cv_.notify_all(); } - } else { - // update_progress_bar_check_rate in this code section is protected - // by the mutex skip_message_in_main_play_loop_mutex_. - update_progress_bar_check_rate( - message_ptr->recv_timestamp, PlayerStatus::RUNNING); } + // update_progress_bar_check_rate in this code section is protected + // by the mutex skip_message_in_main_play_loop_mutex_. + update_progress_bar_check_rate(message_ptr->recv_timestamp, PlayerStatus::RUNNING); } message_ptr = take_next_message_from_queue(); } From 510a248e099fb46f536748b5af5d9c5959b72b03 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 25 Jan 2025 15:54:40 -0800 Subject: [PATCH 23/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 484814c83..8c1a40f3a 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -83,10 +83,159 @@ namespace rosbag2_transport enum class PlayerStatus : char { - RUNNING = 'R', - PAUSED = 'P', BURST = 'B', DELAYED = 'D', + PAUSED = 'P', + RUNNING = 'R', + STOPPED = 'S', +}; + +class PlayerProgressBar +{ +public: + explicit PlayerProgressBar( + rclcpp::Logger logger, + rcutils_time_point_value_t starting_time, + rcutils_time_point_value_t ending_time, + int32_t progress_bar_update_rate = 3, + int32_t progress_bar_separation_lines = 3) + : logger_(std::move(logger)), + enable_progress_bar_(progress_bar_update_rate != 0), + progress_bar_update_always_(progress_bar_update_rate < 0), + progress_bar_update_period_(progress_bar_update_rate != 0 ? + RCUTILS_S_TO_NS(1.0 / progress_bar_update_rate) : + std::numeric_limits::max()), + progress_bar_separation_lines_(progress_bar_separation_lines) + { + starting_time_secs_ = RCUTILS_NS_TO_S( + static_cast(std::min(starting_time, ending_time))); + duration_secs_ = RCUTILS_NS_TO_S( + std::max(static_cast(ending_time - starting_time), 0.0)); + progress_secs_from_start_ = starting_time_secs_; + progress_current_time_secs_ = starting_time_secs_; + std::ostringstream oss_clear_and_move_cursor_down; + for (int i = 0; i < progress_bar_separation_lines; i++) { + oss_clear_and_move_cursor_down << "\033[2K\n"; + } + oss_clear_and_move_cursor_down << "\033[2K"; + progress_bar_helper_clear_and_move_cursor_down_ = oss_clear_and_move_cursor_down.str(); + std::ostringstream oss_move_cursor_up; + oss_move_cursor_up << "\033[" << progress_bar_separation_lines + 1 << "F"; + progress_bar_helper_move_cursor_up_ = oss_move_cursor_up.str(); + } + + ~PlayerProgressBar() + { + // arrange cursor position to be after the progress bar + if (enable_progress_bar_) { + std::ostringstream oss; + oss << "\033[" << progress_bar_separation_lines_ + 1 << "B\n"; + std::cout << oss.str() << std::flush; + } + } + void print_help_str() const + { + std::string help_str; + if (enable_progress_bar_) { + if (progress_bar_update_always_) { + help_str = "Progress bar enabled for every message."; + } else { + std::ostringstream oss; + oss << "Progress bar enabled at " << + (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz"; + help_str = oss.str(); + } + RCLCPP_INFO_STREAM(logger_, help_str); + std::string help_str2 = + "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped"; + RCLCPP_INFO_STREAM(logger_, help_str2); + } else { + help_str = "Progress bar disabled"; + RCLCPP_INFO_STREAM(logger_, help_str); + } + } + + // Update progress bar with an input timestamp, + // taking into account the update rate set by the user. + // The function should be called for regular progress bar updates, for example + // after the recurrent publishing of the messages. + // Call update_progress_bar_check_rate function only where it cannot run + // contemporaneously in multiple threads, i.e. function calls are already protected by a mutex. + // To avoid locking overhead no new mutex inside the function is directly protecting + // the access to the class attribute progress_bar_last_time_updated_. + void update_with_limited_rate( + const rcutils_time_point_value_t & timestamp, + const PlayerStatus & status) + { + if (!enable_progress_bar_) { + return; + } + + // If we are not updating the progress bar for every call, check if we should update it now + // based on the update rate set by the user + if (!progress_bar_update_always_) { + std::chrono::steady_clock::time_point steady_time_now = std::chrono::steady_clock::now(); + if (std::chrono::duration_cast( + steady_time_now - progress_bar_last_time_updated_).count() < progress_bar_update_period_) + { + return; + } + progress_bar_last_time_updated_ = steady_time_now; + } + + draw_progress_bar(timestamp, status); + } + // Update progress bar with the current playback timestamp, + // irrespective of the update rate set by the user. + // The function should be called for extraordinary progress bar updates, for example + // when a log message is printed and we want to 'redraw' the progress bar. + void update(const PlayerStatus & status) + { + if (!enable_progress_bar_) { + return; + } + // Update progress bar irrespective of the update rate set by the user. + draw_progress_bar(-1, status); + } + void draw_progress_bar( + const rcutils_time_point_value_t & timestamp, + const PlayerStatus & status) + { + if (timestamp > 0) { + progress_current_time_secs_ = RCUTILS_NS_TO_S(static_cast(timestamp)); + progress_secs_from_start_ = progress_current_time_secs_ - starting_time_secs_; + } + std::ostringstream oss; + oss << + // Clear and print newlines + progress_bar_helper_clear_and_move_cursor_down_ << + // Print progress bar + "====== Playback Progress ======\n" << + "[" << std::setw(13) << std::fixed << std::setprecision(9) << progress_current_time_secs_ << + "] Duration " << std::setprecision(2) << progress_secs_from_start_ << + // Spaces at the end are used to clear any previous progress bar in case the new one is + // shorter, which can happen when the playback starts a new loop. + "/" << duration_secs_ << " [" << static_cast(status) << "] " << + // Go up to the beginning of the blank lines + progress_bar_helper_move_cursor_up_; + std::cout << oss.str() << std::flush; + } + +private: + rclcpp::Logger logger_; + double starting_time_secs_ = 0.0; + double duration_secs_ = 0.0; + std::string progress_bar_helper_clear_and_move_cursor_down_; + std::string progress_bar_helper_move_cursor_up_; + + // progress bar + bool enable_progress_bar_; + bool progress_bar_update_always_; + rcutils_duration_value_t progress_bar_update_period_; + std::chrono::steady_clock::time_point progress_bar_last_time_updated_{}; + int32_t progress_bar_separation_lines_ = 3; + double progress_secs_from_start_ = 0.0; + double progress_current_time_secs_ = 0.0; }; constexpr Player::callback_handle_t Player::invalid_callback_handle; @@ -129,7 +278,7 @@ class PlayerImpl /// \brief Set the playback rate. /// \return false if an invalid value was provided (<= 0). - virtual bool set_rate(double); + bool set_rate(double); /// \brief Playing next message from queue when in pause. /// \details This is blocking call and it will wait until next available message will be @@ -315,32 +464,6 @@ class PlayerImpl rcutils_time_point_value_t get_message_order_timestamp( const rosbag2_storage::SerializedBagMessageSharedPtr & message) const; - std::atomic_bool is_delayed_status_{false}; - - std::string progress_bar_helper_clear_and_move_cursor_down_; - std::string progress_bar_helper_move_cursor_up_; - - void print_progress_bar_help_str() const; - // Update progress bar with an input timestamp, - // taking into account the update rate set by the user. - // The function should be called for regular progress bar updates, for example - // after the recurrent publishing of the messages. - // Call update_progress_bar_check_rate function only where it cannot run - // contemporaneously in multiple threads, i.e. function calls are already protected by a mutex. - // To avoid locking overhead no new mutex inside the function is directly protecting - // the access to the class attribute progress_bar_last_time_updated_. - void update_progress_bar_check_rate( - const rcutils_time_point_value_t & timestamp, - const PlayerStatus & status); - // Update progress bar with the current playback timestamp, - // irrespective of the update rate set by the user. - // The function should be called for extraordinary progress bar updates, for example - // when a log message is printed and we want to 'redraw' the progress bar. - void update_progress_bar(const PlayerStatus & status) const; - void cout_progress_bar( - const rcutils_time_point_value_t & timestamp, - const PlayerStatus & status) const; - static constexpr double read_ahead_lower_bound_percentage_ = 0.9; static const std::chrono::milliseconds queue_read_wait_period_; std::atomic_bool cancel_wait_for_next_message_{false}; @@ -387,9 +510,6 @@ class PlayerImpl std::atomic_bool play_next_result_{false}; rcutils_time_point_value_t starting_time_; - rcutils_time_point_value_t ending_time_; - double starting_time_secs_; - double duration_secs_; // control services rclcpp::Service::SharedPtr srv_pause_; @@ -412,6 +532,8 @@ class PlayerImpl std::shared_ptr player_service_client_manager_; + std::unique_ptr progress_bar_; + BagMessageComparator get_bag_message_comparator(const MessageOrder & order); /// Comparator for SerializedBagMessageSharedPtr to order chronologically by recv_timestamp. @@ -435,12 +557,6 @@ class PlayerImpl return l->send_timestamp > r->send_timestamp; } } bag_message_chronological_send_timestamp_comparator; - - // progress bar - const bool enable_progress_bar_; - const bool progress_bar_update_always_; - const rcutils_duration_value_t progress_bar_update_period_; - std::chrono::steady_clock::time_point progress_bar_last_time_updated_{}; }; PlayerImpl::BagMessageComparator PlayerImpl::get_bag_message_comparator(const MessageOrder & order) @@ -467,12 +583,7 @@ PlayerImpl::PlayerImpl( play_options_(play_options), message_queue_(get_bag_message_comparator(play_options_.message_order)), keyboard_handler_(std::move(keyboard_handler)), - player_service_client_manager_(std::make_shared()), - enable_progress_bar_(play_options.progress_bar_update_rate != 0), - progress_bar_update_always_(play_options.progress_bar_update_rate < 0), - progress_bar_update_period_(play_options.progress_bar_update_rate != 0 ? - RCUTILS_S_TO_NS(1.0 / play_options.progress_bar_update_rate) : - std::numeric_limits::max()) + player_service_client_manager_(std::make_shared()) { for (auto & topic : play_options_.topics_to_filter) { topic = rclcpp::expand_topic_or_service_name( @@ -498,20 +609,10 @@ PlayerImpl::PlayerImpl( owner_->get_namespace(), false); } - std::ostringstream oss_clear_and_move_cursor_down; - for (int i = 0; i < play_options_.progress_bar_separation_lines; i++) { - oss_clear_and_move_cursor_down << "\033[2K\n"; - } - oss_clear_and_move_cursor_down << "\033[2K"; - progress_bar_helper_clear_and_move_cursor_down_ = oss_clear_and_move_cursor_down.str(); - std::ostringstream oss_move_cursor_up; - oss_move_cursor_up << "\033[" << play_options_.progress_bar_separation_lines + 1 << "F"; - progress_bar_helper_move_cursor_up_ = oss_move_cursor_up.str(); - { std::lock_guard lk(reader_mutex_); starting_time_ = std::numeric_limits::max(); - ending_time_ = std::numeric_limits::min(); + rcutils_time_point_value_t ending_time = std::numeric_limits::min(); for (const auto & [reader, storage_options] : readers_with_options_) { // keep readers open until player is destroyed reader->open(storage_options, {"", rmw_get_serialization_format()}); @@ -524,8 +625,8 @@ PlayerImpl::PlayerImpl( if (metadata_starting_time < starting_time_) { starting_time_ = metadata_starting_time; } - if (metadata_starting_time + metadata_bag_duration > ending_time_) { - ending_time_ = metadata_starting_time + metadata_bag_duration; + if (metadata_starting_time + metadata_bag_duration > ending_time) { + ending_time = metadata_starting_time + metadata_bag_duration; } } // If a non-default (positive) starting time offset is provided in PlayOptions, @@ -539,10 +640,12 @@ PlayerImpl::PlayerImpl( } else { starting_time_ += play_options_.start_offset; } - starting_time_secs_ = RCUTILS_NS_TO_S( - static_cast(std::min(starting_time_, ending_time_))); - duration_secs_ = RCUTILS_NS_TO_S( - std::max(static_cast(ending_time_ - starting_time_), 0.0)); + + progress_bar_ = std::make_unique( + owner_->get_logger(), starting_time_, ending_time, + play_options.progress_bar_update_rate, + play_options.progress_bar_separation_lines); + clock_ = std::make_unique( starting_time_, std::chrono::steady_clock::now, std::chrono::milliseconds{100}, play_options_.start_paused); @@ -553,7 +656,8 @@ PlayerImpl::PlayerImpl( } create_control_services(); add_keyboard_callbacks(); - print_progress_bar_help_str(); + progress_bar_->print_help_str(); + progress_bar_->update(PlayerStatus::STOPPED); } PlayerImpl::~PlayerImpl() @@ -576,13 +680,7 @@ PlayerImpl::~PlayerImpl() reader->close(); } } - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); - // arrange cursor position to be after the progress bar - if (enable_progress_bar_) { - std::ostringstream oss; - oss << "\033[" << play_options_.progress_bar_separation_lines + 1 << "B\n"; - std::cout << oss.str() << std::flush; - } + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); } const std::chrono::milliseconds @@ -606,7 +704,7 @@ bool PlayerImpl::play() RCLCPP_WARN_STREAM( owner_->get_logger(), "Trying to play() while in playback, dismissing request."); - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); return false; } } @@ -632,12 +730,10 @@ bool PlayerImpl::play() [&, delay]() { try { if (delay > rclcpp::Duration(0, 0)) { - is_delayed_status_ = true; RCLCPP_INFO_STREAM(owner_->get_logger(), "Sleep " << delay.nanoseconds() << " ns"); - update_progress_bar(PlayerStatus::DELAYED); + progress_bar_->update(PlayerStatus::DELAYED); std::chrono::nanoseconds delay_duration(delay.nanoseconds()); std::this_thread::sleep_for(delay_duration); - is_delayed_status_ = false; } do { { @@ -651,7 +747,7 @@ bool PlayerImpl::play() clock_publish_timer_->reset(); } - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); load_storage_content_ = true; storage_loading_future_ = std::async( @@ -683,7 +779,7 @@ bool PlayerImpl::play() ready_to_play_from_queue_cv_.notify_all(); } - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); // Wait for all published messages to be acknowledged. if (play_options_.wait_acked_timeout >= 0) { @@ -722,7 +818,7 @@ bool PlayerImpl::play() play_next_result_ = false; finished_play_next_cv_.notify_all(); } - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); }); return true; } @@ -759,7 +855,7 @@ void PlayerImpl::stop() cancel_wait_for_next_message_ = true; } - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); if (clock_->is_paused()) { // Wake up the clock in case it's in a sleep_until(time) call @@ -781,21 +877,21 @@ void PlayerImpl::pause() { clock_->pause(); RCLCPP_INFO_STREAM(owner_->get_logger(), "Pausing play."); - update_progress_bar(PlayerStatus::PAUSED); + progress_bar_->update(PlayerStatus::PAUSED); } void PlayerImpl::resume() { clock_->resume(); RCLCPP_INFO_STREAM(owner_->get_logger(), "Resuming play."); - update_progress_bar(PlayerStatus::RUNNING); + progress_bar_->update(PlayerStatus::RUNNING); } void PlayerImpl::toggle_paused() { // Note: Use upper level public API from owner class to facilitate unit tests owner_->is_paused() ? owner_->resume() : owner_->pause(); - update_progress_bar(owner_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + progress_bar_->update(owner_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); } bool PlayerImpl::is_paused() const @@ -816,7 +912,7 @@ bool PlayerImpl::set_rate(double rate) } else { RCLCPP_WARN_STREAM(owner_->get_logger(), "Failed to set rate to invalid value " << rate); } - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); return ok; } @@ -846,7 +942,7 @@ bool PlayerImpl::play_next() } if (!clock_->is_paused()) { RCLCPP_WARN_STREAM(owner_->get_logger(), "Called play next, but not in paused state."); - update_progress_bar(PlayerStatus::RUNNING); + progress_bar_->update(PlayerStatus::RUNNING); return false; } @@ -877,11 +973,11 @@ size_t PlayerImpl::burst(const size_t num_messages) { if (!clock_->is_paused()) { RCLCPP_WARN_STREAM(owner_->get_logger(), "Burst can only be used when in the paused state."); - update_progress_bar(PlayerStatus::RUNNING); + progress_bar_->update(PlayerStatus::RUNNING); return 0; } - update_progress_bar(PlayerStatus::BURST); + progress_bar_->update(PlayerStatus::BURST); uint64_t messages_played = 0; for (auto ii = 0u; ii < num_messages || num_messages == 0; ++ii) { @@ -893,7 +989,7 @@ size_t PlayerImpl::burst(const size_t num_messages) } RCLCPP_INFO_STREAM(owner_->get_logger(), "Burst " << messages_played << " messages."); - update_progress_bar(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); + progress_bar_->update(clock_->is_paused() ? PlayerStatus::PAUSED : PlayerStatus::RUNNING); return messages_played; } @@ -1151,9 +1247,15 @@ void PlayerImpl::play_messages_from_queue() finished_play_next_cv_.notify_all(); } } - // update_progress_bar_check_rate in this code section is protected + // Updating progress bar in this code section protected // by the mutex skip_message_in_main_play_loop_mutex_. - update_progress_bar_check_rate(message_ptr->recv_timestamp, PlayerStatus::RUNNING); + if (play_options_.message_order == MessageOrder::RECEIVED_TIMESTAMP) { + progress_bar_->update_with_limited_rate( + message_ptr->recv_timestamp, PlayerStatus::RUNNING); + } else if (play_options_.message_order == MessageOrder::SENT_TIMESTAMP) { + progress_bar_->update_with_limited_rate( + message_ptr->send_timestamp, PlayerStatus::RUNNING); + } } message_ptr = take_next_message_from_queue(); } @@ -1785,87 +1887,6 @@ const rosbag2_transport::PlayOptions & PlayerImpl::get_play_options() return play_options_; } -void PlayerImpl::print_progress_bar_help_str() const -{ - std::string help_str; - if (enable_progress_bar_) { - if (progress_bar_update_always_) { - help_str = "Progress bar enabled for every message."; - } else { - std::ostringstream oss; - oss << "Progress bar enabled at " << - play_options_.progress_bar_update_rate << " Hz"; - help_str = oss.str(); - } - RCLCPP_INFO_STREAM(owner_->get_logger(), help_str); - std::string help_str2 = "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed"; - RCLCPP_INFO_STREAM(owner_->get_logger(), help_str2); - } else { - help_str = "Progress bar disabled"; - RCLCPP_INFO_STREAM(owner_->get_logger(), help_str); - } -} - -void PlayerImpl::update_progress_bar_check_rate( - const rcutils_time_point_value_t & timestamp, - const PlayerStatus & status) -{ - if (!enable_progress_bar_) { - return; - } - - // If we are not updating the progress bar for every call, check if we should update it now - // based on the update rate set by the user - if (!progress_bar_update_always_) { - std::chrono::steady_clock::time_point steady_time_now = std::chrono::steady_clock::now(); - if (std::chrono::duration_cast( - steady_time_now - progress_bar_last_time_updated_).count() < progress_bar_update_period_) - { - return; - } - progress_bar_last_time_updated_ = steady_time_now; - } - - cout_progress_bar(timestamp, status); -} - -void PlayerImpl::update_progress_bar(const PlayerStatus & status) const -{ - if (!enable_progress_bar_) { - return; - } - - // Update progress bar irrespective of the update rate set by the user. - // Overwrite the input status if we are in a special case. - if (is_delayed_status_) { - cout_progress_bar(starting_time_, PlayerStatus::DELAYED); - } else { - cout_progress_bar(std::min(clock_->now(), ending_time_), status); - } -} - -void PlayerImpl::cout_progress_bar( - const rcutils_time_point_value_t & timestamp, - const PlayerStatus & status) const -{ - const double current_time_secs = RCUTILS_NS_TO_S(static_cast(timestamp)); - const double progress_secs = current_time_secs - starting_time_secs_; - std::ostringstream oss; - oss << - // Clear and print newlines - progress_bar_helper_clear_and_move_cursor_down_ << - // Print progress bar - "====== Playback Progress ======\n" << - "[" << std::setw(13) << std::fixed << std::setprecision(9) << current_time_secs << - "] Duration " << std::setprecision(2) << progress_secs << - // Spaces at the end are used to clear any previous progress bar in case the new one is shorter, - // which can happen when the playback starts a new loop. - "/" << duration_secs_ << " [" << static_cast(status) << "] " << - // Go up to the beginning of the blank lines - progress_bar_helper_move_cursor_up_; - std::cout << oss.str() << std::flush; -} - /////////////////////////////// // Player public interface From 681c1edbcba672bbfae276347819e1f341c40c91 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 25 Jan 2025 16:31:34 -0800 Subject: [PATCH 24/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 8c1a40f3a..ef594b9ac 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -81,18 +81,18 @@ rclcpp::QoS publisher_qos_for_topic( namespace rosbag2_transport { -enum class PlayerStatus : char -{ - BURST = 'B', - DELAYED = 'D', - PAUSED = 'P', - RUNNING = 'R', - STOPPED = 'S', -}; - class PlayerProgressBar { public: + enum class PlayerStatus : char + { + BURST = 'B', + DELAYED = 'D', + PAUSED = 'P', + RUNNING = 'R', + STOPPED = 'S', + }; + explicit PlayerProgressBar( rclcpp::Logger logger, rcutils_time_point_value_t starting_time, @@ -246,6 +246,7 @@ class PlayerImpl using callback_handle_t = Player::callback_handle_t; using play_msg_callback_t = Player::play_msg_callback_t; using reader_storage_options_pair_t = Player::reader_storage_options_pair_t; + using PlayerStatus = PlayerProgressBar::PlayerStatus; PlayerImpl( Player * owner, From 3a8301b68e4b78006fc4a9d2dd2e18622ddb82d6 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 25 Jan 2025 17:25:38 -0800 Subject: [PATCH 25/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 ef594b9ac..a08aecda6 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -39,6 +39,7 @@ #include "rosbag2_storage/qos.hpp" #include "rosbag2_transport/config_options_from_node_params.hpp" #include "rosbag2_transport/player_service_client.hpp" +#include "rosbag2_transport/player_progress_bar.hpp" #include "rosbag2_transport/reader_writer_factory.hpp" #include "logging.hpp" @@ -80,164 +81,6 @@ rclcpp::QoS publisher_qos_for_topic( namespace rosbag2_transport { - -class PlayerProgressBar -{ -public: - enum class PlayerStatus : char - { - BURST = 'B', - DELAYED = 'D', - PAUSED = 'P', - RUNNING = 'R', - STOPPED = 'S', - }; - - explicit PlayerProgressBar( - rclcpp::Logger logger, - rcutils_time_point_value_t starting_time, - rcutils_time_point_value_t ending_time, - int32_t progress_bar_update_rate = 3, - int32_t progress_bar_separation_lines = 3) - : logger_(std::move(logger)), - enable_progress_bar_(progress_bar_update_rate != 0), - progress_bar_update_always_(progress_bar_update_rate < 0), - progress_bar_update_period_(progress_bar_update_rate != 0 ? - RCUTILS_S_TO_NS(1.0 / progress_bar_update_rate) : - std::numeric_limits::max()), - progress_bar_separation_lines_(progress_bar_separation_lines) - { - starting_time_secs_ = RCUTILS_NS_TO_S( - static_cast(std::min(starting_time, ending_time))); - duration_secs_ = RCUTILS_NS_TO_S( - std::max(static_cast(ending_time - starting_time), 0.0)); - progress_secs_from_start_ = starting_time_secs_; - progress_current_time_secs_ = starting_time_secs_; - std::ostringstream oss_clear_and_move_cursor_down; - for (int i = 0; i < progress_bar_separation_lines; i++) { - oss_clear_and_move_cursor_down << "\033[2K\n"; - } - oss_clear_and_move_cursor_down << "\033[2K"; - progress_bar_helper_clear_and_move_cursor_down_ = oss_clear_and_move_cursor_down.str(); - std::ostringstream oss_move_cursor_up; - oss_move_cursor_up << "\033[" << progress_bar_separation_lines + 1 << "F"; - progress_bar_helper_move_cursor_up_ = oss_move_cursor_up.str(); - } - - ~PlayerProgressBar() - { - // arrange cursor position to be after the progress bar - if (enable_progress_bar_) { - std::ostringstream oss; - oss << "\033[" << progress_bar_separation_lines_ + 1 << "B\n"; - std::cout << oss.str() << std::flush; - } - } - void print_help_str() const - { - std::string help_str; - if (enable_progress_bar_) { - if (progress_bar_update_always_) { - help_str = "Progress bar enabled for every message."; - } else { - std::ostringstream oss; - oss << "Progress bar enabled at " << - (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz"; - help_str = oss.str(); - } - RCLCPP_INFO_STREAM(logger_, help_str); - std::string help_str2 = - "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped"; - RCLCPP_INFO_STREAM(logger_, help_str2); - } else { - help_str = "Progress bar disabled"; - RCLCPP_INFO_STREAM(logger_, help_str); - } - } - - // Update progress bar with an input timestamp, - // taking into account the update rate set by the user. - // The function should be called for regular progress bar updates, for example - // after the recurrent publishing of the messages. - // Call update_progress_bar_check_rate function only where it cannot run - // contemporaneously in multiple threads, i.e. function calls are already protected by a mutex. - // To avoid locking overhead no new mutex inside the function is directly protecting - // the access to the class attribute progress_bar_last_time_updated_. - void update_with_limited_rate( - const rcutils_time_point_value_t & timestamp, - const PlayerStatus & status) - { - if (!enable_progress_bar_) { - return; - } - - // If we are not updating the progress bar for every call, check if we should update it now - // based on the update rate set by the user - if (!progress_bar_update_always_) { - std::chrono::steady_clock::time_point steady_time_now = std::chrono::steady_clock::now(); - if (std::chrono::duration_cast( - steady_time_now - progress_bar_last_time_updated_).count() < progress_bar_update_period_) - { - return; - } - progress_bar_last_time_updated_ = steady_time_now; - } - - draw_progress_bar(timestamp, status); - } - // Update progress bar with the current playback timestamp, - // irrespective of the update rate set by the user. - // The function should be called for extraordinary progress bar updates, for example - // when a log message is printed and we want to 'redraw' the progress bar. - void update(const PlayerStatus & status) - { - if (!enable_progress_bar_) { - return; - } - // Update progress bar irrespective of the update rate set by the user. - draw_progress_bar(-1, status); - } - void draw_progress_bar( - const rcutils_time_point_value_t & timestamp, - const PlayerStatus & status) - { - if (timestamp > 0) { - progress_current_time_secs_ = RCUTILS_NS_TO_S(static_cast(timestamp)); - progress_secs_from_start_ = progress_current_time_secs_ - starting_time_secs_; - } - std::ostringstream oss; - oss << - // Clear and print newlines - progress_bar_helper_clear_and_move_cursor_down_ << - // Print progress bar - "====== Playback Progress ======\n" << - "[" << std::setw(13) << std::fixed << std::setprecision(9) << progress_current_time_secs_ << - "] Duration " << std::setprecision(2) << progress_secs_from_start_ << - // Spaces at the end are used to clear any previous progress bar in case the new one is - // shorter, which can happen when the playback starts a new loop. - "/" << duration_secs_ << " [" << static_cast(status) << "] " << - // Go up to the beginning of the blank lines - progress_bar_helper_move_cursor_up_; - std::cout << oss.str() << std::flush; - } - -private: - rclcpp::Logger logger_; - double starting_time_secs_ = 0.0; - double duration_secs_ = 0.0; - std::string progress_bar_helper_clear_and_move_cursor_down_; - std::string progress_bar_helper_move_cursor_up_; - - // progress bar - bool enable_progress_bar_; - bool progress_bar_update_always_; - rcutils_duration_value_t progress_bar_update_period_; - std::chrono::steady_clock::time_point progress_bar_last_time_updated_{}; - int32_t progress_bar_separation_lines_ = 3; - double progress_secs_from_start_ = 0.0; - double progress_current_time_secs_ = 0.0; -}; - constexpr Player::callback_handle_t Player::invalid_callback_handle; class PlayerImpl diff --git a/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp new file mode 100644 index 000000000..2dc495c11 --- /dev/null +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp @@ -0,0 +1,156 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "rclcpp/logging.hpp" +#include "rosbag2_transport/player_progress_bar.hpp" + +namespace rosbag2_transport +{ + +PlayerProgressBar::PlayerProgressBar( + rclcpp::Logger logger, + rcutils_time_point_value_t starting_time, + rcutils_time_point_value_t ending_time, + int32_t progress_bar_update_rate, + int32_t progress_bar_separation_lines) +: logger_(std::move(logger)), + enable_progress_bar_(progress_bar_update_rate != 0), + progress_bar_update_always_(progress_bar_update_rate < 0), + progress_bar_update_period_(progress_bar_update_rate != 0 ? + RCUTILS_S_TO_NS(1.0 / progress_bar_update_rate) : + std::numeric_limits::max()), + progress_bar_separation_lines_(progress_bar_separation_lines) +{ + starting_time_secs_ = RCUTILS_NS_TO_S( + static_cast(std::min(starting_time, ending_time))); + duration_secs_ = RCUTILS_NS_TO_S( + std::max(static_cast(ending_time - starting_time), 0.0)); + progress_secs_from_start_ = starting_time_secs_; + progress_current_time_secs_ = starting_time_secs_; + std::ostringstream oss_clear_and_move_cursor_down; + for (int i = 0; i < progress_bar_separation_lines; i++) { + oss_clear_and_move_cursor_down << "\033[2K\n"; + } + oss_clear_and_move_cursor_down << "\033[2K"; + progress_bar_helper_clear_and_move_cursor_down_ = oss_clear_and_move_cursor_down.str(); + std::ostringstream oss_move_cursor_up; + oss_move_cursor_up << "\033[" << progress_bar_separation_lines + 1 << "F"; + progress_bar_helper_move_cursor_up_ = oss_move_cursor_up.str(); +} + +PlayerProgressBar::~PlayerProgressBar() +{ + // arrange cursor position to be after the progress bar + if (enable_progress_bar_) { + std::ostringstream oss; + oss << "\033[" << progress_bar_separation_lines_ + 1 << "B\n"; + std::cout << oss.str() << std::flush; + } +} + +void PlayerProgressBar::print_help_str() const +{ + std::string help_str; + if (enable_progress_bar_) { + if (progress_bar_update_always_) { + help_str = "Progress bar enabled for every message."; + } else { + std::ostringstream oss; + oss << "Progress bar enabled at " << + (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz"; + help_str = oss.str(); + } + RCLCPP_INFO_STREAM(logger_, help_str); + std::string help_str2 = + "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped"; + RCLCPP_INFO_STREAM(logger_, help_str2); + } else { + help_str = "Progress bar disabled"; + RCLCPP_INFO_STREAM(logger_, help_str); + } +} + +// Update progress bar with an input timestamp, +// taking into account the update rate set by the user. +// The function should be called for regular progress bar updates, for example +// after the recurrent publishing of the messages. +// Call update_progress_bar_check_rate function only where it cannot run +// contemporaneously in multiple threads, i.e. function calls are already protected by a mutex. +// To avoid locking overhead no new mutex inside the function is directly protecting +// the access to the class attribute progress_bar_last_time_updated_. +void PlayerProgressBar::update_with_limited_rate( + const rcutils_time_point_value_t & timestamp, + const PlayerStatus & status) +{ + if (!enable_progress_bar_) { + return; + } + + // If we are not updating the progress bar for every call, check if we should update it now + // based on the update rate set by the user + if (!progress_bar_update_always_) { + std::chrono::steady_clock::time_point steady_time_now = std::chrono::steady_clock::now(); + if (std::chrono::duration_cast( + steady_time_now - progress_bar_last_time_updated_).count() < progress_bar_update_period_) + { + return; + } + progress_bar_last_time_updated_ = steady_time_now; + } + + draw_progress_bar(timestamp, status); +} +// Update progress bar with the current playback timestamp, +// irrespective of the update rate set by the user. +// The function should be called for extraordinary progress bar updates, for example +// when a log message is printed and we want to 'redraw' the progress bar. +void PlayerProgressBar::update(const PlayerStatus & status) +{ + if (!enable_progress_bar_) { + return; + } + // Update progress bar irrespective of the update rate set by the user. + draw_progress_bar(-1, status); +} +void PlayerProgressBar::draw_progress_bar( + const rcutils_time_point_value_t & timestamp, + const PlayerStatus & status) +{ + if (timestamp > 0) { + progress_current_time_secs_ = RCUTILS_NS_TO_S(static_cast(timestamp)); + progress_secs_from_start_ = progress_current_time_secs_ - starting_time_secs_; + } + std::ostringstream oss; + oss << + // Clear and print newlines + progress_bar_helper_clear_and_move_cursor_down_ << + // Print progress bar + "====== Playback Progress ======\n" << + "[" << std::setw(13) << std::fixed << std::setprecision(9) << progress_current_time_secs_ << + "] Duration " << std::setprecision(2) << progress_secs_from_start_ << + // Spaces at the end are used to clear any previous progress bar in case the new one is + // shorter, which can happen when the playback starts a new loop. + "/" << duration_secs_ << " [" << static_cast(status) << "] " << + // Go up to the beginning of the blank lines + progress_bar_helper_move_cursor_up_; + std::cout << oss.str() << std::flush; +} + +} // namespace rosbag2_transport From bb8dc2a2a73c0132fc05b92630b765b91ccb5610 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 25 Jan 2025 18:40:18 -0800 Subject: [PATCH 26/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 879839754efdf6c63978e4b2661c82de56104df6 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 00:19:49 -0800 Subject: [PATCH 27/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 a08aecda6..66f000ff8 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -486,7 +486,7 @@ PlayerImpl::PlayerImpl( } progress_bar_ = std::make_unique( - owner_->get_logger(), starting_time_, ending_time, + std::cout, starting_time_, ending_time, play_options.progress_bar_update_rate, play_options.progress_bar_separation_lines); diff --git a/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp index 16ef51fbf..7c3377dc3 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar.cpp @@ -21,14 +21,14 @@ namespace rosbag2_transport { PlayerProgressBar::PlayerProgressBar( - rclcpp::Logger logger, + std::ostream & output_stream, rcutils_time_point_value_t starting_time, rcutils_time_point_value_t ending_time, int32_t progress_bar_update_rate, int32_t progress_bar_separation_lines) { pimpl_ = std::make_unique( - std::move(logger), starting_time, ending_time, + output_stream, starting_time, ending_time, progress_bar_update_rate, progress_bar_separation_lines); } diff --git a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp index 626a85e7a..2ca2ed5fa 100644 --- a/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp +++ b/rosbag2_transport/src/rosbag2_transport/player_progress_bar_impl.hpp @@ -35,12 +35,12 @@ class PlayerProgressBarImpl { using PlayerStatus = PlayerProgressBar::PlayerStatus; PlayerProgressBarImpl( - rclcpp::Logger logger, + std::ostream & output_stream, rcutils_time_point_value_t starting_time, rcutils_time_point_value_t ending_time, int32_t progress_bar_update_rate, int32_t progress_bar_separation_lines) - : logger_(std::move(logger)), + : o_stream_(output_stream), enable_progress_bar_(progress_bar_update_rate != 0), progress_bar_update_always_(progress_bar_update_rate < 0), progress_bar_update_period_(progress_bar_update_rate != 0 ? @@ -71,30 +71,25 @@ class PlayerProgressBarImpl { if (enable_progress_bar_) { std::ostringstream oss; oss << "\033[" << progress_bar_separation_lines_ + 1 << "B\n"; - std::cout << oss.str() << std::flush; + o_stream_ << oss.str() << std::flush; } } void print_help_str() const { - std::string help_str; + std::ostringstream oss; if (enable_progress_bar_) { if (progress_bar_update_always_) { - help_str = "Progress bar enabled for every message."; + oss << "Progress bar enabled for every message.\n"; } else { - std::ostringstream oss; oss << "Progress bar enabled at " << - (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz"; - help_str = oss.str(); + (1.0 / (progress_bar_update_period_ / (1000LL * 1000LL))) / 1000LL << " Hz.\n"; } - RCLCPP_INFO_STREAM(logger_, help_str); - std::string help_str2 = - "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped"; - RCLCPP_INFO_STREAM(logger_, help_str2); + oss << "Progress bar [?]: [R]unning, [P]aused, [B]urst, [D]elayed, [S]topped\n"; } else { - help_str = "Progress bar disabled"; - RCLCPP_INFO_STREAM(logger_, help_str); + oss << "Progress bar disabled.\n"; } + o_stream_ << oss.str() << std::flush; } void update_with_limited_rate( @@ -129,9 +124,7 @@ class PlayerProgressBarImpl { draw_progress_bar(-1, status); } - void draw_progress_bar( - const rcutils_time_point_value_t & timestamp, - const PlayerStatus & status) + void draw_progress_bar(const rcutils_time_point_value_t & timestamp, const PlayerStatus & status) { if (timestamp > 0) { progress_current_time_secs_ = RCUTILS_NS_TO_S(static_cast(timestamp)); @@ -150,11 +143,11 @@ class PlayerProgressBarImpl { "/" << duration_secs_ << " [" << static_cast(status) << "] " << // Go up to the beginning of the blank lines progress_bar_helper_move_cursor_up_; - std::cout << oss.str() << std::flush; + o_stream_ << oss.str() << std::flush; } private: - rclcpp::Logger logger_; + std::ostream & o_stream_; double starting_time_secs_ = 0.0; double duration_secs_ = 0.0; std::string progress_bar_helper_clear_and_move_cursor_down_; From 8e4504242ff29aa10c3f6215f317a9e7c399542d Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 00:25:49 -0800 Subject: [PATCH 28/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 295e137bf2a5114d96d175faebbe2a0586d43e89 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 00:34:18 -0800 Subject: [PATCH 29/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 36e07c5b7db5797a7a954f48fefd085798ea4474 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 01:13:45 -0800 Subject: [PATCH 30/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 370987966b5c2dfc46aee7c40bd6fbf4e3bfcc14 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 11:32:05 -0800 Subject: [PATCH 31/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 2f6e66b7b0d41d34ab42bfed67960d39e9724f46 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 26 Jan 2025 12:16:58 -0800 Subject: [PATCH 32/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 74bc9152ec9cc673c7e0d4517c2b114a6e67c39c Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Thu, 30 Jan 2025 00:19:04 +0000 Subject: [PATCH 33/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) {