Playback pause feature

ros2bag/ros2bag/verb/play.py

@@ -52,6 +52,9 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
             '-l', '--loop', action='store_true',
             help='enables loop playback when playing a bagfile: it starts back at the beginning '
                  'on reaching the end and plays indefinitely.')
+        parser.add_argument(
+            '-p', '--paused', action='store_true',
+            help='start rosbag2 in paused mode. No messages are played at startup.')
         parser.add_argument(
             '--remap', '-m', default='', nargs='+',
             help='list of topics to be remapped: in the form '
@@ -82,4 +85,5 @@ def main(self, *, args):  # noqa: D102
             topics=args.topics,
             qos_profile_overrides=qos_profile_overrides,
             loop=args.loop,
+            paused=args.paused,
             topic_remapping=args.remap)

rosbag2_transport/CMakeLists.txt

@@ -22,8 +22,10 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_ros REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
 find_package(rcl REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
 find_package(rcutils REQUIRED)
 find_package(rmw REQUIRED)
 find_package(rosbag2_compression REQUIRED)
@@ -46,8 +48,10 @@ target_include_directories(${PROJECT_NAME} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
 ament_target_dependencies(${PROJECT_NAME}
+  lifecycle_msgs
   rcl
   rclcpp
+  rclcpp_lifecycle
   rcutils
   rmw
   rosbag2_compression
@@ -125,7 +129,9 @@ function(create_tests_for_rmw_implementation)
       $<INSTALL_INTERFACE:include>
     AMENT_DEPS
       ament_index_cpp
+      lifecycle_msgs
       rclcpp
+      rclcpp_lifecycle
       rosbag2_cpp
       rosbag2_test_common
       test_msgs

rosbag2_transport/include/rosbag2_transport/play_options.hpp

@@ -39,6 +39,7 @@ struct PlayOptions
 
   std::unordered_map<std::string, rclcpp::QoS> topic_qos_profile_overrides = {};
   bool loop = false;
+  bool paused = false;
   std::vector<std::string> topic_remapping_options = {};
 };
 

rosbag2_transport/include/rosbag2_transport/rosbag2_transport.hpp

@@ -84,6 +84,18 @@ class Rosbag2Transport
   ROSBAG2_TRANSPORT_PUBLIC
   void print_bag_info(const std::string & uri, const std::string & storage_id);
 
+  /**
+   * Resume playback.
+   */
+  ROSBAG2_TRANSPORT_PUBLIC
+  void resume();
+
+  /**
+   * Pause playback.
+   */
+  ROSBAG2_TRANSPORT_PUBLIC
+  void pause();
+
 private:
   std::shared_ptr<Rosbag2Node> setup_node(
     std::string node_prefix = "",

rosbag2_transport/package.xml

@@ -10,8 +10,10 @@
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
+  <depend>lifecycle_msgs</depend>
   <depend>python_cmake_module</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
   <depend>rosbag2_compression</depend>
   <depend>rosbag2_cpp</depend>
   <depend>rosbag2_storage</depend>

rosbag2_transport/src/rosbag2_transport/player.cpp

@@ -22,6 +22,8 @@
 #include <utility>
 #include <vector>
 
+#include "lifecycle_msgs/msg/state.hpp"
+
 #include "rcl/graph.h"
 
 #include "rclcpp/rclcpp.hpp"
@@ -75,10 +77,13 @@ namespace rosbag2_transport
 
 const std::chrono::milliseconds
 Player::queue_read_wait_period_ = std::chrono::milliseconds(100);
+const std::chrono::milliseconds
+Player::pause_sleep_period_ = std::chrono::milliseconds(30);
 
 Player::Player(
   std::shared_ptr<rosbag2_cpp::Reader> reader, std::shared_ptr<Rosbag2Node> rosbag2_transport)
-: reader_(std::move(reader)), rosbag2_transport_(rosbag2_transport)
+: reader_(std::move(reader)), played_all_(false),
+  rosbag2_transport_(rosbag2_transport)
 {}
 
 bool Player::is_storage_completely_loaded() const
@@ -102,6 +107,14 @@ void Player::play(const PlayOptions & options)
 
   wait_for_filled_queue(options);
 
+  rosbag2_transport_->configure();
+  // If playback should not be paused in the beginning, activate lifecycle node right away.
+  // If playback should be paused, move on.
+  // Playback will detect when node is activated, either from here or externally.
+  if (!options.paused) {
+    rosbag2_transport_->activate();
+  }
+
   play_messages_from_queue(options);
 }
 
@@ -158,6 +171,7 @@ void Player::enqueue_up_to_boundary(const TimePoint & time_first_message, uint64
 void Player::play_messages_from_queue(const PlayOptions & options)
 {
   start_time_ = std::chrono::system_clock::now();
+  paused_duration_ = std::chrono::nanoseconds(0);
   do {
     play_messages_until_queue_empty(options);
     if (!is_storage_completely_loaded() && rclcpp::ok()) {
@@ -166,6 +180,8 @@ void Player::play_messages_from_queue(const PlayOptions & options)
         "increasing the --read-ahead-queue-size option.");
     }
   } while (!is_storage_completely_loaded() && rclcpp::ok());
+
+  played_all_ = true;
 }
 
 void Player::play_messages_until_queue_empty(const PlayOptions & options)
@@ -178,12 +194,37 @@ void Player::play_messages_until_queue_empty(const PlayOptions & options)
     rate = options.rate;
   }
 
+  static size_t counter = 0;
   while (message_queue_.try_dequeue(message) && rclcpp::ok()) {
     std::this_thread::sleep_until(
       start_time_ + std::chrono::duration_cast<std::chrono::nanoseconds>(
-        1.0 / rate * message.time_since_start));
+        1.0 / rate * message.time_since_start + paused_duration_));
+
+    if (rosbag2_transport_->get_current_state().id() !=
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+    {
+      ROSBAG2_TRANSPORT_LOG_INFO("Paused");
+      std::chrono::time_point<std::chrono::system_clock> pause_start =
+        std::chrono::system_clock::now();
+
+      // Sleep until activated externally
+      while (rosbag2_transport_->get_current_state().id() !=
+        lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && rclcpp::ok())
+      {
+        std::this_thread::sleep_for(pause_sleep_period_);
+      }
+
+      paused_duration_ += std::chrono::system_clock::now() - pause_start;
+
+      if (rclcpp::ok()) {
+        ROSBAG2_TRANSPORT_LOG_INFO("Resumed");
+      }
+    }
+
     if (rclcpp::ok()) {
       publishers_[message.message->topic_name]->publish(message.message->serialized_data);
+      // TODO(mabelzhang) TEMPORARY, remove when done debugging. And counter above.
+      fprintf(stderr, "publishing message %zu\n", (++counter));
     }
   }
 }

