Start adding progress-bar for ros2 bag play

Signed-off-by: Nicola Loi <[email protected]>

Author: nicolaloi

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:

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]

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)

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

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 <mutex>
+#include <utility>
+#include <string>
+
+#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<double>(bag_starting_time))),
+    bag_duration_secs_(RCUTILS_NS_TO_S(static_cast<double>(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<double>(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_

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<KeyboardHandler::callback_handle_t> keyboard_callbacks_;
 
   std::shared_ptr<PlayerServiceClientManager> player_service_client_manager_;
+
+  // progress bar
+  std::unique_ptr<PlayerProgressBar> 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<std::chrono::nanoseconds>(
       metadata.starting_time.time_since_epoch()).count();
     // If a non-default (positive) starting time offset is provided in PlayOptions,
@@ -405,17 +410,21 @@ PlayerImpl::PlayerImpl(
           ". Negative start offset ignored.");
     } else {
       starting_time_ += play_options_.start_offset;
+      bag_duration -= play_options_.start_offset;
     }
     clock_ = std::make_unique<rosbag2_cpp::TimeControllerClock>(
       starting_time_, std::chrono::steady_clock::now,
       std::chrono::milliseconds{100}, play_options_.start_paused);
+    progress_bar_ = std::make_unique<PlayerProgressBar>(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();
     configure_play_until_timestamp();
   }
   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();