tier4 · hayato-m126 · Jun 21, 2021 · Jun 18, 2021 · Jun 21, 2021
diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py
@@ -75,6 +75,9 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
             '-c', '--clock', type=float, default=0.0,
             help='Publish to /clock at a specific frequency in Hz, to act as a ROS Time Source. '
                  'Value must be positive. Defaults to not publishing.')
+        parser.add_argument(
+            '-d', '--delay', type=float, default=0.0,
+            help='Sleep SEC seconds after play. Valid range > 0.0')
 
     def main(self, *, args):  # noqa: D102
         qos_profile_overrides = {}  # Specify a valid default
@@ -107,4 +110,5 @@ def main(self, *, args):  # noqa: D102
             loop=args.loop,
             topic_remapping=args.remap,
             storage_config_file=storage_config_file,
-            clock_publish_frequency = args.clock)
+            clock_publish_frequency = args.clock,
+            delay=args.delay)
diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp
@@ -31,6 +31,7 @@ struct PlayOptions
   size_t read_ahead_queue_size;
   std::string node_prefix = "";
   float rate = 1.0;
+  float delay = 0.0;
 
   // Topic names to whitelist when playing a bag.
   // Only messages matching these specified topics will be played.

diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp
@@ -94,6 +94,12 @@ bool Player::is_storage_completely_loaded() const
 
 void Player::play(const PlayOptions & options)
 {
+  double delay = options.delay > 0.0 ? options.delay : 0.0;
+  if (delay > 0.0) {
+    ROSBAG2_TRANSPORT_LOG_INFO_STREAM("Sleep " << delay << " sec");
+    std::chrono::duration<double> duration(delay);
+    std::this_thread::sleep_for(duration);
+  }
   if (reader_->has_next()) {
     // Reader does not have "peek", so we must "pop" the first message to see its timestamp
     auto message = reader_->read_next();

diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp
@@ -268,6 +268,7 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k
     "topic_remapping",
     "storage_config_file",
     "clock_publish_frequency",
+    "delay",
     nullptr
   };
 
@@ -282,8 +283,9 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k
   PyObject * topic_remapping = nullptr;
   char * storage_config_file = nullptr;
   float clock_publish_frequency = 0.0;
+  float delay = 0.0;
   if (!PyArg_ParseTupleAndKeywords(
-      args, kwargs, "sss|kfOObOsf", const_cast<char **>(kwlist),
+      args, kwargs, "sss|kfOObOsff", const_cast<char **>(kwlist),
       &uri,
       &storage_id,
       &node_prefix,
@@ -294,7 +296,8 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k
       &loop,
       &topic_remapping,
       &storage_config_file,
-      &clock_publish_frequency))
+      &clock_publish_frequency,
+      &delay))
   {
     return nullptr;
   }
@@ -308,6 +311,7 @@ rosbag2_transport_play(PyObject * Py_UNUSED(self), PyObject * args, PyObject * k
   play_options.rate = rate;
   play_options.loop = loop;
   play_options.clock_publish_frequency = clock_publish_frequency;
+  play_options.delay = delay;
 
   if (topics) {
     PyObject * topic_iterator = PyObject_GetIter(topics);

diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp
@@ -43,6 +43,7 @@ TEST_F(RosBag2PlayTestFixture, messages_played_in_loop) {
   const size_t read_ahead_queue_size = 1000;
   const float rate = 1.0;
   const bool loop_playback = true;
+  const float delay = 0.0;
 
   auto primitive_message1 = get_messages_basic_types()[0];
   primitive_message1->int32_value = test_value;
@@ -70,7 +71,7 @@ TEST_F(RosBag2PlayTestFixture, messages_played_in_loop) {
     info_);
   std::thread loop_thread(&rosbag2_transport::Rosbag2Transport::play, rosbag2_transport_ptr,
     storage_options_,
-    rosbag2_transport::PlayOptions{read_ahead_queue_size, "", rate, {}, {}, loop_playback, {}});
+    rosbag2_transport::PlayOptions{read_ahead_queue_size, "", rate, delay, {}, {}, loop_playback, {}});
 
   await_received_messages.get();
   rclcpp::shutdown();