diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index 34509fa35a..74a5e762cb 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -170,6 +170,114 @@ create_timer( return timer; } +/// Convenience method to create an aligned timer with an explicit initial trigger time. +/** + * + * \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 its countdown on initialization or not. + * \return shared pointer to an aligned 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::AlignedTimer::SharedPtr +create_aligned_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 aligned timer. + auto timer = rclcpp::AlignedTimer::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 phase-aligned timer from a period and phase offset. +/** + * + * \tparam DurationRepT + * \tparam DurationT + * \tparam PhaseRepT + * \tparam PhaseT + * \tparam CallbackT + * \param clock clock to be used + * \param period period to execute callback (and alignment interval; must be 0 < period < + * nanoseconds::max()) + * \param phase offset added to each interval boundary + * \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 its countdown on initialization or not. + * \return shared pointer to an aligned timer + * \throws std::invalid_argument if either clock, node_base or node_timers + * are nullptr, or period is not greater than zero or too large + */ +template< + typename DurationRepT, typename DurationT, + typename PhaseRepT, typename PhaseT, + typename CallbackT> +typename rclcpp::AlignedTimer::SharedPtr +create_aligned_timer( + rclcpp::Clock::SharedPtr clock, + std::chrono::duration period, + std::chrono::duration phase, + 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); + const auto phase_ns = std::chrono::duration_cast(phase); + + // Add a new aligned timer. + auto timer = rclcpp::AlignedTimer::make_shared( + std::move(clock), period_ns, phase_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 +319,104 @@ create_wall_timer( node_timers->add_timer(timer, group); return timer; } + +/// Convenience method to create an aligned wall timer with an explicit initial trigger time. +/** + * + * \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 + * \param autostart defines if the timer should start its countdown on initialization or not. + * \return shared pointer to an aligned 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::AlignedWallTimer::SharedPtr +create_aligned_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 aligned wall timer. + auto timer = rclcpp::AlignedWallTimer::make_shared( + 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 phase-aligned wall timer from a period and phase offset. +/** + * + * \tparam DurationRepT + * \tparam DurationT + * \tparam PhaseRepT + * \tparam PhaseT + * \tparam CallbackT + * \param period period to execute callback (and alignment interval; must be 0 < period < + * nanoseconds::max()) + * \param phase offset added to each interval boundary + * \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 its countdown on initialization or not. + * \return shared pointer to an aligned wall timer + * \throws std::invalid_argument if either node_base or node_timers + * are null, or period is not greater than zero or too large + */ +template< + typename DurationRepT, typename DurationT, + typename PhaseRepT, typename PhaseT, + typename CallbackT> +typename rclcpp::AlignedWallTimer::SharedPtr +create_aligned_wall_timer( + std::chrono::duration period, + std::chrono::duration phase, + 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); + const auto phase_ns = std::chrono::duration_cast(phase); + + // Add a new aligned wall timer. + auto timer = rclcpp::AlignedWallTimer::make_shared( + period_ns, phase_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..bf6c0a4b5b 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -242,6 +242,44 @@ class Node : public std::enable_shared_from_this const rclcpp::CallbackGroup::SharedPtr & group = nullptr, bool autostart = true); + /// Create an aligned wall timer that uses the wall clock to drive the callback with an + /// explicit 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::AlignedWallTimer::SharedPtr + create_aligned_wall_timer( + Time initial_call_time, + std::chrono::duration period, + CallbackT callback, + const rclcpp::CallbackGroup::SharedPtr & group = nullptr, + bool autostart = true); + + /// Create a phase-aligned wall timer that uses the wall clock to drive the callback. + /** + * \param[in] period Time interval between triggers of the callback (and alignment interval). + * \param[in] phase Offset added to each interval boundary. + * \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 DurationRepT = int64_t, typename DurationT = std::milli, + typename PhaseRepT = int64_t, typename PhaseT = std::milli, + typename CallbackT> + typename rclcpp::AlignedWallTimer::SharedPtr + create_aligned_wall_timer( + std::chrono::duration period, + std::chrono::duration phase, + 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 +293,44 @@ class Node : public std::enable_shared_from_this CallbackT callback, const rclcpp::CallbackGroup::SharedPtr & group = nullptr); + /// Create an aligned timer that uses the node clock to drive the callback with an explicit + /// 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 timer on initialization. + */ + template + typename rclcpp::AlignedTimer::SharedPtr + create_aligned_timer( + Time initial_call_time, + std::chrono::duration period, + CallbackT callback, + const rclcpp::CallbackGroup::SharedPtr & group = nullptr, + bool autostart = true); + + /// Create a phase-aligned timer that uses the node clock to drive the callback. + /** + * \param[in] period Time interval between triggers of the callback (and alignment interval). + * \param[in] phase Offset added to each interval boundary. + * \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 DurationRepT = int64_t, typename DurationT = std::milli, + typename PhaseRepT = int64_t, typename PhaseT = std::milli, + typename CallbackT> + typename rclcpp::AlignedTimer::SharedPtr + create_aligned_timer( + std::chrono::duration period, + std::chrono::duration phase, + CallbackT callback, + const rclcpp::CallbackGroup::SharedPtr & group = nullptr, + bool autostart = true); + /// 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 c699e7519e..d8740ac250 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -115,6 +115,47 @@ Node::create_wall_timer( autostart); } +template +typename rclcpp::AlignedWallTimer::SharedPtr +Node::create_aligned_wall_timer( + rclcpp::Time initial_call_time, + std::chrono::duration period, + CallbackT callback, + const rclcpp::CallbackGroup::SharedPtr & group, + bool autostart) +{ + return rclcpp::create_aligned_wall_timer( + initial_call_time, + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get(), + autostart); +} + +template< + typename DurationRepT, typename DurationT, + typename PhaseRepT, typename PhaseT, + typename CallbackT> +typename rclcpp::AlignedWallTimer::SharedPtr +Node::create_aligned_wall_timer( + std::chrono::duration period, + std::chrono::duration phase, + CallbackT callback, + const rclcpp::CallbackGroup::SharedPtr & group, + bool autostart) +{ + return rclcpp::create_aligned_wall_timer( + period, + phase, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get(), + autostart); +} + template typename rclcpp::GenericTimer::SharedPtr Node::create_timer( @@ -131,6 +172,49 @@ Node::create_timer( this->node_timers_.get()); } +template +typename rclcpp::AlignedTimer::SharedPtr +Node::create_aligned_timer( + rclcpp::Time initial_call_time, + std::chrono::duration period, + CallbackT callback, + const rclcpp::CallbackGroup::SharedPtr & group, + bool autostart) +{ + return rclcpp::create_aligned_timer( + this->get_clock(), + initial_call_time, + period, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get(), + autostart); +} + +template< + typename DurationRepT, typename DurationT, + typename PhaseRepT, typename PhaseT, + typename CallbackT> +typename rclcpp::AlignedTimer::SharedPtr +Node::create_aligned_timer( + std::chrono::duration period, + std::chrono::duration phase, + CallbackT callback, + const rclcpp::CallbackGroup::SharedPtr & group, + bool autostart) +{ + return rclcpp::create_aligned_timer( + this->get_clock(), + period, + phase, + std::move(callback), + group, + this->node_base_.get(), + this->node_timers_.get(), + autostart); +} + template typename Client::SharedPtr Node::create_client( diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index dd229da4e1..ce8602480a 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -26,6 +26,8 @@ #include "rclcpp/clock.hpp" #include "rclcpp/context.hpp" +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/exceptions.hpp" #include "rclcpp/function_traits.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/utilities.hpp" @@ -47,6 +49,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: @@ -192,6 +218,22 @@ class TimerBase const Clock::SharedPtr & get_clock() const; protected: + /// Constructor for subclasses that initialize `timer_handle_` themselves. + /** + * Subclasses that use an alternate rcl timer initialization function can delegate to this + * constructor and call `create_timer_handle(context)` to allocate the zero-initialized + * `timer_handle_` before initializing the rcl timer. + * + * \param clock A clock to use for time and sleeping + */ + RCLCPP_PUBLIC + explicit TimerBase(Clock::SharedPtr clock); + + /// Allocate a zero-initialized `rcl_timer_t` shared_ptr with its cleanup deleter. + RCLCPP_PUBLIC + std::shared_ptr + create_timer_handle(rclcpp::Context::SharedPtr context); + std::recursive_mutex callback_mutex_; // Declare callback before timer_handle_, so on destruction // the callback is destroyed last. Otherwise, the rcl timer @@ -262,7 +304,9 @@ class GenericTimer : public TimerBase virtual ~GenericTimer() { // Stop the timer from running. - cancel(); + if (timer_handle_) { + cancel(); + } } /** @@ -345,6 +389,11 @@ class GenericTimer : public TimerBase protected: RCLCPP_DISABLE_COPY(GenericTimer) + /// Constructor for subclasses that initialize `timer_handle_` themselves. + explicit GenericTimer(Clock::SharedPtr clock, FunctorT && callback) + : TimerBase(clock), callback_(std::forward(callback)) + {} + FunctorT callback_; }; @@ -381,6 +430,194 @@ class WallTimer : public GenericTimer RCLCPP_DISABLE_COPY(WallTimer) }; +/// Timer with an explicit start time or phase alignment that is preserved across reset() calls. +/** + * Unlike GenericTimer, whose first trigger is at `now() + period` and whose `reset()` method + * re-anchors the schedule to `now() + period`, AlignedTimer is initialized with an explicit + * initial trigger time (or a period and phase offset) via `rcl_timer_init_with_start_time()` + * and `reset()` preserves its schedule phase. + */ +template< + typename FunctorT, + typename std::enable_if< + rclcpp::function_traits::same_arguments::value || + rclcpp::function_traits::same_arguments::value || + rclcpp::function_traits::same_arguments::value + >::type * = nullptr +> +class AlignedTimer : public GenericTimer +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(AlignedTimer) + + /// Constructor with an explicit 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[in] autostart timer state on initialization + * + * \throws std::runtime_error if initial_call_time's clock type does not match clock's + */ + explicit AlignedTimer( + Clock::SharedPtr clock, Time initial_call_time, std::chrono::nanoseconds period, + FunctorT && callback, rclcpp::Context::SharedPtr context, bool autostart = true + ) + : GenericTimer(clock, std::forward(callback)) + { + // 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 (this->clock_->get_clock_type() != RCL_CLOCK_UNINITIALIZED && + initial_call_time.get_clock_type() != this->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(); + } + auto rcl_context = context->get_rcl_context(); + auto timer_handle = this->create_timer_handle(context); + rcl_clock_t * clock_handle = this->clock_->get_clock_handle(); + { + std::lock_guard clock_guard(this->clock_->get_clock_mutex()); + 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) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize rcl timer handle"); + } + } + this->timer_handle_ = std::move(timer_handle); + + TRACETOOLS_TRACEPOINT( + rclcpp_timer_callback_added, + static_cast(this->get_timer_handle().get()), + reinterpret_cast(&this->callback_)); +#ifndef TRACETOOLS_DISABLED + if (TRACETOOLS_TRACEPOINT_ENABLED(rclcpp_callback_register)) { + char * symbol = tracetools::get_symbol(this->callback_); + TRACETOOLS_DO_TRACEPOINT( + rclcpp_callback_register, + reinterpret_cast(&this->callback_), + symbol); + std::free(symbol); + } +#endif + } + + /// Convenience constructor that computes a phase-aligned initial trigger time from period and + /// phase offset. + /** + * The initial trigger time is computed via `compute_phase_aligned_time(*clock, period, phase)` + * as the smallest time `>= clock->now()` of the form `k * period + phase`. + * + * \param[in] clock The clock providing the current time. + * \param[in] period The interval at which the timer fires (and alignment interval; must be > 0). + * \param[in] phase Offset added to each interval boundary. + * \param[in] callback User-specified callback function. + * \param[in] context custom context to be used. + * \param[in] autostart timer state on initialization + * + * \throws std::invalid_argument if period is not greater than zero + */ + explicit AlignedTimer( + Clock::SharedPtr clock, std::chrono::nanoseconds period, std::chrono::nanoseconds phase, + FunctorT && callback, rclcpp::Context::SharedPtr context, bool autostart = true + ) + : AlignedTimer( + clock, + compute_initial_call_time(clock, period, phase), + period, + std::forward(callback), + context, + autostart) + {} + +protected: + RCLCPP_DISABLE_COPY(AlignedTimer) + +private: + static Time + compute_initial_call_time( + const Clock::SharedPtr & clock, + std::chrono::nanoseconds period, + std::chrono::nanoseconds phase) + { + if (nullptr == clock) { + throw std::invalid_argument("clock cannot be null"); + } + if (clock->get_clock_type() == RCL_CLOCK_UNINITIALIZED) { + return Time(static_cast(0), RCL_CLOCK_UNINITIALIZED); + } + return compute_phase_aligned_time(*clock, period, phase); + } +}; + +/// Wall-clock timer with an explicit start time or phase alignment preserved across reset() calls. +template< + typename FunctorT, + typename std::enable_if< + rclcpp::function_traits::same_arguments::value || + rclcpp::function_traits::same_arguments::value || + rclcpp::function_traits::same_arguments::value + >::type * = nullptr +> +class AlignedWallTimer : public AlignedTimer +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(AlignedWallTimer) + + /// AlignedWallTimer constructor with an explicit initial trigger 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 The callback function to execute every interval. + * \param[in] context node context. + * \param[in] autostart timer state on initialization. + * + * \throws std::runtime_error if initial_call_time's clock type is not RCL_STEADY_TIME + */ + AlignedWallTimer( + Time initial_call_time, + std::chrono::nanoseconds period, + FunctorT && callback, + rclcpp::Context::SharedPtr context, + bool autostart = true) + : AlignedTimer( + std::make_shared(RCL_STEADY_TIME), initial_call_time, period, + std::move(callback), context, autostart) + {} + + /// AlignedWallTimer convenience constructor with period and phase offset. + /** + * \param[in] period The interval at which the timer fires (and alignment interval; must be > 0). + * \param[in] phase Offset added to each interval boundary. + * \param[in] callback The callback function to execute every interval. + * \param[in] context node context. + * \param[in] autostart timer state on initialization. + * + * \throws std::invalid_argument if period is not greater than zero + */ + AlignedWallTimer( + std::chrono::nanoseconds period, + std::chrono::nanoseconds phase, + FunctorT && callback, + rclcpp::Context::SharedPtr context, + bool autostart = true) + : AlignedTimer( + std::make_shared(RCL_STEADY_TIME), period, phase, + std::move(callback), context, autostart) + {} + +protected: + RCLCPP_DISABLE_COPY(AlignedWallTimer) +}; + } // namespace rclcpp #endif // RCLCPP__TIMER_HPP_ diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index fe3f24e968..6011b28e82 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 @@ -28,20 +29,22 @@ using rclcpp::TimerBase; -TimerBase::TimerBase( - rclcpp::Clock::SharedPtr clock, - std::chrono::nanoseconds period, - rclcpp::Context::SharedPtr context, - bool autostart) +TimerBase::TimerBase(rclcpp::Clock::SharedPtr clock) : clock_(clock), timer_handle_(nullptr) +{ +} + +std::shared_ptr +TimerBase::create_timer_handle(rclcpp::Context::SharedPtr context) { if (nullptr == context) { context = rclcpp::contexts::get_global_default_context(); } auto rcl_context = context->get_rcl_context(); + auto clock = clock_; - timer_handle_ = std::shared_ptr( + auto handle = std::shared_ptr( new rcl_timer_t, [ = ](rcl_timer_t * timer) mutable { { @@ -59,7 +62,23 @@ TimerBase::TimerBase( rcl_context.reset(); }); - *timer_handle_.get() = rcl_get_zero_initialized_timer(); + *handle.get() = rcl_get_zero_initialized_timer(); + return handle; +} + +TimerBase::TimerBase( + rclcpp::Clock::SharedPtr clock, + std::chrono::nanoseconds period, + rclcpp::Context::SharedPtr context, + bool autostart) +: TimerBase(clock) +{ + if (nullptr == context) { + context = rclcpp::contexts::get_global_default_context(); + } + + auto rcl_context = context->get_rcl_context(); + timer_handle_ = create_timer_handle(context); rcl_clock_t * clock_handle = clock_->get_clock_handle(); { @@ -221,3 +240,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_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index 9995c7136c..338eb1a7c7 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -196,6 +196,222 @@ TEST(TestCreateTimer, timer_function_pointer) rclcpp::shutdown(); } +TEST(TestCreateAlignedTimer, call_aligned_timer_with_initial_call_time_bad_arguments) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_aligned_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_aligned_timer( + clock, initial_call_time, -1ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // clock is null + EXPECT_THROW( + rclcpp::create_aligned_timer( + nullptr, initial_call_time, 1ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // node_interface is null + EXPECT_THROW( + rclcpp::create_aligned_timer( + clock, initial_call_time, 1ms, callback, group, nullptr, timers_interface), + std::invalid_argument); + + // timers_interface is null + EXPECT_THROW( + rclcpp::create_aligned_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_aligned_timer( + clock, mismatched_clock_type_time, 1ms, callback, group, node_interface, timers_interface), + std::runtime_error); + + // Same check for create_aligned_wall_timer, which always uses a steady clock internally + // regardless of what clock type initial_call_time was constructed with. + EXPECT_THROW( + rclcpp::create_aligned_wall_timer( + initial_call_time, 1ms, callback, group, node_interface, timers_interface), + std::runtime_error); + + // Phase-offset overload bad arguments + EXPECT_THROW( + rclcpp::create_aligned_timer( + clock, 0ms, 0ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + EXPECT_THROW( + rclcpp::create_aligned_timer( + clock, -1ms, 0ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + EXPECT_THROW( + rclcpp::create_aligned_timer( + nullptr, 1ms, 0ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + EXPECT_THROW( + rclcpp::create_aligned_timer( + clock, 1ms, 0ms, callback, group, nullptr, timers_interface), + std::invalid_argument); + EXPECT_THROW( + rclcpp::create_aligned_timer( + clock, 1ms, 0ms, callback, group, node_interface, nullptr), + std::invalid_argument); + + rclcpp::shutdown(); +} + +TEST(TestCreateAlignedTimer, call_aligned_timer_honors_initial_call_time) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_aligned_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_aligned_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_overload) +{ + 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(); + + auto aligned_timer = rclcpp::create_aligned_timer( + clock, 100ms, 0ms, []() {}, nullptr, node_interface, timers_interface, false); + + EXPECT_TRUE(aligned_timer->is_canceled()); + aligned_timer->reset(); + EXPECT_FALSE(aligned_timer->is_canceled()); + aligned_timer->cancel(); + + rclcpp::shutdown(); +} + +TEST(TestCreateAlignedWallTimer, call_aligned_wall_timer_with_bad_arguments) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_aligned_wall_timer_with_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_aligned_wall_timer( + initial_call_time, -1ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // node_interface is null + EXPECT_THROW( + rclcpp::create_aligned_wall_timer( + initial_call_time, 1ms, callback, group, nullptr, timers_interface), + std::invalid_argument); + + // timers_interface is null + EXPECT_THROW( + rclcpp::create_aligned_wall_timer( + initial_call_time, 1ms, callback, group, node_interface, nullptr), + std::invalid_argument); + + // Phase-offset overload bad arguments + EXPECT_THROW( + rclcpp::create_aligned_wall_timer( + 0ms, 0ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + EXPECT_THROW( + rclcpp::create_aligned_wall_timer( + -1ms, 0ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + EXPECT_THROW( + rclcpp::create_aligned_wall_timer( + 1ms, 0ms, callback, group, nullptr, timers_interface), + std::invalid_argument); + EXPECT_THROW( + rclcpp::create_aligned_wall_timer( + 1ms, 0ms, callback, group, node_interface, nullptr), + std::invalid_argument); + + rclcpp::shutdown(); +} + +TEST(TestCreateAlignedWallTimer, call_aligned_wall_timer_honors_initial_call_time) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_aligned_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_aligned_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..801d7796cc 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" @@ -37,11 +38,13 @@ using namespace std::chrono_literals; -/// We want to test everything for both the wall and generic timer. +/// We want to test everything for both the wall and generic timer, as well as the aligned timers. enum class TimerType { WALL_TIMER, GENERIC_TIMER, + ALIGNED_WALL_TIMER, + ALIGNED_TIMER, }; /// Timer testing bring up and teardown @@ -79,6 +82,14 @@ class TestTimer : public ::testing::TestWithParam timer = test_node->create_timer(100ms, timer_callback); EXPECT_FALSE(timer->is_steady()); break; + case TimerType::ALIGNED_WALL_TIMER: + timer = test_node->create_aligned_wall_timer(100ms, 0ms, timer_callback); + EXPECT_TRUE(timer->is_steady()); + break; + case TimerType::ALIGNED_TIMER: + timer = test_node->create_aligned_timer(100ms, 0ms, timer_callback); + EXPECT_FALSE(timer->is_steady()); + break; } timer_without_autostart = test_node->create_wall_timer( 100ms, @@ -233,6 +244,196 @@ TEST_P(TestTimer, test_bad_arguments) { EXPECT_THROW( rclcpp::GenericTimer(unitialized_clock, 1us, []() {}, context), rclcpp::exceptions::RCLError); + + // Same set of checks, but using AlignedTimer / AlignedWallTimer. + auto now = steady_clock->now(); + + // Negative period (initial_call_time constructor) + EXPECT_THROW( + rclcpp::AlignedTimer(steady_clock, now, -1ms, []() {}, context), + rclcpp::exceptions::RCLInvalidArgument); + + // 0 duration period, should be ok for initial_call_time constructor + EXPECT_NO_THROW( + rclcpp::AlignedTimer(steady_clock, now, 0ms, []() {}, context)); + + // Non-positive period for phase-offset constructor throws std::invalid_argument + EXPECT_THROW( + rclcpp::AlignedTimer(steady_clock, 0ms, 0ms, []() {}, context), + std::invalid_argument); + EXPECT_THROW( + rclcpp::AlignedTimer(steady_clock, -1ms, 0ms, []() {}, context), + std::invalid_argument); + EXPECT_THROW( + rclcpp::AlignedWallTimer(0ms, 0ms, []() {}, context), + std::invalid_argument); + + // Clock is uninitialized + EXPECT_THROW( + rclcpp::AlignedTimer(unitialized_clock, now, 1us, []() {}, context), + rclcpp::exceptions::RCLError); + EXPECT_THROW( + rclcpp::AlignedTimer(unitialized_clock, 1us, 0us, []() {}, 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::AlignedTimer( + steady_clock, mismatched_clock_type_time, 1ms, []() {}, context), + std::runtime_error); + + // Same check for AlignedWallTimer, which always uses a steady clock internally regardless of what + // clock type initial_call_time was constructed with. + EXPECT_THROW( + rclcpp::AlignedWallTimer(mismatched_clock_type_time, 1ms, []() {}, context), + std::runtime_error); +} + +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: + case TimerType::ALIGNED_WALL_TIMER: + { + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + initial_time_timer = test_node->create_aligned_wall_timer( + steady_clock.now() + rclcpp::Duration(initial_delay), period, []() {}); + break; + } + case TimerType::GENERIC_TIMER: + case TimerType::ALIGNED_TIMER: + { + initial_time_timer = test_node->create_aligned_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, aligned_timer_reset_does_not_advance_if_not_yet_due) +{ + const auto period = 50ms; + const auto initial_delay = std::chrono::seconds(10); + + std::shared_ptr aligned_timer; + switch (timer_type) { + case TimerType::WALL_TIMER: + case TimerType::ALIGNED_WALL_TIMER: + { + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + aligned_timer = test_node->create_aligned_wall_timer( + steady_clock.now() + rclcpp::Duration(initial_delay), period, []() {}, + nullptr, false); + break; + } + case TimerType::GENERIC_TIMER: + case TimerType::ALIGNED_TIMER: + { + aligned_timer = test_node->create_aligned_timer( + test_node->get_clock()->now() + rclcpp::Duration(initial_delay), period, []() {}, + nullptr, false); + break; + } + } + + EXPECT_TRUE(aligned_timer->is_canceled()); + aligned_timer->reset(); + EXPECT_FALSE(aligned_timer->is_canceled()); + + // The original phase-anchored schedule should be preserved by reset(), not recomputed from now(). + EXPECT_GT( + aligned_timer->time_until_trigger().count(), + std::chrono::nanoseconds(period).count()); + EXPECT_LE( + aligned_timer->time_until_trigger().count(), + std::chrono::nanoseconds(initial_delay).count()); + + aligned_timer->cancel(); +} + +TEST_P(TestTimer, aligned_timer_reset_catches_up_if_overdue) +{ + // Simulate an aligned timer that was paused (canceled) for much longer than several periods. + const auto period = 100ms; + const auto overdue_by = 1050ms; + + std::shared_ptr aligned_timer; + switch (timer_type) { + case TimerType::WALL_TIMER: + case TimerType::ALIGNED_WALL_TIMER: + { + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + aligned_timer = test_node->create_aligned_wall_timer( + steady_clock.now() - rclcpp::Duration(overdue_by), period, []() {}, + nullptr, false); + break; + } + case TimerType::GENERIC_TIMER: + case TimerType::ALIGNED_TIMER: + { + aligned_timer = test_node->create_aligned_timer( + test_node->get_clock()->now() - rclcpp::Duration(overdue_by), period, []() {}, + nullptr, false); + break; + } + } + + aligned_timer->reset(); + EXPECT_FALSE(aligned_timer->is_canceled()); + + // Should have caught up to the next period boundary after now, not restarted from now(). + EXPECT_GT(aligned_timer->time_until_trigger().count(), 0); + EXPECT_LE(aligned_timer->time_until_trigger().count(), std::chrono::nanoseconds(period).count()); + + aligned_timer->cancel(); +} + +TEST_P(TestTimer, aligned_timer_reset_uncancels_and_preserves_phase) +{ + const auto period = 10s; + + std::shared_ptr aligned_timer; + switch (timer_type) { + case TimerType::WALL_TIMER: + case TimerType::ALIGNED_WALL_TIMER: + aligned_timer = test_node->create_aligned_wall_timer(period, 0ms, []() {}); + break; + case TimerType::GENERIC_TIMER: + case TimerType::ALIGNED_TIMER: + aligned_timer = test_node->create_aligned_timer(period, 0ms, []() {}); + break; + } + + const auto time_until_trigger_before = aligned_timer->time_until_trigger(); + + aligned_timer->cancel(); + EXPECT_TRUE(aligned_timer->is_canceled()); + + aligned_timer->reset(); + EXPECT_FALSE(aligned_timer->is_canceled()); + + // A short cancel/reset cycle with a long period should not have shifted the phase. + EXPECT_NEAR( + static_cast(time_until_trigger_before.count()), + static_cast(aligned_timer->time_until_trigger().count()), + static_cast(std::chrono::nanoseconds(500ms).count())); + + aligned_timer->cancel(); } TEST_P(TestTimer, callback_with_timer) { @@ -247,6 +448,12 @@ TEST_P(TestTimer, callback_with_timer) { case TimerType::GENERIC_TIMER: timer = test_node->create_timer(1ms, timer_callback); break; + case TimerType::ALIGNED_WALL_TIMER: + timer = test_node->create_aligned_wall_timer(1ms, 0ms, timer_callback); + break; + case TimerType::ALIGNED_TIMER: + timer = test_node->create_aligned_timer(1ms, 0ms, timer_callback); + break; } auto start = std::chrono::steady_clock::now(); while (nullptr == timer_ptr && @@ -271,6 +478,12 @@ TEST_P(TestTimer, callback_with_timer_info) { case TimerType::GENERIC_TIMER: timer = test_node->create_timer(1ms, timer_callback); break; + case TimerType::ALIGNED_WALL_TIMER: + timer = test_node->create_aligned_wall_timer(1ms, 0ms, timer_callback); + break; + case TimerType::ALIGNED_TIMER: + timer = test_node->create_aligned_timer(1ms, 0ms, timer_callback); + break; } auto start = std::chrono::steady_clock::now(); while (info.actual_call_time.nanoseconds() == 0 && @@ -293,6 +506,15 @@ TEST_P(TestTimer, callback_with_period_zero) { case TimerType::GENERIC_TIMER: timer = test_node->create_timer(0ms, timer_callback); break; + case TimerType::ALIGNED_WALL_TIMER: + { + rclcpp::Clock steady_clock(RCL_STEADY_TIME); + timer = test_node->create_aligned_wall_timer(steady_clock.now(), 0ms, timer_callback); + break; + } + case TimerType::ALIGNED_TIMER: + timer = test_node->create_aligned_timer(test_node->get_clock()->now(), 0ms, timer_callback); + break; } auto start = std::chrono::steady_clock::now(); while (nullptr == timer_ptr && @@ -314,12 +536,25 @@ TEST_P(TestTimer, test_failures_with_exceptions) std::shared_ptr timer_to_test_destructor; // Test destructor failure, just logs a msg auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_timer_fini, RCL_RET_ERROR); - if (timer_type == TimerType::WALL_TIMER) { - timer_to_test_destructor = - test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); - } else { - timer_to_test_destructor = - test_node->create_timer(std::chrono::milliseconds(0), [](void) {}); + switch (timer_type) { + case TimerType::WALL_TIMER: + timer_to_test_destructor = + test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); + break; + case TimerType::GENERIC_TIMER: + timer_to_test_destructor = + test_node->create_timer(std::chrono::milliseconds(0), [](void) {}); + break; + case TimerType::ALIGNED_WALL_TIMER: + timer_to_test_destructor = + test_node->create_aligned_wall_timer( + rclcpp::Clock(RCL_STEADY_TIME).now(), std::chrono::milliseconds(0), [](void) {}); + break; + case TimerType::ALIGNED_TIMER: + timer_to_test_destructor = + test_node->create_aligned_timer( + test_node->get_clock()->now(), std::chrono::milliseconds(0), [](void) {}); + break; } timer_to_test_destructor.reset(); } @@ -359,13 +594,21 @@ TEST_P(TestTimer, test_failures_with_exceptions) INSTANTIATE_TEST_SUITE_P( PerTimerType, TestTimer, - ::testing::Values(TimerType::WALL_TIMER, TimerType::GENERIC_TIMER), + ::testing::Values( + TimerType::WALL_TIMER, + TimerType::GENERIC_TIMER, + TimerType::ALIGNED_WALL_TIMER, + TimerType::ALIGNED_TIMER), [](const ::testing::TestParamInfo & info) -> std::string { switch (info.param) { case TimerType::WALL_TIMER: return std::string("wall_timer"); case TimerType::GENERIC_TIMER: return std::string("generic_timer"); + case TimerType::ALIGNED_WALL_TIMER: + return std::string("aligned_wall_timer"); + case TimerType::ALIGNED_TIMER: + return std::string("aligned_timer"); default: break; } @@ -387,3 +630,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()); +}