From 0e2349c760d335e04f03843a16809c4864592608 Mon Sep 17 00:00:00 2001 From: Thomas Moore Date: Fri, 3 May 2024 17:38:36 +0000 Subject: [PATCH 1/4] Allow for specifying an initial timer trigger time Signed-off-by: Thomas Moore --- rclcpp/include/rclcpp/create_timer.hpp | 94 ++++++++++++++++++++++++++ rclcpp/include/rclcpp/node.hpp | 49 ++++++++++++++ rclcpp/include/rclcpp/node_impl.hpp | 37 ++++++++++ rclcpp/include/rclcpp/timer.hpp | 70 +++++++++++++++++++ rclcpp/src/rclcpp/timer.cpp | 46 +++++++++++++ 5 files changed, 296 insertions(+) diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index 34509fa35a..0118bc1a4e 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -170,6 +170,56 @@ create_timer( return timer; } +/// Convenience method to create a general timer with an initial trigger time using node resources. +/** + * + * \tparam DurationRepT + * \tparam DurationT + * \tparam CallbackT + * \param clock clock to be used + * \param initial_call_time time at which the callback should be initially triggered + * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() + * \param callback callback to execute via the timer period + * \param group callback group + * \param node_base node base interface + * \param node_timers node timer interface + * \param autostart defines if the timer should start it's countdown on initialization or not. + * \return shared pointer to a generic timer + * \throws std::invalid_argument if either clock, node_base or node_timers + * are nullptr, or period is negative or too large + */ +template +typename rclcpp::GenericTimer::SharedPtr +create_timer( + rclcpp::Clock::SharedPtr clock, + Time initial_call_time, + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group, + node_interfaces::NodeBaseInterface * node_base, + node_interfaces::NodeTimersInterface * node_timers, + bool autostart = true) +{ + if (clock == nullptr) { + throw std::invalid_argument{"clock cannot be null"}; + } + if (node_base == nullptr) { + throw std::invalid_argument{"input node_base cannot be null"}; + } + if (node_timers == nullptr) { + throw std::invalid_argument{"input node_timers cannot be null"}; + } + + const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); + + // Add a new generic timer. + auto timer = rclcpp::GenericTimer::make_shared( + std::move(clock), initial_call_time, period_ns, std::move(callback), + node_base->get_context(), autostart); + node_timers->add_timer(timer, group); + return timer; +} + /// Convenience method to create a wall timer with node resources. /** * @@ -211,6 +261,50 @@ create_wall_timer( node_timers->add_timer(timer, group); return timer; } + +/// Convenience method to create a wall timer with an initial trigger time using node resources. +/** + * + * \tparam DurationRepT + * \tparam DurationT + * \tparam CallbackT + * \param initial_call_time time at which the callback should be initially triggered + * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() + * \param callback callback to execute via the timer period + * \param group callback group + * \param node_base node base interface + * \param node_timers node timer interface + * \return shared pointer to a wall timer + * \throws std::invalid_argument if either node_base or node_timers + * are null, or period is negative or too large + */ +template +typename rclcpp::WallTimer::SharedPtr +create_wall_timer( + Time initial_call_time, + std::chrono::duration period, + CallbackT callback, + rclcpp::CallbackGroup::SharedPtr group, + node_interfaces::NodeBaseInterface * node_base, + node_interfaces::NodeTimersInterface * node_timers, + bool autostart = true) +{ + if (node_base == nullptr) { + throw std::invalid_argument{"input node_base cannot be null"}; + } + + if (node_timers == nullptr) { + throw std::invalid_argument{"input node_timers cannot be null"}; + } + + const std::chrono::nanoseconds period_ns = detail::safe_cast_to_period_in_ns(period); + + // Add a new wall timer. + auto timer = rclcpp::WallTimer::make_shared( + initial_call_time, period_ns, std::move(callback), node_base->get_context(), autostart); + node_timers->add_timer(timer, group); + return timer; +} } // namespace rclcpp #endif // RCLCPP__CREATE_TIMER_HPP_ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index e8a1273c63..5f8beee02f 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -242,6 +242,24 @@ class Node : public std::enable_shared_from_this const rclcpp::CallbackGroup::SharedPtr & group = nullptr, bool autostart = true); + /// Create a wall timer that uses the wall clock to drive the callback with an initial trigger + /// time. + /** + * \param[in] initial_call_time Time at which the callback should be initially triggered. + * \param[in] period Time interval between triggers of the callback. + * \param[in] callback User-defined callback function. + * \param[in] group Callback group to execute this timer's callback in. + * \param[in] autostart The state of the clock on initialization. + */ + template + typename rclcpp::WallTimer::SharedPtr + create_wall_timer( + Time initial_call_time, + std::chrono::duration period, + CallbackT callback, + const rclcpp::CallbackGroup::SharedPtr & group = nullptr, + bool autostart = true); + /// Create a timer that uses the node clock to drive the callback. /** * \param[in] period Time interval between triggers of the callback. @@ -255,6 +273,37 @@ class Node : public std::enable_shared_from_this CallbackT callback, const rclcpp::CallbackGroup::SharedPtr & group = nullptr); + /// Create a timer that uses the node clock to drive the callback with an initial trigger time. + /** + * \param[in] initial_call_time Time at which the callback should be initially triggered. + * \param[in] period Time interval between triggers of the callback. + * \param[in] callback User-defined callback function. + * \param[in] group Callback group to execute this timer's callback in. + */ + template + typename rclcpp::GenericTimer::SharedPtr + create_timer( + Time initial_call_time, + std::chrono::duration period, + CallbackT callback, + const rclcpp::CallbackGroup::SharedPtr & group = nullptr); + + /// Create and return a Client. + /** + * \param[in] service_name The topic to service on. + * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client. + * \param[in] group Callback group to call the service. + * \return Shared pointer to the created client. + * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t + */ + template + [[deprecated("use rclcpp::QoS instead of rmw_qos_profile_t")]] + typename rclcpp::Client::SharedPtr + create_client( + const std::string & service_name, + const rmw_qos_profile_t & qos_profile, + rclcpp::CallbackGroup::SharedPtr group = nullptr); + /// Create and return a Client. /** * \param[in] service_name The name on which the service is accessible. diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index df2c49715a..82c4d8b6c5 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -116,15 +116,52 @@ Node::create_wall_timer( autostart); } +template +typename rclcpp::WallTimer::SharedPtr +Node::create_wall_timer( + rclcpp::Time initial_call_time, + std::chrono::duration period, + CallbackT callback, + const rclcpp::CallbackGroup::SharedPtr & group, + bool autostart) +{ + return rclcpp::create_wall_timer( + initial_call_time, + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get(), + autostart); +} + +template +typename rclcpp::GenericTimer::SharedPtr +Node::create_timer( + std::chrono::duration period, + CallbackT callback, + const rclcpp::CallbackGroup::SharedPtr & group) +{ + return rclcpp::create_timer( + this->get_clock(), + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get()); +} + template typename rclcpp::GenericTimer::SharedPtr Node::create_timer( + rclcpp::Time initial_call_time, std::chrono::duration period, CallbackT callback, const rclcpp::CallbackGroup::SharedPtr & group) { return rclcpp::create_timer( this->get_clock(), + initial_call_time, period, std::move(callback), group, diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index dd229da4e1..88ab69dbaa 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -69,6 +69,25 @@ class TimerBase rclcpp::Context::SharedPtr context, bool autostart = true); + /// Constructor allowing for specification of an initial trigger time + /** + * \param clock A clock to use for time and sleeping + * \param initial_call_time The time at which the callback should be initially triggered + * \param period The interval at which the timer fires + * \param context node context + * \param autostart timer state on initialization + * + * In order to activate a timer that is not started on initialization, + * user should call the reset() method. + */ + RCLCPP_PUBLIC + explicit TimerBase( + Clock::SharedPtr clock, + Time initial_call_time, + std::chrono::nanoseconds period, + rclcpp::Context::SharedPtr context, + bool autostart = true); + /// TimerBase destructor RCLCPP_PUBLIC virtual @@ -258,6 +277,38 @@ class GenericTimer : public TimerBase #endif } + /// Constructor allowing for specification of an initial trigger time. + /** + * \param[in] clock The clock providing the current time. + * \param[in] initial_call_time The time at which the callback should be initially triggered. + * \param[in] period The interval at which the timer fires. + * \param[in] callback User-specified callback function. + * \param[in] context custom context to be used. + * \param autostart timer state on initialization + */ + explicit GenericTimer( + Clock::SharedPtr clock, Time initial_call_time, std::chrono::nanoseconds period, + FunctorT && callback, rclcpp::Context::SharedPtr context, bool autostart = true + ) + : TimerBase(clock, initial_call_time, period, context, autostart), + callback_(std::forward(callback)) + { + TRACETOOLS_TRACEPOINT( + rclcpp_timer_callback_added, + static_cast(get_timer_handle().get()), + reinterpret_cast(&callback_)); +#ifndef TRACETOOLS_DISABLED + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { + char * symbol = tracetools::get_symbol(callback_); + TRACETOOLS_DO_TRACEPOINT( + rclcpp_callback_register, + reinterpret_cast(&callback_), + symbol); + std::free(symbol); + } +#endif + } + /// Default destructor. virtual ~GenericTimer() { @@ -377,6 +428,25 @@ class WallTimer : public GenericTimer std::make_shared(RCL_STEADY_TIME), period, std::move(callback), context, autostart) {} + /// Wall timer constructor allowing for specification of an initial trigger time + /** + * \param initial_call_time The time at which the callback should be initially triggered. + * \param period The interval at which the timer fires + * \param callback The callback function to execute every interval + * \param context node context + * \param autostart timer state on initialization + */ + WallTimer( + Time initial_call_time, + std::chrono::nanoseconds period, + FunctorT && callback, + rclcpp::Context::SharedPtr context, + bool autostart = true) + : GenericTimer( + std::make_shared(RCL_STEADY_TIME), initial_call_time, period, + std::move(callback), context, autostart) + {} + protected: RCLCPP_DISABLE_COPY(WallTimer) }; diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index fe3f24e968..5361f85afb 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -73,6 +73,52 @@ TimerBase::TimerBase( } } +TimerBase::TimerBase( + rclcpp::Clock::SharedPtr clock, + rclcpp::Time initial_call_time, + std::chrono::nanoseconds period, + rclcpp::Context::SharedPtr context, + bool autostart) +: clock_(clock), timer_handle_(nullptr) +{ + if (nullptr == context) { + context = rclcpp::contexts::get_global_default_context(); + } + + auto rcl_context = context->get_rcl_context(); + + timer_handle_ = std::shared_ptr( + new rcl_timer_t, [ = ](rcl_timer_t * timer) mutable + { + { + std::lock_guard clock_guard(clock->get_clock_mutex()); + if (rcl_timer_fini(timer) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rclcpp", + "Failed to clean up rcl timer handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + } + delete timer; + // Captured shared pointers by copy, reset to make sure timer is finalized before clock + clock.reset(); + rcl_context.reset(); + }); + + *timer_handle_.get() = rcl_get_zero_initialized_timer(); + + rcl_clock_t * clock_handle = clock_->get_clock_handle(); + { + std::lock_guard clock_guard(clock_->get_clock_mutex()); + rcl_ret_t ret = rcl_timer_init3( + timer_handle_.get(), clock_handle, rcl_context.get(), initial_call_time.nanoseconds(), + period.count(), nullptr, rcl_get_default_allocator(), autostart); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize rcl timer handle"); + } + } +} + TimerBase::~TimerBase() { clear_on_reset_callback(); From 736d3ed693c7cc13e4fa160d2f3e397c1b56f9f6 Mon Sep 17 00:00:00 2001 From: Thomas Moore Date: Wed, 2 Sep 2026 21:09:57 +0000 Subject: [PATCH 2/4] Add tests for timer/create_timer initial call time overloads Signed-off-by: Thomas Moore --- rclcpp/test/rclcpp/test_create_timer.cpp | 153 +++++++++++++++++++++++ rclcpp/test/rclcpp/test_timer.cpp | 50 ++++++++ 2 files changed, 203 insertions(+) diff --git a/rclcpp/test/rclcpp/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index 9995c7136c..92da11069f 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -196,6 +196,159 @@ TEST(TestCreateTimer, timer_function_pointer) rclcpp::shutdown(); } +TEST(TestCreateTimer, call_timer_with_initial_call_time_bad_arguments) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_timers_with_initial_call_time_bad_arguments"); + auto callback = []() {}; + rclcpp::CallbackGroup::SharedPtr group = nullptr; + auto node_interface = + rclcpp::node_interfaces::get_node_base_interface(node).get(); + auto timers_interface = + rclcpp::node_interfaces::get_node_timers_interface(node).get(); + + auto clock = node.get_node_clock_interface()->get_clock(); + auto initial_call_time = clock->now() + rclcpp::Duration(1s); + + // Negative period + EXPECT_THROW( + rclcpp::create_timer( + clock, initial_call_time, -1ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // clock is null + EXPECT_THROW( + rclcpp::create_timer( + nullptr, initial_call_time, 1ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // node_interface is null + EXPECT_THROW( + rclcpp::create_timer(clock, initial_call_time, 1ms, callback, group, nullptr, timers_interface), + std::invalid_argument); + + // timers_interface is null + EXPECT_THROW( + rclcpp::create_timer(clock, initial_call_time, 1ms, callback, group, node_interface, nullptr), + std::invalid_argument); + + rclcpp::shutdown(); +} + +TEST(TestCreateTimer, call_timer_honors_initial_call_time) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_timer_honors_initial_call_time"); + auto node_interface = + rclcpp::node_interfaces::get_node_base_interface(node).get(); + auto timers_interface = + rclcpp::node_interfaces::get_node_timers_interface(node).get(); + auto clock = node.get_node_clock_interface()->get_clock(); + + const auto period = 50ms; + const auto initial_delay = 10s; + auto initial_call_time = clock->now() + rclcpp::Duration(initial_delay); + + auto timer = rclcpp::create_timer( + clock, initial_call_time, period, []() {}, nullptr, node_interface, timers_interface); + + // The next call should be driven by initial_call_time, not by now + period. + EXPECT_GT( + timer->time_until_trigger().count(), + std::chrono::nanoseconds(period).count()); + EXPECT_LE( + timer->time_until_trigger().count(), + std::chrono::nanoseconds(initial_delay).count()); + + timer->cancel(); + rclcpp::shutdown(); +} + +TEST(TestCreateTimer, call_timer_forwards_autostart_regardless_of_initial_call_time_overload) +{ + // Regression test: rclcpp::create_timer(clock, period, callback, group, node_base, node_timers, + // autostart) must honor a false autostart, rather than always starting the timer, when it + // delegates to the initial-call-time overload internally. + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_timer_forwards_autostart"); + auto node_interface = + rclcpp::node_interfaces::get_node_base_interface(node).get(); + auto timers_interface = + rclcpp::node_interfaces::get_node_timers_interface(node).get(); + auto clock = node.get_node_clock_interface()->get_clock(); + + auto timer = rclcpp::create_timer( + clock, 100ms, []() {}, nullptr, node_interface, timers_interface, false); + + EXPECT_TRUE(timer->is_canceled()); + timer->reset(); + EXPECT_FALSE(timer->is_canceled()); + timer->cancel(); + + rclcpp::shutdown(); +} + +TEST(TestCreateWallTimer, call_wall_timer_with_initial_call_time_bad_arguments) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_wall_timer_with_initial_call_time_bad_arguments"); + auto callback = []() {}; + rclcpp::CallbackGroup::SharedPtr group = nullptr; + auto node_interface = + rclcpp::node_interfaces::get_node_base_interface(node).get(); + auto timers_interface = + rclcpp::node_interfaces::get_node_timers_interface(node).get(); + + auto initial_call_time = rclcpp::Clock(RCL_STEADY_TIME).now() + rclcpp::Duration(1s); + + // Negative period + EXPECT_THROW( + rclcpp::create_wall_timer( + initial_call_time, -1ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // node_interface is null + EXPECT_THROW( + rclcpp::create_wall_timer(initial_call_time, 1ms, callback, group, nullptr, timers_interface), + std::invalid_argument); + + // timers_interface is null + EXPECT_THROW( + rclcpp::create_wall_timer(initial_call_time, 1ms, callback, group, node_interface, nullptr), + std::invalid_argument); + + rclcpp::shutdown(); +} + +TEST(TestCreateWallTimer, call_wall_timer_honors_initial_call_time) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_wall_timer_honors_initial_call_time"); + auto node_interface = + rclcpp::node_interfaces::get_node_base_interface(node).get(); + auto timers_interface = + rclcpp::node_interfaces::get_node_timers_interface(node).get(); + + const auto period = 50ms; + const auto initial_delay = 10s; + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + auto initial_call_time = steady_clock.now() + rclcpp::Duration(initial_delay); + + auto timer = rclcpp::create_wall_timer( + initial_call_time, period, []() {}, nullptr, node_interface, timers_interface); + + // The next call should be driven by initial_call_time, not by now + period. + EXPECT_GT( + timer->time_until_trigger().count(), + std::chrono::nanoseconds(period).count()); + EXPECT_LE( + timer->time_until_trigger().count(), + std::chrono::nanoseconds(initial_delay).count()); + + timer->cancel(); + rclcpp::shutdown(); +} + TEST(TestCreateTimer, timer_without_autostart) { rclcpp::init(0, nullptr); diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 108ad4317e..d1dcbdbc42 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -233,6 +233,56 @@ TEST_P(TestTimer, test_bad_arguments) { EXPECT_THROW( rclcpp::GenericTimer(unitialized_clock, 1us, []() {}, context), rclcpp::exceptions::RCLError); + + // Same set of checks, but using the constructor that takes an explicit initial call time. + auto now = steady_clock->now(); + + // Negative period + EXPECT_THROW( + rclcpp::GenericTimer(steady_clock, now, -1ms, []() {}, context), + rclcpp::exceptions::RCLInvalidArgument); + + // 0 duration period, should be ok + EXPECT_NO_THROW( + rclcpp::GenericTimer(steady_clock, now, 0ms, []() {}, context)); + + // Clock is unitialized + EXPECT_THROW( + rclcpp::GenericTimer(unitialized_clock, now, 1us, []() {}, context), + rclcpp::exceptions::RCLError); +} + +TEST_P(TestTimer, test_initial_call_time) +{ + const auto period = 50ms; + const auto initial_delay = std::chrono::seconds(10); + + std::shared_ptr initial_time_timer; + switch (timer_type) { + case TimerType::WALL_TIMER: + { + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + initial_time_timer = test_node->create_wall_timer( + steady_clock.now() + rclcpp::Duration(initial_delay), period, []() {}); + break; + } + case TimerType::GENERIC_TIMER: + { + initial_time_timer = test_node->create_timer( + test_node->get_clock()->now() + rclcpp::Duration(initial_delay), period, []() {}); + break; + } + } + + // The next call should be driven by initial_call_time, not by now + period. + EXPECT_GT( + initial_time_timer->time_until_trigger().count(), + std::chrono::nanoseconds(period).count()); + EXPECT_LE( + initial_time_timer->time_until_trigger().count(), + std::chrono::nanoseconds(initial_delay).count()); + + initial_time_timer->cancel(); } TEST_P(TestTimer, callback_with_timer) { From 5a1e0e4abc80e3a13591cc71de0b8c693d2a7ed6 Mon Sep 17 00:00:00 2001 From: Thomas Moore Date: Mon, 14 Sep 2026 16:28:55 +0000 Subject: [PATCH 3/4] Validate initial_call_time's clock type matches the timer's clock WallTimer (and create_wall_timer) always construct their underlying Clock as RCL_STEADY_TIME, but the new initial_call_time overloads accepted a Time of any clock type without checking it. Passing e.g. a ROS-time Time into create_wall_timer silently produced a garbage next_call_time, since rcl treats the value as a bare nanosecond count with no cross-clock conversion. TimerBase's initial_call_time constructor now throws std::runtime_error if initial_call_time's clock type doesn't match the timer's clock, consistent with the existing clock-type-mismatch checks in Time::operator- and Clock::sleep_until. The check is skipped when the clock itself is uninitialized, leaving that case to the existing rcl-level validation. Signed-off-by: Thomas Moore Co-Authored-By: Claude Sonnet 5 --- rclcpp/include/rclcpp/create_timer.hpp | 2 ++ rclcpp/include/rclcpp/timer.hpp | 6 ++++++ rclcpp/src/rclcpp/timer.cpp | 9 +++++++++ rclcpp/test/rclcpp/test_create_timer.cpp | 14 ++++++++++++++ rclcpp/test/rclcpp/test_timer.cpp | 13 +++++++++++++ 5 files changed, 44 insertions(+) diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index 0118bc1a4e..fb3b48db08 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -187,6 +187,7 @@ create_timer( * \return shared pointer to a generic timer * \throws std::invalid_argument if either clock, node_base or node_timers * are nullptr, or period is negative or too large + * \throws std::runtime_error if initial_call_time's clock type does not match clock's */ template typename rclcpp::GenericTimer::SharedPtr @@ -277,6 +278,7 @@ create_wall_timer( * \return shared pointer to a wall timer * \throws std::invalid_argument if either node_base or node_timers * are null, or period is negative or too large + * \throws std::runtime_error if initial_call_time's clock type is not RCL_STEADY_TIME */ template typename rclcpp::WallTimer::SharedPtr diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 88ab69dbaa..0482e555c1 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -79,6 +79,8 @@ class TimerBase * * In order to activate a timer that is not started on initialization, * user should call the reset() method. + * + * \throws std::runtime_error if initial_call_time's clock type does not match clock's */ RCLCPP_PUBLIC explicit TimerBase( @@ -285,6 +287,8 @@ class GenericTimer : public TimerBase * \param[in] callback User-specified callback function. * \param[in] context custom context to be used. * \param autostart timer state on initialization + * + * \throws std::runtime_error if initial_call_time's clock type does not match clock's */ explicit GenericTimer( Clock::SharedPtr clock, Time initial_call_time, std::chrono::nanoseconds period, @@ -435,6 +439,8 @@ class WallTimer : public GenericTimer * \param callback The callback function to execute every interval * \param context node context * \param autostart timer state on initialization + * + * \throws std::runtime_error if initial_call_time's clock type is not RCL_STEADY_TIME */ WallTimer( Time initial_call_time, diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 5361f85afb..d0dd25f644 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -81,6 +81,15 @@ TimerBase::TimerBase( bool autostart) : clock_(clock), timer_handle_(nullptr) { + // An uninitialized clock is rejected below by rcl_timer_init3 itself; skip the type + // comparison here so that pre-existing error path is still what surfaces in that case. + if (clock_->get_clock_type() != RCL_CLOCK_UNINITIALIZED && + initial_call_time.get_clock_type() != clock_->get_clock_type()) + { + throw std::runtime_error( + "initial_call_time's clock type does not match clock's clock type"); + } + if (nullptr == context) { context = rclcpp::contexts::get_global_default_context(); } diff --git a/rclcpp/test/rclcpp/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index 92da11069f..3560c8b41f 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -232,6 +232,20 @@ TEST(TestCreateTimer, call_timer_with_initial_call_time_bad_arguments) rclcpp::create_timer(clock, initial_call_time, 1ms, callback, group, node_interface, nullptr), std::invalid_argument); + // initial_call_time's clock type does not match clock's clock type + rclcpp::Time mismatched_clock_type_time(static_cast(0), RCL_STEADY_TIME); + EXPECT_THROW( + rclcpp::create_timer( + clock, mismatched_clock_type_time, 1ms, callback, group, node_interface, timers_interface), + std::runtime_error); + + // Same check for create_wall_timer, which always uses a steady clock internally regardless of + // what clock type initial_call_time was constructed with. + EXPECT_THROW( + rclcpp::create_wall_timer( + initial_call_time, 1ms, callback, group, node_interface, timers_interface), + std::runtime_error); + rclcpp::shutdown(); } diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index d1dcbdbc42..7400ada943 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -250,6 +250,19 @@ TEST_P(TestTimer, test_bad_arguments) { EXPECT_THROW( rclcpp::GenericTimer(unitialized_clock, now, 1us, []() {}, context), rclcpp::exceptions::RCLError); + + // initial_call_time's clock type does not match the timer's clock + rclcpp::Time mismatched_clock_type_time(static_cast(0), RCL_SYSTEM_TIME); + EXPECT_THROW( + rclcpp::GenericTimer( + steady_clock, mismatched_clock_type_time, 1ms, []() {}, context), + std::runtime_error); + + // Same check for WallTimer, which always uses a steady clock internally regardless of what + // clock type initial_call_time was constructed with. + EXPECT_THROW( + rclcpp::WallTimer(mismatched_clock_type_time, 1ms, []() {}, context), + std::runtime_error); } TEST_P(TestTimer, test_initial_call_time) From 8e7fa089201096d97da2f1cf84822d7d92e65042 Mon Sep 17 00:00:00 2001 From: Thomas Moore Date: Tue, 15 Sep 2026 15:27:25 +0000 Subject: [PATCH 4/4] Add TimerBase::resume() and a phase-aligned start time helper Adds rclcpp::TimerBase::resume(), a thin wrapper over the new rcl_timer_resume() (see the companion rcl change): unlike reset(), it preserves the timer's existing schedule phase, only catching up if it is overdue rather than unconditionally recomputing from now(). This is what makes autostart=false combined with an explicit initial_call_time actually useful: a timer can be created paused with a specific phase-anchored schedule and later resumed without losing it, which reset() cannot do. Also fixes a gap found while writing this: Node::create_timer( initial_call_time, ...) had no way to pass autostart at all, unlike its create_wall_timer sibling overload, even though the underlying free function already supported it. Adds rclcpp::compute_phase_aligned_time(clock, interval, phase): a small utility to compute the smallest instant >= now() of the form k * interval + phase. This lets independent nodes/processes sharing a synchronized clock agree on the same aligned timer start instants without exchanging an explicit time out-of-band. Renamed the internal call from rcl_timer_init3 to rcl_timer_init_with_start_time to match the companion rcl rename. Signed-off-by: Thomas Moore Co-Authored-By: Claude Sonnet 5 --- rclcpp/include/rclcpp/node.hpp | 4 +- rclcpp/include/rclcpp/node_impl.hpp | 6 +- rclcpp/include/rclcpp/timer.hpp | 46 ++++++++ rclcpp/src/rclcpp/timer.cpp | 55 ++++++++- rclcpp/test/rclcpp/test_timer.cpp | 170 ++++++++++++++++++++++++++++ 5 files changed, 275 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 5f8beee02f..b0d071a9f7 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -279,6 +279,7 @@ class Node : public std::enable_shared_from_this * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. * \param[in] group Callback group to execute this timer's callback in. + * \param[in] autostart The state of the timer on initialization. */ template typename rclcpp::GenericTimer::SharedPtr @@ -286,7 +287,8 @@ class Node : public std::enable_shared_from_this Time initial_call_time, std::chrono::duration period, CallbackT callback, - const rclcpp::CallbackGroup::SharedPtr & group = nullptr); + const rclcpp::CallbackGroup::SharedPtr & group = nullptr, + bool autostart = true); /// Create and return a Client. /** diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 82c4d8b6c5..9bb3c7160a 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -157,7 +157,8 @@ Node::create_timer( rclcpp::Time initial_call_time, std::chrono::duration period, CallbackT callback, - const rclcpp::CallbackGroup::SharedPtr & group) + const rclcpp::CallbackGroup::SharedPtr & group, + bool autostart) { return rclcpp::create_timer( this->get_clock(), @@ -166,7 +167,8 @@ Node::create_timer( std::move(callback), group, this->node_base_.get(), - this->node_timers_.get()); + this->node_timers_.get(), + autostart); } template diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 0482e555c1..f6746aaee7 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -47,6 +47,30 @@ struct TimerInfo Time actual_call_time; }; +/// Compute a phase-aligned start time for a periodic timer. +/** + * The result is the smallest time greater than or equal to `clock.now()` + * of the form `k * interval + phase` for some non-negative integer `k`. + * + * This is useful for synchronizing periodic timers across multiple nodes + * or processes that share a common, synchronized clock (e.g. ROS time) + * without needing to exchange an explicit start time out-of-band: any two + * callers with a synchronized clock computing this function with the same + * interval and phase will agree on the same sequence of aligned instants. + * + * \param[in] clock clock used to obtain the current time + * \param[in] interval alignment interval; must be greater than zero + * \param[in] phase offset added to each interval boundary + * \return the computed, phase-aligned start time, using clock's clock type + * \throws std::invalid_argument if interval is not greater than zero + */ +RCLCPP_PUBLIC +Time +compute_phase_aligned_time( + const Clock & clock, + std::chrono::nanoseconds interval, + std::chrono::nanoseconds phase = std::chrono::nanoseconds(0)); + class TimerBase { public: @@ -121,6 +145,28 @@ class TimerBase void reset(); + /// Resume the timer, preserving its existing schedule phase. + /** + * Unlike reset(), this does not unconditionally recompute the next call + * time from the current time; if the timer's next call time is still in + * the future, it is left unchanged. + * If it is in the past (e.g. because the timer was canceled and is being + * resumed some time later), it is advanced by whole periods until it is + * in the future again, without shifting the phase established when the + * timer was initialized (or last had its next call time explicitly set). + * A canceled timer is also made not canceled by this call. + * + * This makes it possible to initialize a timer with autostart false and + * an explicit initial call time, and later resume it without losing the + * originally intended schedule, which is not possible with reset() since + * it always recomputes the next call time as now() + period. + * + * \throws std::runtime_error if the rcl_timer_resume returns a failure + */ + RCLCPP_PUBLIC + void + resume(); + /// Indicate that we're about to execute the callback. /** * The multithreaded executor takes advantage of this to avoid scheduling diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index d0dd25f644..9ceaf9c1ac 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -15,6 +15,7 @@ #include "rclcpp/timer.hpp" #include +#include #include #include @@ -81,8 +82,8 @@ TimerBase::TimerBase( bool autostart) : clock_(clock), timer_handle_(nullptr) { - // An uninitialized clock is rejected below by rcl_timer_init3 itself; skip the type - // comparison here so that pre-existing error path is still what surfaces in that case. + // An uninitialized clock is rejected below by rcl_timer_init_with_start_time itself; skip + // the type comparison here so that pre-existing error path is still what surfaces in that case. if (clock_->get_clock_type() != RCL_CLOCK_UNINITIALIZED && initial_call_time.get_clock_type() != clock_->get_clock_type()) { @@ -119,7 +120,7 @@ TimerBase::TimerBase( rcl_clock_t * clock_handle = clock_->get_clock_handle(); { std::lock_guard clock_guard(clock_->get_clock_mutex()); - rcl_ret_t ret = rcl_timer_init3( + rcl_ret_t ret = rcl_timer_init_with_start_time( timer_handle_.get(), clock_handle, rcl_context.get(), initial_call_time.nanoseconds(), period.count(), nullptr, rcl_get_default_allocator(), autostart); if (ret != RCL_RET_OK) { @@ -166,6 +167,19 @@ TimerBase::reset() } } +void +TimerBase::resume() +{ + rcl_ret_t ret = RCL_RET_OK; + { + std::lock_guard lock(callback_mutex_); + ret = rcl_timer_resume(timer_handle_.get()); + } + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't resume timer"); + } +} + bool TimerBase::is_ready() { @@ -276,3 +290,38 @@ const rclcpp::Clock::SharedPtr & TimerBase::get_clock() const { return clock_; } + +namespace +{ +/// Floor division for a positive divisor (unlike operator/, rounds toward negative infinity). +int64_t +floor_div(int64_t dividend, int64_t positive_divisor) +{ + int64_t quotient = dividend / positive_divisor; + if (dividend % positive_divisor != 0 && dividend < 0) { + --quotient; + } + return quotient; +} +} // namespace + +rclcpp::Time +rclcpp::compute_phase_aligned_time( + const rclcpp::Clock & clock, + std::chrono::nanoseconds interval, + std::chrono::nanoseconds phase) +{ + if (interval <= std::chrono::nanoseconds(0)) { + throw std::invalid_argument("interval must be greater than zero"); + } + const rclcpp::Time now = clock.now(); + const int64_t now_ns = now.nanoseconds(); + const int64_t interval_ns = interval.count(); + const int64_t phase_ns = phase.count(); + const int64_t k = floor_div(now_ns - phase_ns, interval_ns); + int64_t aligned_ns = k * interval_ns + phase_ns; + if (aligned_ns < now_ns) { + aligned_ns += interval_ns; + } + return rclcpp::Time(aligned_ns, now.get_clock_type()); +} diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 7400ada943..b2404b9092 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -22,6 +22,7 @@ #include #include +#include "rcl/time.h" #include "rcl/timer.h" #include "rclcpp/clock.hpp" @@ -298,6 +299,111 @@ TEST_P(TestTimer, test_initial_call_time) initial_time_timer->cancel(); } +TEST_P(TestTimer, resume_does_not_advance_if_not_yet_due) +{ + const auto period = 50ms; + const auto initial_delay = std::chrono::seconds(10); + + std::shared_ptr timer; + switch (timer_type) { + case TimerType::WALL_TIMER: + { + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + timer = test_node->create_wall_timer( + steady_clock.now() + rclcpp::Duration(initial_delay), period, []() {}, + nullptr, false); + break; + } + case TimerType::GENERIC_TIMER: + { + timer = test_node->create_timer( + test_node->get_clock()->now() + rclcpp::Duration(initial_delay), period, []() {}, + nullptr, false); + break; + } + } + + EXPECT_TRUE(timer->is_canceled()); + timer->resume(); + EXPECT_FALSE(timer->is_canceled()); + + // The original phase-anchored schedule should be preserved, not recomputed from now(). + EXPECT_GT( + timer->time_until_trigger().count(), + std::chrono::nanoseconds(period).count()); + EXPECT_LE( + timer->time_until_trigger().count(), + std::chrono::nanoseconds(initial_delay).count()); + + timer->cancel(); +} + +TEST_P(TestTimer, resume_catches_up_if_overdue) +{ + // Simulate a timer that was paused (canceled) for much longer than several periods. + const auto period = 100ms; + const auto overdue_by = 1050ms; + + std::shared_ptr timer; + switch (timer_type) { + case TimerType::WALL_TIMER: + { + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + timer = test_node->create_wall_timer( + steady_clock.now() - rclcpp::Duration(overdue_by), period, []() {}, + nullptr, false); + break; + } + case TimerType::GENERIC_TIMER: + { + timer = test_node->create_timer( + test_node->get_clock()->now() - rclcpp::Duration(overdue_by), period, []() {}, + nullptr, false); + break; + } + } + + timer->resume(); + EXPECT_FALSE(timer->is_canceled()); + + // Should have caught up to the next period boundary after now, not restarted from now(). + EXPECT_GT(timer->time_until_trigger().count(), 0); + EXPECT_LE(timer->time_until_trigger().count(), std::chrono::nanoseconds(period).count()); + + timer->cancel(); +} + +TEST_P(TestTimer, resume_uncancels_a_canceled_timer) +{ + const auto period = 10s; + + std::shared_ptr timer; + switch (timer_type) { + case TimerType::WALL_TIMER: + timer = test_node->create_wall_timer(period, []() {}); + break; + case TimerType::GENERIC_TIMER: + timer = test_node->create_timer(period, []() {}); + break; + } + + const auto time_until_trigger_before = timer->time_until_trigger(); + + timer->cancel(); + EXPECT_TRUE(timer->is_canceled()); + + timer->resume(); + EXPECT_FALSE(timer->is_canceled()); + + // A short cancel/resume cycle with a long period should not have shifted the phase. + EXPECT_NEAR( + static_cast(time_until_trigger_before.count()), + static_cast(timer->time_until_trigger().count()), + static_cast(std::chrono::nanoseconds(500ms).count())); + + timer->cancel(); +} + TEST_P(TestTimer, callback_with_timer) { rclcpp::TimerBase * timer_ptr = nullptr; auto timer_callback = [&timer_ptr](rclcpp::TimerBase & timer) { @@ -450,3 +556,67 @@ TEST_P(TestTimer, test_timer_without_autostart) std::chrono::nanoseconds::max().count()); EXPECT_FALSE(timer_without_autostart->is_canceled()); } + +class TestComputePhaseAlignedTime : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + clock_ = std::make_shared(RCL_ROS_TIME); + rcl_clock_ = clock_->get_clock_handle(); + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(rcl_clock_)); + } + + void TearDown() override + { + rclcpp::shutdown(); + } + + void set_now(rcl_time_point_value_t now_ns) + { + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(rcl_clock_, now_ns)); + } + + rclcpp::Clock::SharedPtr clock_; + rcl_clock_t * rcl_clock_; +}; + +TEST_F(TestComputePhaseAlignedTime, rejects_non_positive_interval) +{ + set_now(1); + EXPECT_THROW( + rclcpp::compute_phase_aligned_time(*clock_, 0ns), + std::invalid_argument); + EXPECT_THROW( + rclcpp::compute_phase_aligned_time(*clock_, -1ns), + std::invalid_argument); +} + +TEST_F(TestComputePhaseAlignedTime, advances_to_next_boundary_with_zero_phase) +{ + set_now(1'000'000'000); // 1.0s + auto result = rclcpp::compute_phase_aligned_time(*clock_, 300ms); + EXPECT_EQ(1'200'000'000, result.nanoseconds()); +} + +TEST_F(TestComputePhaseAlignedTime, returns_now_when_already_on_a_boundary) +{ + set_now(900'000'000); // 0.9s, an exact multiple of the 300ms interval + auto result = rclcpp::compute_phase_aligned_time(*clock_, 300ms); + EXPECT_EQ(900'000'000, result.nanoseconds()); +} + +TEST_F(TestComputePhaseAlignedTime, honors_phase_offset) +{ + set_now(1'000'000'000); // 1.0s + auto result = rclcpp::compute_phase_aligned_time(*clock_, 300ms, 50ms); + EXPECT_EQ(1'250'000'000, result.nanoseconds()); +} + +TEST_F(TestComputePhaseAlignedTime, result_uses_clocks_type) +{ + set_now(1); + auto result = rclcpp::compute_phase_aligned_time(*clock_, 300ms); + EXPECT_EQ(RCL_ROS_TIME, result.get_clock_type()); +}