Add playback progress output to rosbag2 player

rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp

@@ -39,8 +39,8 @@ class TimeControllerClock : public PlayerClock
    * \param now_fn: Function used to get the current steady time
    *   defaults to std::chrono::steady_clock::now
    *   Used to control for unit testing, or for specialized needs
-   * \param sleep_time_while_paused: Amount of time to sleep in `sleep_until` when the clock
-   *   is paused. Allows the caller to spin at a defined rate while receiving `false`
+  * \param sleep_time_while_paused: Maximum interval used by `sleep_until` while the clock
+  *   is paused or waiting. Allows the caller to spin at a defined rate while receiving `false`
    * \param paused: Start the clock paused
    */
   ROSBAG2_CPP_PUBLIC

rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp

@@ -175,8 +175,10 @@ bool TimeControllerClock::sleep_until(rcutils_time_point_value_t until)
     } else {
       const auto steady_until = impl_->ros_to_steady(until);
       // wait only if necessary for performance
-      if (steady_until > impl_->now_fn()) {
-        impl_->cv.wait_until(lock, steady_until);
+      const auto steady_now = impl_->now_fn();
+      if (steady_until > steady_now) {
+        const auto wakeup_time = std::min(steady_until, steady_now + impl_->sleep_time_while_paused);
+        impl_->cv.wait_until(lock, wakeup_time);
       }
     }
     if (impl_->paused) {

rosbag2_transport/include/rosbag2_transport/player.hpp

@@ -204,6 +204,7 @@ class Player : public rclcpp::Node
   bool publish_message(rosbag2_storage::SerializedBagMessageSharedPtr message);
   static constexpr double read_ahead_lower_bound_percentage_ = 0.9;
   static const std::chrono::milliseconds queue_read_wait_period_;
+  static const std::chrono::milliseconds progress_bar_update_period_;
   std::atomic_bool cancel_wait_for_next_message_{false};
 
   std::mutex reader_mutex_;
@@ -216,6 +217,10 @@ class Player : public rclcpp::Node
   void add_keyboard_callbacks();
 
   void create_control_services();
+  void print_playback_progress(
+    rcutils_time_point_value_t current_time,
+    const char * state,
+    bool force = false);
 
   rosbag2_storage::StorageOptions storage_options_;
   rosbag2_transport::PlayOptions play_options_;
@@ -229,6 +234,9 @@ class Player : public rclcpp::Node
     skip_message_in_main_play_loop_mutex_) = false;
 
   rcutils_time_point_value_t starting_time_;
+  rcutils_time_point_value_t bag_duration_ns_ = 0;
+  rcutils_time_point_value_t last_logged_playback_timestamp_ = -1;
+  bool has_playback_progress_output_ = false;
 
   // control services
   rclcpp::Service<rosbag2_interfaces::srv::Pause>::SharedPtr srv_pause_;

rosbag2_transport/src/rosbag2_transport/player.cpp

@@ -16,6 +16,7 @@
 
 #include <algorithm>
 #include <chrono>
+#include <cstdio>
 #include <memory>
 #include <queue>
 #include <string>
@@ -147,6 +148,7 @@ Player::Player(
     auto metadata = reader_->get_metadata();
     starting_time_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
       metadata.starting_time.time_since_epoch()).count();
+    bag_duration_ns_ = metadata.duration.count();
     // 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) {
@@ -157,6 +159,8 @@ Player::Player(
           ". Negative start offset ignored.");
     } else {
       starting_time_ += play_options_.start_offset;
+      bag_duration_ns_ = std::max<rcutils_time_point_value_t>(
+        0, bag_duration_ns_ - play_options_.start_offset);
     }
     clock_ = std::make_unique<rosbag2_cpp::TimeControllerClock>(
       starting_time_, std::chrono::steady_clock::now,
@@ -186,6 +190,9 @@ Player::~Player()
 const std::chrono::milliseconds
 Player::queue_read_wait_period_ = std::chrono::milliseconds(100);
 
+const std::chrono::milliseconds
+Player::progress_bar_update_period_ = std::chrono::milliseconds(100);
+
 bool Player::is_storage_completely_loaded() const
 {
   if (storage_loading_future_.valid() &&
@@ -198,6 +205,8 @@ bool Player::is_storage_completely_loaded() const
 
 void Player::play()
 {
+  last_logged_playback_timestamp_ = -1;
+  has_playback_progress_output_ = false;
   rclcpp::Duration delay(0, 0);
   if (play_options_.delay >= rclcpp::Duration(0, 0)) {
     delay = play_options_.delay;
@@ -219,6 +228,7 @@ void Player::play()
         reader_->seek(starting_time_);
         clock_->jump(starting_time_);
       }
+      print_playback_progress(starting_time_, is_paused() ? "PAUSED" : "RUNNING", true);
       storage_loading_future_ = std::async(std::launch::async, [this]() {load_storage_content();});
       wait_for_filled_queue();
       play_messages_from_queue();
@@ -235,6 +245,11 @@ void Player::play()
   is_ready_to_play_from_queue_ = false;
   ready_to_play_from_queue_cv_.notify_all();
 
+  if (has_playback_progress_output_) {
+    std::fputc('\n', stdout);
+    std::fflush(stdout);
+  }
+
   // Wait for all published messages to be acknowledged.
   if (play_options_.wait_acked_timeout >= 0) {
     std::chrono::milliseconds timeout(play_options_.wait_acked_timeout);
@@ -264,12 +279,14 @@ void Player::play()
 void Player::pause()
 {
   clock_->pause();
+  print_playback_progress(clock_->now(), "PAUSED", true);
   RCLCPP_INFO_STREAM(get_logger(), "Pausing play.");
 }
 
 void Player::resume()
 {
   clock_->resume();
+  print_playback_progress(clock_->now(), "RUNNING", true);
   RCLCPP_INFO_STREAM(get_logger(), "Resuming play.");
 }
 
@@ -352,6 +369,9 @@ bool Player::play_next()
     {
       next_message_published = publish_message(message_ptr);
       clock_->jump(message_ptr->time_stamp);
+      if (next_message_published) {
+        print_playback_progress(message_ptr->time_stamp, "PAUSED", true);
+      }
     }
     message_queue_.pop();
     message_ptr = peek_next_message_from_queue();
@@ -397,6 +417,7 @@ void Player::seek(rcutils_time_point_value_t time_point)
     while (message_queue_.pop()) {}
     reader_->seek(time_point);
     clock_->jump(time_point);
+    print_playback_progress(time_point, is_paused() ? "PAUSED" : "RUNNING", true);
     // Restart queuing thread if it has finished running (previously reached end of bag),
     // otherwise, queueing should continue automatically after releasing mutex
     if (is_storage_completely_loaded() && rclcpp::ok()) {
@@ -464,6 +485,7 @@ void Player::play_messages_from_queue()
     // Do not move on until sleep_until returns true
     // It will always sleep, so this is not a tight busy loop on pause
     while (rclcpp::ok() && !clock_->sleep_until(message_ptr->time_stamp)) {
+      print_playback_progress(clock_->now(), is_paused() ? "PAUSED" : "RUNNING");
       if (std::atomic_exchange(&cancel_wait_for_next_message_, false)) {
         break;
       }
@@ -476,11 +498,14 @@ void Player::play_messages_from_queue()
         message_ptr = peek_next_message_from_queue();
         continue;
       }
-      publish_message(message_ptr);
+      if (publish_message(message_ptr)) {
+        print_playback_progress(message_ptr->time_stamp, "RUNNING");
+      }
     }
     message_queue_.pop();
     message_ptr = peek_next_message_from_queue();
   }
+  print_playback_progress(clock_->now(), is_paused() ? "PAUSED" : "RUNNING", true);
   // while we're in pause state, make sure we don't return
   // if we happen to be at the end of queue
   while (is_paused() && rclcpp::ok()) {
@@ -594,6 +619,39 @@ bool Player::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr mess
   return message_published;
 }
 
+void Player::print_playback_progress(
+  rcutils_time_point_value_t current_time,
+  const char * state,
+  bool force)
+{
+  if (!force) {
+    if (last_logged_playback_timestamp_ >= 0 &&
+      current_time - last_logged_playback_timestamp_ <
+      std::chrono::duration_cast<std::chrono::nanoseconds>(progress_bar_update_period_).count())
+    {
+      return;
+    }
+  }
+
+  auto clamped_time = std::max(current_time, starting_time_);
+  auto playback_duration_ns = std::max<rcutils_time_point_value_t>(0, clamped_time - starting_time_);
+  auto playback_duration_s = RCUTILS_NS_TO_S(static_cast<double>(playback_duration_ns));
+  auto bag_duration_s = RCUTILS_NS_TO_S(static_cast<double>(bag_duration_ns_));
+  auto bag_time_s = RCUTILS_NS_TO_S(static_cast<double>(clamped_time));
+
+  std::fprintf(
+    stdout,
+    "\r [%-7s]  Bag Time: %13.6f   Duration: %.6f / %.6f               ",
+    state,
+    bag_time_s,
+    playback_duration_s,
+    bag_duration_s);
+  std::fflush(stdout);
+
+  last_logged_playback_timestamp_ = clamped_time;
+  has_playback_progress_output_ = true;
+}
+
 void Player::add_key_callback(
   KeyboardHandler::KeyCode key,
   const std::function<void()> & cb,

rosbag2_transport/test/rosbag2_transport/mock_sequential_reader.hpp

@@ -111,6 +111,11 @@ class MockSequentialReader : public rosbag2_cpp::reader_interfaces::BaseReaderIn
     topics_ = std::move(topics);
   }
 
+  void set_metadata(const rosbag2_storage::BagMetadata & metadata)
+  {
+    metadata_ = metadata;
+  }
+
   size_t max_messages_per_file() const
   {
     return max_messages_per_file_;

rosbag2_transport/test/rosbag2_transport/test_play.cpp

@@ -109,6 +109,42 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics)
           ElementsAre(40.0f, 2.0f, 0.0f)))));
 }
 
