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); +}