rosbag2_transport/src/rosbag2_transport/player.hpp

@@ -60,12 +60,15 @@ class Player
   void play_messages_from_queue(const PlayOptions & options);
   void play_messages_until_queue_empty(const PlayOptions & options);
   void prepare_publishers(const PlayOptions & options);
+
   static constexpr double read_ahead_lower_bound_percentage_ = 0.9;
   static const std::chrono::milliseconds queue_read_wait_period_;
-
+  static const std::chrono::milliseconds pause_sleep_period_;
   std::shared_ptr<rosbag2_cpp::Reader> reader_;
   moodycamel::ReaderWriterQueue<ReplayableMessage> message_queue_;
   std::chrono::time_point<std::chrono::system_clock> start_time_;
+  std::chrono::nanoseconds paused_duration_;
+  bool played_all_;
   mutable std::future<void> storage_loading_future_;
   std::shared_ptr<Rosbag2Node> rosbag2_transport_;
   std::unordered_map<std::string, std::shared_ptr<GenericPublisher>> publishers_;

rosbag2_transport/src/rosbag2_transport/recorder.cpp

@@ -23,6 +23,8 @@
 #include <utility>
 #include <vector>
 
+#include "lifecycle_msgs/msg/state.hpp"
+
 #include "rosbag2_cpp/writer.hpp"
 
 #include "rosbag2_transport/logging.hpp"
