From 3637cae94f3ee27c00da4dcf91489288a3a28d28 Mon Sep 17 00:00:00 2001 From: Yadunund Vijay Date: Wed, 14 Oct 2026 15:56:54 -0700 Subject: [PATCH] Re-key EventsCBGExecutor timers when rcl reschedules them 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 --- .../events_cbg_executor/timer_manager.hpp | 71 ++++++++- rclcpp/test/rclcpp/CMakeLists.txt | 9 ++ .../executors/test_executors_time_jump.cpp | 148 ++++++++++++++++++ 3 files changed, 227 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 3bbef98cc3..77243dbc18 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 @@ -136,6 +138,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: @@ -171,6 +177,7 @@ class TimerQueue if (auto shrPtr = tData->timer_ref.lock()) { shrPtr->clear_on_reset_callback(); } + clear_on_schedule_change_callback(*tData); } // Release all references now, as the queue itself may outlive the libraries @@ -232,6 +239,7 @@ class TimerQueue if(it2 != running_timers.end()) { running_timers.erase(it2); } + clear_on_schedule_change_callback(**it); all_timers.erase(it); } @@ -280,6 +288,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 @@ -290,6 +311,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 @@ -331,6 +381,7 @@ class TimerQueue ); if (it != all_timers.end()) { + clear_on_schedule_change_callback(**it); all_timers.erase(it); } return true; @@ -381,6 +432,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; @@ -442,6 +506,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()) { @@ -467,7 +534,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 &) { @@ -496,6 +563,8 @@ class TimerQueue std::atomic_bool running = true; std::atomic_bool thread_terminated = false; + // set by the schedule change callback, consumed by the timer thread + std::atomic_bool rekey_pending_any = false; std::vector> all_timers; diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index e43ce91ffe..a287917581 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -492,6 +492,15 @@ if(TARGET test_executors_timer_cancel_behavior) target_link_libraries(test_executors_timer_cancel_behavior ${PROJECT_NAME} rosgraph_msgs::rosgraph_msgs) 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); +}