Publish clock after delay is over and disable delay on next loops (#1861)

* publish clock after the delay is over

Signed-off-by: Nicola Loi <[email protected]>

* Disable delay period in subsequent loops (ros2 bag play)

Signed-off-by: Nicola Loi <[email protected]>

* Reset clock publisher timer outisde playback loop

Signed-off-by: Nicola Loi <[email protected]>

* review

Signed-off-by: Nicola Loi <[email protected]>

---------

Signed-off-by: Nicola Loi <[email protected]>

Author: nicolaloi

Diff for: ros2bag/ros2bag/verb/play.py

@@ -103,7 +103,8 @@ def add_arguments(self, parser, cli_name):  # noqa: D102
         )
         parser.add_argument(
             '-d', '--delay', type=positive_float, default=0.0,
-            help='Sleep duration before play (each loop), in seconds. Negative durations invalid.')
+            help='Sleep duration before play (loops are not affected), in seconds.'
+                 'Negative durations invalid.')
         parser.add_argument(
             '--playback-duration', type=float, default=-1.0,
             help='Playback duration, in seconds. Negative durations mark an infinite playback. '

Diff for: rosbag2_transport/include/rosbag2_transport/play_options.hpp

@@ -83,7 +83,7 @@ struct PlayOptions
   // a /clock update to be published. If list is empty, all topics will act as a trigger
   std::vector<std::string> clock_trigger_topics = {};
 
-  // Sleep before play. Negative durations invalid. Will delay at the beginning of each loop.
+  // Sleep before play. Negative durations invalid. Loops are not affected.
   rclcpp::Duration delay = rclcpp::Duration(0, 0);
 
   // Determines the maximum duration of the playback. Negative durations will make the playback to

Diff for: rosbag2_transport/src/rosbag2_transport/player.cpp

@@ -524,19 +524,22 @@ bool PlayerImpl::play()
   playback_thread_ = std::thread(
     [&, delay]() {
       try {
+        if (delay > rclcpp::Duration(0, 0)) {
+          RCLCPP_INFO_STREAM(owner_->get_logger(), "Sleep " << delay.nanoseconds() << " ns");
+          std::chrono::nanoseconds delay_duration(delay.nanoseconds());
+          std::this_thread::sleep_for(delay_duration);
+        }
         do {
-          if (delay > rclcpp::Duration(0, 0)) {
-            RCLCPP_INFO_STREAM(owner_->get_logger(), "Sleep " << delay.nanoseconds() << " ns");
-            std::chrono::nanoseconds delay_duration(delay.nanoseconds());
-            std::this_thread::sleep_for(delay_duration);
-          }
           {
             std::lock_guard<std::mutex> lk(reader_mutex_);
             for (const auto & [reader, _] : readers_with_options_) {
               reader->seek(starting_time_);
             }
             clock_->jump(starting_time_);
           }
+          if (clock_publish_timer_ != nullptr) {
+            clock_publish_timer_->reset();
+          }
           load_storage_content_ = true;
           storage_loading_future_ = std::async(
             std::launch::async, [this]() {
@@ -1165,7 +1168,7 @@ void PlayerImpl::prepare_publishers()
     clock_publish_timer_ = owner_->create_wall_timer(
       publish_period, [this]() {
         publish_clock_update();
-      });
+      }, nullptr, false);
   }
 
   if (play_options_.clock_publish_on_topic_publish) {