@@ -184,7 +186,9 @@ Recorder::create_subscription(
 
 void Recorder::record_messages() const
 {
-  spin(node_);
+  rclcpp::executors::MultiThreadedExecutor exe;
+  exe.add_node(node_->get_node_base_interface());
+  exe.spin();
 }
 
 std::string Recorder::serialized_offered_qos_profiles_for_topic(const std::string & topic_name)

rosbag2_transport/src/rosbag2_transport/rosbag2_node.cpp

@@ -34,11 +34,11 @@ namespace rosbag2_transport
 {
 
 Rosbag2Node::Rosbag2Node(const std::string & node_name)
-: rclcpp::Node(node_name)
+: rclcpp_lifecycle::LifecycleNode(node_name)
 {}
 
 Rosbag2Node::Rosbag2Node(const std::string & node_name, const rclcpp::NodeOptions & options)
-: rclcpp::Node(node_name, options)
+: rclcpp_lifecycle::LifecycleNode(node_name, options)
 {}
 
 std::shared_ptr<GenericPublisher> Rosbag2Node::create_generic_publisher(

rosbag2_transport/src/rosbag2_transport/rosbag2_node.hpp

@@ -21,9 +21,10 @@
 #include <unordered_map>
 #include <vector>
 
-#include "rclcpp/node.hpp"
+#include "rclcpp/rclcpp.hpp"
 #include "rclcpp/serialized_message.hpp"
 #include "rclcpp/node_options.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "rcpputils/shared_library.hpp"
 #include "rcutils/types.h"
 
@@ -33,7 +34,7 @@
 namespace rosbag2_transport
 {
 
-class Rosbag2Node : public rclcpp::Node
+class Rosbag2Node : public rclcpp_lifecycle::LifecycleNode
 {
 public:
   explicit Rosbag2Node(const std::string & node_name);

rosbag2_transport/src/rosbag2_transport/rosbag2_transport.cpp

@@ -126,4 +126,24 @@ void Rosbag2Transport::print_bag_info(const std::string & uri, const std::string
   Formatter::format_bag_meta_data(metadata);
 }
 
+void Rosbag2Transport::resume()
+{
+  if (transport_node_) {
+    transport_node_->activate();
+  } else {
+    ROSBAG2_TRANSPORT_LOG_ERROR(
+      "transport_node_ not initialized. Call play() before activating lifecycle node.");
+  }
+}
+
+void Rosbag2Transport::pause()
+{
+  if (transport_node_) {
+    transport_node_->deactivate();
+  } else {
+    ROSBAG2_TRANSPORT_LOG_ERROR(
+      "transport_node_ not initialized. Call play() before deactivating lifecycle node.");
+  }
+}
+
 }  // namespace rosbag2_transport

rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp

@@ -236,6 +236,7 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k
     "topics",
     "qos_profile_overrides",
     "loop",
+    "paused",
     "topic_remapping",
     nullptr
   };
@@ -248,9 +249,10 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k
   PyObject * topics = nullptr;
   PyObject * qos_profile_overrides{nullptr};
   bool loop = false;
+  bool paused = false;
   PyObject * topic_remapping = nullptr;
   if (!PyArg_ParseTupleAndKeywords(
-      args, kwargs, "sss|kfOObO", const_cast<char **>(kwlist),
+      args, kwargs, "sss|kfOObbO", const_cast<char **>(kwlist),
       &uri,
       &storage_id,
       &node_prefix,
@@ -259,6 +261,7 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k
       &topics,
       &qos_profile_overrides,
       &loop,
+      &paused,
       &topic_remapping))
   {
     return nullptr;
@@ -271,6 +274,7 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k
   play_options.read_ahead_queue_size = read_ahead_queue_size;
   play_options.rate = rate;
   play_options.loop = loop;
+  play_options.paused = paused;
 
   if (topics) {
     PyObject * topic_iterator = PyObject_GetIter(topics);

rosbag2_transport/test/rosbag2_transport/test_play.cpp

@@ -202,6 +202,82 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics)
   EXPECT_THAT(replayed_test_arrays, SizeIs(Ge(2u)));
 }
 
+TEST_F(RosBag2PlayTestFixture, starting_paused_can_resume)
+{
+  auto primitive_message1 = get_messages_basic_types()[0];
+  primitive_message1->int32_value = 42;
+
+  auto complex_message1 = get_messages_arrays()[0];
+  complex_message1->float32_values = {{40.0f, 2.0f, 0.0f}};
+  complex_message1->bool_values = {{true, false, true}};
+
+  auto topic_types = std::vector<rosbag2_storage::TopicMetadata>{
+    {"topic1", "test_msgs/BasicTypes", "", ""},
+    {"topic2", "test_msgs/Arrays", "", ""},
+  };
+
+  // 3 messages of each type to be played
+  std::vector<std::shared_ptr<rosbag2_storage::SerializedBagMessage>> messages =
+  {serialize_test_message("topic1", 500, primitive_message1),
+    serialize_test_message("topic1", 700, primitive_message1),
+    serialize_test_message("topic1", 900, primitive_message1),
+    serialize_test_message("topic2", 550, complex_message1),
+    serialize_test_message("topic2", 750, complex_message1),
+    serialize_test_message("topic2", 950, complex_message1)};
+
+  auto prepared_mock_reader = std::make_unique<MockSequentialReader>();
+  prepared_mock_reader->prepare(messages, topic_types);
+  reader_ = std::make_unique<rosbag2_cpp::Reader>(std::move(prepared_mock_reader));
+
+  // Due to a problem related to the subscriber, we play many (3) messages but make the subscriber
+  // node spin only until 2 have arrived. Hence the 2 as `launch_subscriber()` argument.
+  sub_->add_subscription<test_msgs::msg::BasicTypes>("/topic1", 2);
+  sub_->add_subscription<test_msgs::msg::Arrays>("/topic2", 2);
+
+  auto await_received_messages = sub_->spin_subscriptions();
+
+  // Start playback paused
+  play_options_.paused = true;
+
+  Rosbag2Transport rosbag2_transport(reader_, writer_, info_);
+
+  std::future<void> future_handle = std::async(
+    std::launch::async, [&rosbag2_transport]() mutable {
+      // Wait for node initialization, then resume
+      std::this_thread::sleep_for(std::chrono::milliseconds(500));
+      rosbag2_transport.resume();
+    });
+
+  rosbag2_transport.play(storage_options_, play_options_);
+
+  await_received_messages.get();
+
+  auto replayed_test_primitives = sub_->get_received_messages<test_msgs::msg::BasicTypes>(
+    "/topic1");
+  EXPECT_THAT(replayed_test_primitives, SizeIs(Ge(2u)));
+  EXPECT_THAT(
+    replayed_test_primitives,
+    Each(Pointee(Field(&test_msgs::msg::BasicTypes::int32_value, 42))));
+
+  auto replayed_test_arrays = sub_->get_received_messages<test_msgs::msg::Arrays>(
+    "/topic2");
+  EXPECT_THAT(replayed_test_arrays, SizeIs(Ge(2u)));
+  EXPECT_THAT(
+    replayed_test_arrays,
+    Each(
+      Pointee(
+        Field(
+          &test_msgs::msg::Arrays::bool_values,
+          ElementsAre(true, false, true)))));
+  EXPECT_THAT(
+    replayed_test_arrays,
+    Each(
+      Pointee(
+        Field(
+          &test_msgs::msg::Arrays::float32_values,
+          ElementsAre(40.0f, 2.0f, 0.0f)))));
+}
+
 class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture
 {
 public: