Bugfix: Player can't play with read_ahead_queue_size equal 1 (#2174)

* Bugfix: Player can't play with read_ahead_queue_size equal 1

Signed-off-by: Michael Orlov <morlovmr@gmail.com>

* Add missing deserialize_test_message() from #2172

Signed-off-by: Michael Orlov <michael.orlov@apex.ai>

* Address review comments

Co-Authored by: Christophe Bedard <bedard.christophe@gmail.com>

Co-authored-by: Christophe Bedard <bedard.christophe@gmail.com>
Signed-off-by: Michael Orlov <morlovmr@gmail.com>

---------

Signed-off-by: Michael Orlov <morlovmr@gmail.com>
Signed-off-by: Michael Orlov <michael.orlov@apex.ai>
Co-authored-by: Christophe Bedard <bedard.christophe@gmail.com>

Author: MichaelOrlov

rosbag2_transport/src/rosbag2_transport/player.cpp

@@ -463,6 +463,10 @@ PlayerImpl::PlayerImpl(
   keyboard_handler_(std::move(keyboard_handler)),
   player_service_client_manager_(std::make_shared<PlayerServiceClientManager>())
 {
+  if (play_options_.read_ahead_queue_size < 1) {
+    throw std::invalid_argument("read_ahead_queue_size must be at least 1");
+  }
+
   for (auto & topic : play_options_.topics_to_filter) {
     topic = rclcpp::expand_topic_or_service_name(
       topic, owner_->get_name(),
@@ -1141,7 +1145,7 @@ void PlayerImpl::load_storage_content()
   while (load_storage_content_ && !stop_playback_ && readers_->has_next()) {
     // The message queue size may get smaller after this, but that's OK
     const size_t message_queue_size = message_queue_.size();
-    if (message_queue_size < queue_lower_boundary) {
+    if (message_queue_size <= queue_lower_boundary) {
       enqueue_up_to_boundary(queue_upper_boundary, message_queue_size);
     } else {
       std::this_thread::sleep_for(std::chrono::milliseconds(1));

rosbag2_transport/test/rosbag2_transport/rosbag2_transport_test_fixture.hpp

@@ -78,6 +78,13 @@ class Rosbag2TransportTestFixture : public Test
     return bag_msg;
   }
 
+  template<typename MessageT>
+  std::shared_ptr<MessageT> deserialize_test_message(
+    const std::shared_ptr<rosbag2_storage::SerializedBagMessage> bag_msg)
+  {
+    return memory_management_.deserialize_message<MessageT>(bag_msg->serialized_data);
+  }
+
   static std::string format_message_order(
     const TestParamInfo<rosbag2_transport::MessageOrder> & info)
   {

rosbag2_transport/test/rosbag2_transport/test_play.cpp

@@ -403,6 +403,76 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics)
           ElementsAre(40.0f, 2.0f, 0.0f)))));
 }
 
+TEST_F(RosBag2PlayTestFixture, can_play_messages_with_queue_size_equal_one)
+{
+  const auto msg = get_messages_basic_types()[0];
+  auto topic_types = std::vector<rosbag2_storage::TopicMetadata>{
+    {1u, "topic1", "test_msgs/msg/BasicTypes", "", {}, ""},
+    {2u, "topic2", "test_msgs/msg/BasicTypes", "", {}, ""},
+  };
+
+  std::vector<std::vector<std::shared_ptr<rosbag2_storage::SerializedBagMessage>>> messages_list{};
+  msg->int32_value = 0;
+  messages_list.emplace_back(std::vector<std::shared_ptr<rosbag2_storage::SerializedBagMessage>>{
+    (msg->int32_value++, serialize_test_message("topic1", 1, msg)),
+    (msg->int32_value++, serialize_test_message("topic2", 2, msg)),
+    (msg->int32_value++, serialize_test_message("topic1", 5, msg)),
+    (msg->int32_value++, serialize_test_message("topic2", 6, msg)),
+    (msg->int32_value++, serialize_test_message("topic1", 9, msg)),
+    (msg->int32_value++, serialize_test_message("topic2", 10, msg))
+  });
+
+  std::vector<rosbag2_transport::Player::reader_storage_options_pair_t> bags{};
+  std::size_t total_messages = 0u;
+  for (const auto & curr_bag_messages : messages_list) {
+    auto prepared_mock_reader = std::make_unique<MockSequentialReader>();
+    total_messages += curr_bag_messages.size();
+    prepared_mock_reader->prepare(curr_bag_messages, topic_types);
+    bags.emplace_back(
+      std::make_unique<rosbag2_cpp::Reader>(std::move(prepared_mock_reader)), storage_options_);
+  }
+  ASSERT_GT(total_messages, 0u);
+  // Note: For the sake of the test, the read_ahead_queue_size is set to 1
+  // to test the corner case of having the smallest possible queue size.
+  // This means that only one message will be prefetched from the readers at a time.
+  // This is to make sure that the player can handle this case correctly.
+  // In a real scenario, a larger queue size is recommended to improve performance.
+  play_options_.read_ahead_queue_size = 1;
+  const rosbag2_transport::MessageOrder message_order = play_options_.message_order;
+
+  auto player = std::make_shared<rosbag2_transport::Player>(std::move(bags), play_options_);
+  std::size_t num_played_messages = 0u;
+  rcutils_time_point_value_t last_timestamp = 0;
+  const auto get_timestamp =
+    [message_order](std::shared_ptr<rosbag2_storage::SerializedBagMessage> msg) {
+      switch (message_order) {
+        case rosbag2_transport::MessageOrder::RECEIVED_TIMESTAMP:
+          return msg->recv_timestamp;
+        case rosbag2_transport::MessageOrder::SENT_TIMESTAMP:
+          return msg->send_timestamp;
+        default:
+          throw std::runtime_error("unknown rosbag2_transport::MessageOrder value");
+      }
+    };
+  const auto callback = [&](std::shared_ptr<rosbag2_storage::SerializedBagMessage> playing_msg) {
+      // Make sure messages are played in order
+      num_played_messages++;
+      const auto timestamp = get_timestamp(playing_msg);
+      using MessageT = typename decltype(msg)::element_type;
+      const auto deserialized_msg = deserialize_test_message<MessageT>(playing_msg);
+      // The int32_value was set in an increasing order when creating the messages
+      EXPECT_EQ(deserialized_msg->int32_value, num_played_messages) << ", timestamp = " <<
+        RCUTILS_NS_TO_MS(timestamp) << ", topic_name = `" << playing_msg->topic_name << "`\n";
+      EXPECT_LE(last_timestamp, timestamp);
+      last_timestamp = timestamp;
+    };
+  player->add_on_play_message_pre_callback(callback);
+  player->play();
+  ASSERT_TRUE(player->wait_for_playback_to_start(10s));
+  ASSERT_TRUE(player->wait_for_playback_to_finish(10s));
+  EXPECT_EQ(total_messages, num_played_messages);
+}
+
 TEST_F(RosBag2PlayTestFixture, can_play_when_one_bag_has_fewer_messages_than_other_bags)
 {
   auto msg = get_messages_basic_types()[0];