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

) (#1878) * publish clock after the delay is over Signed-off-by: Nicola Loi <nicolaloi@outlook.com> * Disable delay period in subsequent loops (ros2 bag play) Signed-off-by: Nicola Loi <nicolaloi@outlook.com> * Reset clock publisher timer outisde playback loop Signed-off-by: Nicola Loi <nicolaloi@outlook.com> * review Signed-off-by: Nicola Loi <nicolaloi@outlook.com> --------- Signed-off-by: Nicola Loi <nicolaloi@outlook.com> (cherry picked from commit 15b8260) Co-authored-by: Nicola Loi <nicolaloi@outlook.com>
ros2 · Dec 8, 2024 · cfd35ce · cfd35ce
1 parent 6091bc7
commit cfd35ce
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 8 deletions.
diff --git a/ros2bag/ros2bag/verb/play.py b/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 --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/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 --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/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) {