+TEST_F(RosBag2PlayTestFixture, playback_progress_is_printed_to_stdout)
+{
+  auto primitive_message = get_messages_basic_types()[0];
+  primitive_message->int32_value = 42;
+
+  auto topic_types = std::vector<rosbag2_storage::TopicMetadata>{
+    {"topic1", "test_msgs/BasicTypes", "", ""},
+  };
+
+  std::vector<std::shared_ptr<rosbag2_storage::SerializedBagMessage>> messages = {
+    serialize_test_message("topic1", 1000000000, primitive_message),
+    serialize_test_message("topic1", 2000000000, primitive_message),
+  };
+
+  rosbag2_storage::BagMetadata metadata;
+  metadata.starting_time = std::chrono::high_resolution_clock::time_point(std::chrono::seconds(1));
+  metadata.duration = std::chrono::seconds(1);
+  metadata.message_count = messages.size();
+
+  auto prepared_mock_reader = std::make_unique<MockSequentialReader>();
+  prepared_mock_reader->prepare(messages, topic_types);
+  prepared_mock_reader->set_metadata(metadata);
+  auto reader = std::make_unique<rosbag2_cpp::Reader>(std::move(prepared_mock_reader));
+
+  auto player = std::make_shared<rosbag2_transport::Player>(
+    std::move(reader), storage_options_, play_options_);
+
+  testing::internal::CaptureStdout();
+  player->play();
+  const auto output = testing::internal::GetCapturedStdout();
+
+  EXPECT_THAT(output, HasSubstr("[RUNNING]"));
+  EXPECT_THAT(output, HasSubstr("Bag Time:"));
+  EXPECT_THAT(output, HasSubstr("Duration: 1.000000 / 1.000000"));
+}
+
 TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics_with_unknown_type)
 {
   auto primitive_message1 = get_messages_basic_types()[0];