From 5f7da84e0997cc499bf30edb5d156ed2c5c60ae4 Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Thu, 21 Nov 2024 22:46:43 +0000 Subject: [PATCH 1/3] publish clock after the delay is over Signed-off-by: Nicola Loi --- 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) { From db4c90e06185ffea10b9ed50f191d6aaf273a36c Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Sun, 24 Nov 2024 22:06:22 +0000 Subject: [PATCH 2/3] Disable delay period in subsequent loops (ros2 bag play) Signed-off-by: Nicola Loi --- ros2bag/ros2bag/verb/play.py | 3 ++- .../include/rosbag2_transport/play_options.hpp | 2 +- rosbag2_transport/src/rosbag2_transport/player.cpp | 13 +++++-------- 3 files changed, 8 insertions(+), 10 deletions(-) diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index 363d43d59..7f988839f 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -101,7 +101,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 index e5029c923..cb12c11f9 100644 --- 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 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 index e2f39e45f..fb9bbd610 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -483,15 +483,12 @@ 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)) { - 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); - } { std::lock_guard lk(reader_mutex_); reader_->seek(starting_time_); From cecd120a0164406b21afcae627d453464d7be794 Mon Sep 17 00:00:00 2001 From: Nicola Loi Date: Sun, 24 Nov 2024 23:12:58 +0000 Subject: [PATCH 3/3] Reset clock publisher timer outisde playback loop Signed-off-by: Nicola Loi --- rosbag2_transport/src/rosbag2_transport/player.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index fb9bbd610..47313361c 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -488,15 +488,19 @@ bool PlayerImpl::play() std::chrono::nanoseconds delay_duration(delay.nanoseconds()); std::this_thread::sleep_for(delay_duration); } + { + std::lock_guard lk(reader_mutex_); + clock_->jump(starting_time_); + } + if (clock_publish_timer_ != nullptr) { + clock_publish_timer_->reset(); + } do { { std::lock_guard lk(reader_mutex_); 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]() {