From 5576c77a7cf4795e83f9478aff230a1ddec34f8b Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Thu, 21 Nov 2024 22:46:43 +0000 Subject: [PATCH] publish clock after the delay is over - and pause clock until delay is over Signed-off-by: Nicola Loi --- rosbag2_transport/src/rosbag2_transport/player.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 74a911f1d..7c3f5cff9 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -408,7 +408,8 @@ PlayerImpl::PlayerImpl( } clock_ = std::make_unique( 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 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) {