publish clock after the delay is over

- and pause clock until delay is over

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

rosbag2_transport/src/rosbag2_transport/player.cpp

@@ -408,7 +408,8 @@ PlayerImpl::PlayerImpl(
     }
     clock_ = std::make_unique<rosbag2_cpp::TimeControllerClock>(
       starting_time_, std::chrono::steady_clock::now,
-      std::chrono::milliseconds{100}, play_options_.start_paused);
+      std::chrono::milliseconds{100},
+      play_options_.start_paused || play_options_.delay > rclcpp::Duration(0, 0));
     set_rate(play_options_.rate);
     topic_qos_profile_overrides_ = play_options_.topic_qos_profile_overrides;
     prepare_publishers();
@@ -488,6 +489,12 @@ bool PlayerImpl::play()
             RCLCPP_INFO_STREAM(owner_->get_logger(), "Sleep " << delay.nanoseconds() << " ns");
             std::chrono::nanoseconds delay_duration(delay.nanoseconds());
             std::this_thread::sleep_for(delay_duration);
+            if (!play_options_.start_paused) {
+              clock_->resume();
+            }
+          }
+          if (clock_publish_timer_ != nullptr) {
+            clock_publish_timer_->reset();
           }
           {
             std::lock_guard<std::mutex> lk(reader_mutex_);
@@ -1072,7 +1079,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) {