From 616df0d3d93f8ea777c668cdc7a34342419f7be3 Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Tue, 15 Oct 2024 22:34:34 +0200 Subject: [PATCH 1/5] 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 2/5] 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 3/5] 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 4/5] 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 5/5] 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; } ///////////////////////////////