From 5f7da84e0997cc499bf30edb5d156ed2c5c60ae4 Mon Sep 17 00:00:00 2001
From: Nicola Loi <nicolaloi@outlook.com>
Date: Thu, 21 Nov 2024 22:46:43 +0000
Subject: [PATCH] publish clock after the delay is over

Signed-off-by: Nicola Loi <nicolaloi@outlook.com>
---
 rosbag2_transport/src/rosbag2_transport/player.cpp | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp
index 74a911f1d..e2f39e45f 100644
--- a/rosbag2_transport/src/rosbag2_transport/player.cpp
+++ b/rosbag2_transport/src/rosbag2_transport/player.cpp
@@ -485,6 +485,9 @@ bool PlayerImpl::play()
       try {
         do {
           if (delay > rclcpp::Duration(0, 0)) {
+            if (clock_publish_timer_ != nullptr) {
+              clock_publish_timer_->cancel();
+            }
             RCLCPP_INFO_STREAM(owner_->get_logger(), "Sleep " << delay.nanoseconds() << " ns");
             std::chrono::nanoseconds delay_duration(delay.nanoseconds());
             std::this_thread::sleep_for(delay_duration);
@@ -494,6 +497,9 @@ bool PlayerImpl::play()
             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]() {
@@ -1072,7 +1078,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) {