Add wait for player to be ready for playback in Player::play_next() method (ros2#814)

Signed-off-by: Michael Orlov <[email protected]>

Author: morlov-apexai

rosbag2_transport/include/rosbag2_transport/player.hpp

@@ -118,13 +118,14 @@ class Player : public rclcpp::Node
   /// 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::play() has been started, player in pause mode and successfully
-  /// played next message, otherwise false.
+  /// \return true if player in pause mode and successfully played next message, otherwise false.
   ROSBAG2_TRANSPORT_PUBLIC
   bool play_next();
 
 protected:
-  std::atomic<bool> playing_messages_from_queue_{false};
+  bool is_ready_to_play_from_queue_{false};
+  std::mutex ready_to_play_from_queue_mutex_;
+  std::condition_variable ready_to_play_from_queue_cv_;
   rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_publisher_;
   std::unordered_map<std::string, std::shared_ptr<rclcpp::GenericPublisher>> publishers_;
 
@@ -151,7 +152,6 @@ class Player : public rclcpp::Node
   std::mutex skip_message_in_main_play_loop_mutex_;
   bool skip_message_in_main_play_loop_ RCPPUTILS_TSA_GUARDED_BY
     (skip_message_in_main_play_loop_mutex_) = false;
-  std::atomic_bool is_in_play_{false};
 
   rclcpp::Service<rosbag2_interfaces::srv::Pause>::SharedPtr srv_pause_;
   rclcpp::Service<rosbag2_interfaces::srv::Resume>::SharedPtr srv_resume_;

rosbag2_transport/src/rosbag2_transport/player.cpp

@@ -212,16 +212,12 @@ bool Player::is_storage_completely_loaded() const
 
 void Player::play()
 {
-  is_in_play_ = true;
-
   float delay;
   if (play_options_.delay >= 0.0) {
     delay = play_options_.delay;
   } else {
     RCLCPP_WARN(
-      this->get_logger(),
-      "Invalid delay value: %f. Delay is disabled.",
-      play_options_.delay);
+      this->get_logger(), "Invalid delay value: %f. Delay is disabled.", play_options_.delay);
     delay = 0.0;
   }
 
@@ -237,18 +233,28 @@ void Player::play()
         reader_->get_metadata().starting_time.time_since_epoch()).count();
       clock_->jump(starting_time);
 
-      storage_loading_future_ = std::async(
-        std::launch::async,
-        [this]() {load_storage_content();});
+      storage_loading_future_ = std::async(std::launch::async, [this]() {load_storage_content();});
 
       wait_for_filled_queue();
+      {  // Notify play_next() that we are ready for playback
+        std::lock_guard<std::mutex> lk(ready_to_play_from_queue_mutex_);
+        is_ready_to_play_from_queue_ = true;
+        ready_to_play_from_queue_cv_.notify_all();
+      }
       play_messages_from_queue();
+      {
+        std::lock_guard<std::mutex> lk(ready_to_play_from_queue_mutex_);
+        is_ready_to_play_from_queue_ = false;
+        ready_to_play_from_queue_cv_.notify_all();
+      }
       reader_->close();
     } while (rclcpp::ok() && play_options_.loop);
   } catch (std::runtime_error & e) {
     RCLCPP_ERROR(this->get_logger(), "Failed to play: %s", e.what());
   }
-  is_in_play_ = false;
+  std::lock_guard<std::mutex> lk(ready_to_play_from_queue_mutex_);
+  is_ready_to_play_from_queue_ = false;
+  ready_to_play_from_queue_cv_.notify_all();
 }
 
 void Player::pause()
