Add rcl_timer_call_with_info function that retrieves the expected and…

… the actual timer trigger times

Signed-off-by: Alexis Tsogias <a.tsogias@cellumation.com>

rcl/include/rcl/timer.h

@@ -51,6 +51,13 @@ typedef struct rcl_timer_on_reset_callback_data_s
   size_t reset_counter;
 } rcl_timer_on_reset_callback_data_t;
 
+/// Structure which encapsulates call times about a ROS Timer.
+typedef struct rcl_timer_call_info_s
+{
+  rcl_time_point_value_t expected_call_time;
+  rcl_time_point_value_t actual_call_time;
+} rcl_timer_call_info_t;
+
 /// User callback signature for timers.
 /**
  * The first argument the callback gets is a pointer to the timer.
@@ -261,6 +268,34 @@ RCL_WARN_UNUSED
 rcl_ret_t
 rcl_timer_call(rcl_timer_t * timer);
 
+/// Same as rcl_timer_call() except that it also retrieves the actual and expected call time.
+/**
+ * Same as rcl_timer_call() except that it also retrieves the actual and expected call time.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | Yes [1]
+ * Uses Atomics       | Yes
+ * Lock-Free          | Yes [2]
+ * <i>[1] user callback might not be thread-safe</i>
+ *
+ * <i>[2] if `atomic_is_lock_free()` returns true for `atomic_int_least64_t`</i>
+ *
+ * \param[inout] timer the handle to the timer to call
+ * \param[out] call_info the struct in which the actual and ecpected call times are stored
+ * \return #RCL_RET_OK if the timer was called successfully, or
+ * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
+ * \return #RCL_RET_TIMER_INVALID if the timer->impl is invalid, or
+ * \return #RCL_RET_TIMER_CANCELED if the timer has been canceled, or
+ * \return #RCL_RET_ERROR an unspecified error occur.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_timer_call_with_info(rcl_timer_t * timer, rcl_timer_call_info_t * call_info);
+
 /// Retrieve the clock of the timer.
 /**
  * This function retrieves the clock pointer and copies it into the given variable.

rcl/src/rcl/timer.c

@@ -265,10 +265,18 @@ rcl_timer_clock(rcl_timer_t * timer, rcl_clock_t ** clock)
 
 rcl_ret_t
 rcl_timer_call(rcl_timer_t * timer)
+{
+  rcl_timer_call_info_t info;
+  return rcl_timer_call_with_info(timer, &info);
+}
+
+rcl_ret_t
+rcl_timer_call_with_info(rcl_timer_t * timer, rcl_timer_call_info_t * call_info)
 {
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Calling timer");
   RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
   RCL_CHECK_ARGUMENT_FOR_NULL(timer->impl, RCL_RET_TIMER_INVALID);
+  RCL_CHECK_ARGUMENT_FOR_NULL(call_info, RCL_RET_INVALID_ARGUMENT);
   if (rcutils_atomic_load_bool(&timer->impl->canceled)) {
     RCL_SET_ERROR_MSG("timer is canceled");
     return RCL_RET_TIMER_CANCELED;
@@ -288,6 +296,8 @@ rcl_timer_call(rcl_timer_t * timer)
     (rcl_timer_callback_t)rcutils_atomic_load_uintptr_t(&timer->impl->callback);
 
   int64_t next_call_time = rcutils_atomic_load_int64_t(&timer->impl->next_call_time);
+  call_info->expected_call_time = next_call_time;
+  call_info->actual_call_time = now;
   int64_t period = rcutils_atomic_load_int64_t(&timer->impl->period);
   // always move the next call time by exactly period forward
   // don't use now as the base to avoid extending each cycle by the time