diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 6060d8bd78..73a74f7ad9 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -198,16 +198,23 @@ class TimerBase set_on_reset_callback(rcl_event_callback_t callback, const void * user_data); }; +struct TimerInfo +{ + Time expected_call_time; + Time actual_call_time; +}; using VoidCallbackType = std::function; using TimerCallbackType = std::function; +using TimerInfoCallbackType = std::function; /// Generic timer. Periodically executes a user-specified callback. 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 || + rclcpp::function_traits::same_arguments::value >::type * = nullptr > class GenericTimer : public TimerBase @@ -259,7 +266,7 @@ class GenericTimer : public TimerBase bool call() override { - rcl_ret_t ret = rcl_timer_call(timer_handle_.get()); + rcl_ret_t ret = rcl_timer_call_with_info(timer_handle_.get(), &timer_call_info_); if (ret == RCL_RET_TIMER_CANCELED) { return false; } @@ -305,6 +312,21 @@ class GenericTimer : public TimerBase callback_(*this); } + + template< + typename CallbackT = FunctorT, + typename std::enable_if< + rclcpp::function_traits::same_arguments::value + >::type * = nullptr + > + void + execute_callback_delegate() + { + const TimerInfo info{Time{timer_call_info_.expected_call_time, clock_->get_clock_type()}, + Time{timer_call_info_.actual_call_time, clock_->get_clock_type()}}; + callback_(info); + } + /// Is the clock steady (i.e. is the time between ticks constant?) /** \return True if the clock used by this timer is steady. */ bool @@ -317,6 +339,7 @@ class GenericTimer : public TimerBase RCLCPP_DISABLE_COPY(GenericTimer) FunctorT callback_; + rcl_timer_call_info_t timer_call_info_; }; template<