From a5d299843dc7e62f7d36703721b127d031f1f463 Mon Sep 17 00:00:00 2001 From: yadunund Date: Mon, 9 Nov 2026 14:46:15 -0800 Subject: [PATCH 1/2] Re-key EventsCBGExecutor timers when rcl reschedules them (#3344) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit rcl moves next_call_time of a ROS time timer when the clock jumps back by more than one period and when ROS time is activated or deactivated, without calling the on reset callback. TimerQueue keyed its running timers by a copy of next_call_time taken when the timer was added, fired or reset, so after such a jump the timer thread slept until the stale deadline: a /clock jump from 1000 s back to 900 s silenced a 100 ms timer for 100 s of simulated time, and a timer that was created before use_sim_time was enabled never fired again. Register rcl's schedule change callback on every ROS time timer. It runs inside the clock jump callback with the clock mutex held, which the timer thread takes while holding its wait mutex, so it only flags the timer; the timer thread re-keys flagged timers before it computes its next wakeup, and the flag is part of its wait predicate. Signed-off-by: Yadunund Vijay Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Alejandro Hernández Cordero (cherry picked from commit ae09d1b9db287e3be0ae56ba75b31b53e9dc5b20) # Conflicts: # rclcpp/src/rclcpp/executors/events_cbg_executor/timer_manager.hpp --- .../events_cbg_executor/timer_manager.hpp | 74 ++++++++- rclcpp/test/rclcpp/CMakeLists.txt | 9 ++ .../executors/test_executors_time_jump.cpp | 148 ++++++++++++++++++ 3 files changed, 230 insertions(+), 1 deletion(-) create mode 100644 rclcpp/test/rclcpp/executors/test_executors_time_jump.cpp diff --git a/rclcpp/src/rclcpp/executors/events_cbg_executor/timer_manager.hpp b/rclcpp/src/rclcpp/executors/events_cbg_executor/timer_manager.hpp index eff95119c3..d1fd046f01 100644 --- a/rclcpp/src/rclcpp/executors/events_cbg_executor/timer_manager.hpp +++ b/rclcpp/src/rclcpp/executors/events_cbg_executor/timer_manager.hpp @@ -16,7 +16,9 @@ #include +#include #include +#include #include #include #include @@ -126,6 +128,10 @@ class TimerQueue rclcpp::Clock::SharedPtr clock; bool in_running_list = false; std::function & executed_cb)> timer_ready_callback; + // set by rcl when it moved next_call_time, consumed by the timer thread + std::atomic rekey_pending = false; + // set for ROS time timers, which register the schedule change callback + TimerQueue * queue = nullptr; }; public: @@ -161,6 +167,7 @@ class TimerQueue if (auto shrPtr = tData->timer_ref.lock()) { shrPtr->clear_on_reset_callback(); } + clear_on_schedule_change_callback(*tData); } } @@ -215,6 +222,7 @@ class TimerQueue if(it2 != running_timers.end()) { running_timers.erase(it2); } + clear_on_schedule_change_callback(**it); all_timers.erase(it); } @@ -262,6 +270,19 @@ class TimerQueue } }); + if (timer_type == RCL_ROS_TIME) { + // rcl moves next_call_time of a ROS time timer on a backward jump and + // when ROS time is (de)activated, without calling the on reset callback + data->queue = this; + // the clock mutex serializes this with a jump callback in flight + std::scoped_lock clock_lock(data->clock->get_clock_mutex()); + if (rcl_timer_set_on_schedule_change_callback( + data->rcl_ref.get(), &TimerQueue::on_schedule_change, data.get()) != RCL_RET_OK) + { + assert(false); + } + } + { std::scoped_lock l(mutex); // this will wake up the timer thread if needed @@ -272,6 +293,35 @@ class TimerQueue } private: + /** + * Called by rcl after it moved next_call_time of a ROS time timer. Runs + * inside the clock's jump callback with the clock mutex held, which the + * timer thread takes while holding its wait mutex: it must not take a lock. + */ + static void on_schedule_change(const void * user_data, size_t) + { + auto * timer_data = static_cast(const_cast(user_data)); + timer_data->rekey_pending = true; + timer_data->queue->rekey_pending_any = true; + } + + /** + * Must run before the timer data is destroyed. The clock mutex serializes + * it with a jump callback in flight on the time source thread. + */ + void clear_on_schedule_change_callback(const TimerData & timer_data) + { + if (!timer_data.queue) { + return; + } + std::scoped_lock clock_lock(timer_data.clock->get_clock_mutex()); + if (rcl_timer_set_on_schedule_change_callback( + timer_data.rcl_ref.get(), nullptr, nullptr) != RCL_RET_OK) + { + assert(false); + } + } + /** * Wakes the timer thread. Must be called under lock * by mutex @@ -313,6 +363,7 @@ class TimerQueue ); if (it != all_timers.end()) { + clear_on_schedule_change_callback(**it); all_timers.erase(it); } return true; @@ -363,6 +414,19 @@ class TimerQueue } } + /** + * Re-keys the running timers whose next call time was moved by rcl. + * Must be called under lock by mutex. + */ + void rekey_pending_timers() + { + for (auto & timer_data : all_timers) { + if (timer_data->rekey_pending.exchange(false) && timer_data->in_running_list) { + add_timer_to_running_map(timer_data.get()); + } + } + } + std::vector> get_ready_timer_callbacks() { std::vector> ready_timer_callbacks; @@ -424,6 +488,9 @@ class TimerQueue rclcpp::Clock::SharedPtr used_clock; { std::scoped_lock l(mutex); + if (rekey_pending_any.exchange(false)) { + rekey_pending_timers(); + } ready_timer_callbacks = get_ready_timer_callbacks(); if(running_timers.empty()) { @@ -449,7 +516,7 @@ class TimerQueue std::unique_lock l(clock_waiter.mutex()); clock_waiter.wait_until(l, used_clock, rclcpp::Time(next_wakeup_time.count(), timer_type), [this] () -> bool { - return wake_up || !running || !rclcpp::ok(); + return wake_up || rekey_pending_any || !running || !rclcpp::ok(); }); wake_up = false; } catch (const std::runtime_error &) { @@ -478,6 +545,11 @@ class TimerQueue std::atomic_bool running = true; std::atomic_bool thread_terminated = false; +<<<<<<< HEAD +======= + // set by the schedule change callback, consumed by the timer thread + std::atomic_bool rekey_pending_any = false; +>>>>>>> ae09d1b (Re-key EventsCBGExecutor timers when rcl reschedules them (#3344)) std::vector> all_timers; diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 5ce4a7f028..407a95e639 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -496,6 +496,15 @@ if(TARGET test_executors_timer_cancel_behavior) target_link_libraries(test_executors_timer_cancel_behavior ${PROJECT_NAME} ${rosgraph_msgs_TARGETS}) endif() +ament_add_ros_isolated_gtest( + test_executors_time_jump + executors/test_executors_time_jump.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors_time_jump) + target_link_libraries(test_executors_time_jump ${PROJECT_NAME} rosgraph_msgs::rosgraph_msgs) +endif() + ament_add_ros_isolated_gtest( test_executors_callback_group_behavior executors/test_executors_callback_group_behavior.cpp diff --git a/rclcpp/test/rclcpp/executors/test_executors_time_jump.cpp b/rclcpp/test/rclcpp/executors/test_executors_time_jump.cpp new file mode 100644 index 0000000000..f127aa5a58 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_time_jump.cpp @@ -0,0 +1,148 @@ +// Copyright 2026 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/node_options.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/utilities.hpp" + +#include "rosgraph_msgs/msg/clock.hpp" + +#include "./executor_types.hpp" + +using namespace std::chrono_literals; + +// Timers on simulation time must keep firing when /clock jumps, backward as +// well as forward, and when simulation time is activated after the timer was +// created. The test publishes /clock itself so that the jumps are exact. +template +class TestExecutorsTimeJump : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() override + { + clock_node = std::make_shared("clock_publisher"); + clock_publisher = + clock_node->create_publisher("/clock", rclcpp::ClockQoS()); + } + + void TearDown() override + { + executor.cancel(); + if (spinner.joinable()) { + spinner.join(); + } + timer.reset(); + node.reset(); + clock_publisher.reset(); + clock_node.reset(); + } + + void create_node_and_spin(bool use_sim_time) + { + rclcpp::NodeOptions options; + if (use_sim_time) { + options.parameter_overrides({rclcpp::Parameter("use_sim_time", true)}); + } + node = std::make_shared("timer_node", options); + timer = node->create_timer(100ms, [this]() {fires++;}); + executor.add_node(node); + spinner = std::thread([this]() {executor.spin();}); + } + + void wait_for_clock_subscription() + { + const auto deadline = std::chrono::steady_clock::now() + 10s; + while (clock_publisher->get_subscription_count() == 0 && + std::chrono::steady_clock::now() < deadline) + { + std::this_thread::sleep_for(1ms); + } + ASSERT_NE(0u, clock_publisher->get_subscription_count()); + } + + // Publishes sim time from start_ns on in 10ms steps paced by real time and + // returns how often the timer fired meanwhile. Each second of sim time + // should produce about ten timer calls. + int play_clock(int64_t start_ns, std::chrono::nanoseconds sim_duration) + { + const int fires_before = fires; + const auto step = std::chrono::nanoseconds(10ms).count(); + rosgraph_msgs::msg::Clock msg; + for (int64_t elapsed = 0; elapsed < sim_duration.count(); elapsed += step) { + msg.clock = rclcpp::Time(start_ns + elapsed); + clock_publisher->publish(msg); + std::this_thread::sleep_for(5ms); + } + return fires - fires_before; + } + + std::shared_ptr clock_node; + rclcpp::Publisher::SharedPtr clock_publisher; + std::shared_ptr node; + rclcpp::TimerBase::SharedPtr timer; + std::atomic fires = 0; + std::thread spinner; + T executor; +}; + +// The EventsExecutor does not support simulation time, see +// https://github.com/ros2/rclcpp/issues/2457. +using TimeJumpExecutorTypes = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor, + rclcpp::executors::EventsCBGExecutor>; + +TYPED_TEST_SUITE(TestExecutorsTimeJump, TimeJumpExecutorTypes, ExecutorTypeNames); + +TYPED_TEST(TestExecutorsTimeJump, timer_runs_through_backward_jump) { + this->create_node_and_spin(true); + this->wait_for_clock_subscription(); + + const auto second = std::chrono::nanoseconds(1s).count(); + EXPECT_GE(this->play_clock(1000 * second, 1s), 5); + // backward by far more than one timer period + EXPECT_GE(this->play_clock(900 * second, 1s), 5); + EXPECT_GE(this->play_clock(1004 * second, 1s), 5); +} + +TYPED_TEST(TestExecutorsTimeJump, timer_runs_after_sim_time_activation) { + this->create_node_and_spin(false); + // the timer is scheduled on system time until the first /clock sample arrives + this->node->set_parameter(rclcpp::Parameter("use_sim_time", true)); + this->wait_for_clock_subscription(); + + const auto second = std::chrono::nanoseconds(1s).count(); + EXPECT_GE(this->play_clock(1000 * second, 1s), 5); +} From 436f463709c3afc62f5d2126a8eb4ec739821339 Mon Sep 17 00:00:00 2001 From: Alejandro Hernandez Cordero Date: Tue, 10 Nov 2026 09:43:57 +0100 Subject: [PATCH 2/2] fix conflicts Signed-off-by: Alejandro Hernandez Cordero --- .../executors/events_cbg_executor/timer_manager.hpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/rclcpp/src/rclcpp/executors/events_cbg_executor/timer_manager.hpp b/rclcpp/src/rclcpp/executors/events_cbg_executor/timer_manager.hpp index d1fd046f01..ce4213dffc 100644 --- a/rclcpp/src/rclcpp/executors/events_cbg_executor/timer_manager.hpp +++ b/rclcpp/src/rclcpp/executors/events_cbg_executor/timer_manager.hpp @@ -258,9 +258,10 @@ class TimerQueue return; } - std::unique_ptr data = std::make_unique(TimerData{std::move(handle), - timer, - timer->get_clock(), false, timer_ready_callback}); + // TimerData holds an atomic and is not movable, so construct it in place + std::unique_ptr data(new TimerData{std::move(handle), + timer, + timer->get_clock(), false, timer_ready_callback}); timer->set_on_reset_callback( [data_ptr = data.get(), this](size_t) { @@ -545,11 +546,8 @@ class TimerQueue std::atomic_bool running = true; std::atomic_bool thread_terminated = false; -<<<<<<< HEAD -======= // set by the schedule change callback, consumed by the timer thread std::atomic_bool rekey_pending_any = false; ->>>>>>> ae09d1b (Re-key EventsCBGExecutor timers when rcl reschedules them (#3344)) std::vector> all_timers;