@@ -299,14 +305,19 @@ rosbag2_storage::SerializedBagMessageSharedPtr * Player::peek_next_message_from_
 
 bool Player::play_next()
 {
-  // Temporary take over playback from play_messages_from_queue()
-  std::lock_guard<std::mutex> lk(skip_message_in_main_play_loop_mutex_);
-
-  if (!clock_->is_paused() || !is_in_play_) {
+  if (!clock_->is_paused()) {
     return false;
   }
 
+  // Temporary take over playback from play_messages_from_queue()
+  std::lock_guard<std::mutex> main_play_loop_lk(skip_message_in_main_play_loop_mutex_);
   skip_message_in_main_play_loop_ = true;
+  // Wait for player to be ready for playback messages from queue i.e. wait for Player:play() to
+  // be called if not yet and queue to be filled with messages.
+  {
+    std::unique_lock<std::mutex> lk(ready_to_play_from_queue_mutex_);
+    ready_to_play_from_queue_cv_.wait(lk, [this] {return is_ready_to_play_from_queue_;});
+  }
   rosbag2_storage::SerializedBagMessageSharedPtr * message_ptr = peek_next_message_from_queue();
 
   bool next_message_published = false;
@@ -361,7 +372,6 @@ void Player::enqueue_up_to_boundary(uint64_t boundary)
 
 void Player::play_messages_from_queue()
 {
-  playing_messages_from_queue_ = true;
   // Note: We need to use message_queue_.peek() instead of message_queue_.try_dequeue(message)
   // to support play_next() API logic.
   rosbag2_storage::SerializedBagMessageSharedPtr * message_ptr = peek_next_message_from_queue();
@@ -386,7 +396,6 @@ void Player::play_messages_from_queue()
       message_ptr = peek_next_message_from_queue();
     }
   }
-  playing_messages_from_queue_ = false;
 }
 
 void Player::prepare_publishers()

rosbag2_transport/test/rosbag2_transport/test_play_next.cpp

@@ -51,9 +51,8 @@ class MockPlayer : public rosbag2_transport::Player
 
   void wait_for_playback_to_start()
   {
-    while (!playing_messages_from_queue_) {
-      std::this_thread::sleep_for(std::chrono::milliseconds(10));
-    }
+    std::unique_lock<std::mutex> lk(ready_to_play_from_queue_mutex_);
+    ready_to_play_from_queue_cv_.wait(lk, [this] {return is_ready_to_play_from_queue_;});
   }
 };
 
@@ -74,10 +73,8 @@ TEST_F(RosBag2PlayTestFixture, play_next_with_false_preconditions) {
 
   ASSERT_FALSE(player->is_paused());
   ASSERT_FALSE(player->play_next());
-
   player->pause();
   ASSERT_TRUE(player->is_paused());
-  ASSERT_FALSE(player->play_next());
 }
 
 TEST_F(RosBag2PlayTestFixture, play_next_playing_all_messages_without_delays) {
@@ -167,7 +164,6 @@ TEST_F(RosBag2PlayTestFixture, play_next_playing_one_by_one_messages_with_the_sa
   ASSERT_TRUE(player->is_paused());
 
   auto player_future = std::async(std::launch::async, [&player]() -> void {player->play();});
-  player->wait_for_playback_to_start();
 
   ASSERT_TRUE(player->is_paused());
   ASSERT_TRUE(player->play_next());
@@ -221,7 +217,6 @@ TEST_F(RosBag2PlayTestFixture, play_respect_messages_timing_after_play_next) {
   ASSERT_TRUE(player->is_paused());
 
   auto player_future = std::async(std::launch::async, [&player]() -> void {player->play();});
-  player->wait_for_playback_to_start();
 
   ASSERT_TRUE(player->is_paused());
   ASSERT_TRUE(player->play_next());
@@ -276,7 +271,6 @@ TEST_F(RosBag2PlayTestFixture, player_can_resume_after_play_next) {
   ASSERT_TRUE(player->is_paused());
 
   auto player_future = std::async(std::launch::async, [&player]() -> void {player->play();});
-  player->wait_for_playback_to_start();
 
   ASSERT_TRUE(player->is_paused());
   ASSERT_TRUE(player->play_next());
@@ -333,7 +327,6 @@ TEST_F(RosBag2PlayTestFixture, play_next_playing_only_filtered_topics) {
   ASSERT_TRUE(player->is_paused());
 
   auto player_future = std::async(std::launch::async, [&player]() -> void {player->play();});
-  player->wait_for_playback_to_start();
 
   ASSERT_TRUE(player->is_paused());
   ASSERT_TRUE(player->play_next());