Add burst-mode to Player (ros2#977)

Signed-off-by: Geoffrey Biggs <[email protected]>

Author: gbiggs

rosbag2_interfaces/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(builtin_interfaces REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/Burst.srv"
   "srv/GetRate.srv"
   "srv/IsPaused.srv"
   "srv/Pause.srv"

rosbag2_interfaces/srv/Burst.srv

@@ -0,0 +1,3 @@
+uint64 num_messages  # Number of messages to burst
+---
+uint64 actually_burst  # Number of messages actually burst

rosbag2_transport/CMakeLists.txt

@@ -144,6 +144,12 @@ function(create_tests_for_rmw_implementation)
       LINK_LIBS rosbag2_transport
       AMENT_DEPS test_msgs rosbag2_test_common)
 
+  rosbag2_transport_add_gmock(test_burst
+      test/rosbag2_transport/test_burst.cpp
+      INCLUDE_DIRS $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/rosbag2_transport>
+      LINK_LIBS rosbag2_transport
+      AMENT_DEPS test_msgs rosbag2_test_common)
+
   rosbag2_transport_add_gmock(test_qos
     test/rosbag2_transport/test_qos.cpp
     INCLUDE_DIRS $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

rosbag2_transport/include/rosbag2_transport/player.hpp

@@ -36,6 +36,7 @@
 #include "rosbag2_interfaces/srv/is_paused.hpp"
 #include "rosbag2_interfaces/srv/pause.hpp"
 #include "rosbag2_interfaces/srv/play_next.hpp"
+#include "rosbag2_interfaces/srv/burst.hpp"
 #include "rosbag2_interfaces/srv/resume.hpp"
 #include "rosbag2_interfaces/srv/set_rate.hpp"
 #include "rosbag2_interfaces/srv/seek.hpp"
@@ -129,6 +130,16 @@ class Player : public rclcpp::Node
   ROSBAG2_TRANSPORT_PUBLIC
   virtual bool play_next();
 
+  /// \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.
+  /// \details This call will play the next \p num_messages from the queue in burst mode. The
+  /// timing of the messages is ignored.
+  /// \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 The number of messages that was played.
+  ROSBAG2_TRANSPORT_PUBLIC
+  virtual size_t burst(const size_t num_messages);
+
   /// \brief Advance player to the message with closest timestamp >= time_point.
   /// \details This is blocking call and it will wait until current message will be published
   /// and message queue will be refilled.
@@ -193,6 +204,7 @@ class Player : public rclcpp::Node
   rclcpp::Service<rosbag2_interfaces::srv::GetRate>::SharedPtr srv_get_rate_;
   rclcpp::Service<rosbag2_interfaces::srv::SetRate>::SharedPtr srv_set_rate_;
   rclcpp::Service<rosbag2_interfaces::srv::PlayNext>::SharedPtr srv_play_next_;
+  rclcpp::Service<rosbag2_interfaces::srv::Burst>::SharedPtr srv_burst_;
   rclcpp::Service<rosbag2_interfaces::srv::Seek>::SharedPtr srv_seek_;
 
   // defaults

rosbag2_transport/src/rosbag2_transport/player.cpp

@@ -349,6 +349,21 @@ bool Player::play_next()
   return next_message_published;
 }
 
+size_t Player::burst(const size_t num_messages)
+{
+  uint64_t messages_played = 0;
+
+  for (auto ii = 0u; ii < num_messages; ++ii) {
+    if (play_next()) {
+      ++messages_played;
+    } else {
+      break;
+    }
+  }
+
+  return messages_played;
+}
+
 void Player::seek(rcutils_time_point_value_t time_point)
 {
   // Temporary stop playback in play_messages_from_queue() and block play_next()
@@ -658,6 +673,14 @@ void Player::create_control_services()
     {
       response->success = play_next();
     });
+  srv_burst_ = create_service<rosbag2_interfaces::srv::Burst>(
+    "~/burst",
+    [this](
+      rosbag2_interfaces::srv::Burst::Request::ConstSharedPtr request,
+      rosbag2_interfaces::srv::Burst::Response::SharedPtr response)
+    {
+      response->actually_burst = burst(request->num_messages);
+    });
   srv_seek_ = create_service<rosbag2_interfaces::srv::Seek>(
     "~/seek",
     [this](