Re-key EventsCBGExecutor timers after a time jump

rclcpp/src/rclcpp/executors/events_cbg_executor/timer_manager.hpp

@@ -16,7 +16,9 @@
 
 #include <rcl/timer.h>
 
+#include <atomic>
 #include <chrono>
+#include <cstddef>
 #include <functional>
 #include <map>
 #include <memory>
@@ -136,6 +138,10 @@ class TimerQueue
     rclcpp::Clock::SharedPtr clock;
     bool in_running_list = false;
     std::function<void(const std::function<void()> & executed_cb)> timer_ready_callback;
+    // set by rcl when it moved next_call_time, consumed by the timer thread
+    std::atomic<bool> 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<TimerData *>(const_cast<void *>(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<std::function<void()>> get_ready_timer_callbacks()
   {
     std::vector<std::function<void()>> 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<std::mutex> 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<std::shared_ptr<TimerData>> all_timers;
 

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

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 <gtest/gtest.h>
+
+#include <atomic>
+#include <chrono>
+#include <cstdint>
+#include <memory>
+#include <thread>
+
+#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<typename T>
+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<rclcpp::Node>("clock_publisher");
+    clock_publisher =
+      clock_node->create_publisher<rosgraph_msgs::msg::Clock>("/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<rclcpp::Node>("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<rclcpp::Node> clock_node;
+  rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_publisher;
+  std::shared_ptr<rclcpp::Node> node;
+  rclcpp::TimerBase::SharedPtr timer;
+  std::atomic<int> 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);
+}