Add rcl_timer_call_with_info function that retrieves the expected and…

… the actual timer trigger times (#1113)

Signed-off-by: Alexis Tsogias <[email protected]>
Signed-off-by: Janosch Machowinski <[email protected]>
Co-authored-by: Alexis Tsogias <[email protected]>
Co-authored-by: Michael Carroll <[email protected]>
Co-authored-by: Tomoya Fujita <[email protected]>

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 timer information when called.
+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 expected 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

rcl/test/rcl/test_timer.cpp

@@ -988,6 +988,91 @@ TEST_F(TestPreInitTimer, test_timer_get_period) {
   rcl_reset_error();
 }
 
+TEST_F(TestPreInitTimer, test_timer_info) {
+  int64_t next_call_start = 0;
+  int64_t old_period = 0;
+  times_called = 0;
+  rcl_timer_call_info_t call_info;
+  call_info.actual_call_time = 0;
+  call_info.expected_call_time = 0;
+  int64_t period = RCL_MS_TO_NS(10);
+
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_exchange_period(&timer, period, &old_period));
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_reset(&timer));
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_call_with_info(&timer, &call_info)) << rcl_get_error_string().str;
+  EXPECT_EQ(times_called, 1);
+
+  int64_t next_expected_call_time = call_info.expected_call_time + period;
+
+  EXPECT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &next_call_start));
+  std::this_thread::sleep_for(std::chrono::nanoseconds(next_call_start));
+
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_call_with_info(&timer, &call_info)) << rcl_get_error_string().str;
+  ASSERT_EQ(next_expected_call_time, call_info.expected_call_time);
+  ASSERT_GE(call_info.actual_call_time, call_info.expected_call_time);
+  EXPECT_EQ(times_called, 2);
+
+  next_expected_call_time = call_info.expected_call_time + period;
+
+  EXPECT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &next_call_start));
+  std::this_thread::sleep_for(std::chrono::nanoseconds(next_call_start));
+
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_call_with_info(&timer, &call_info)) << rcl_get_error_string().str;
+  ASSERT_EQ(next_expected_call_time, call_info.expected_call_time);
+  ASSERT_GE(call_info.actual_call_time, call_info.expected_call_time);
+  EXPECT_EQ(times_called, 3);
+
+  next_expected_call_time = call_info.expected_call_time + period;
+
+  EXPECT_EQ(RCL_RET_OK, rcl_timer_cancel(&timer)) << rcl_get_error_string().str;
+  EXPECT_EQ(RCL_RET_TIMER_CANCELED, rcl_timer_call(&timer));
+  rcl_reset_error();
+  EXPECT_EQ(times_called, 3);
+}
+
+TEST_F(TestPreInitTimer, test_timer_info_detect_overrun) {
+  int64_t next_call_start = 0;
+  int64_t old_period = 0;
+  times_called = 0;
+  rcl_timer_call_info_t call_info;
+  call_info.actual_call_time = 0;
+  call_info.expected_call_time = 0;
+  int64_t period = RCL_MS_TO_NS(10);
+
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_exchange_period(&timer, period, &old_period));
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_reset(&timer));
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_call_with_info(&timer, &call_info)) << rcl_get_error_string().str;
+  EXPECT_EQ(times_called, 1);
+
+  int64_t next_expected_call_time = call_info.expected_call_time + period;
+
+  EXPECT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &next_call_start));
+  std::this_thread::sleep_for(std::chrono::nanoseconds(next_call_start + period));
+
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_call_with_info(&timer, &call_info)) << rcl_get_error_string().str;
+  ASSERT_EQ(next_expected_call_time, call_info.expected_call_time);
+  ASSERT_GE(call_info.actual_call_time, call_info.expected_call_time);
+  // check, if we can detect a timer overrun
+  ASSERT_GE(call_info.actual_call_time - call_info.expected_call_time, period);
+  EXPECT_EQ(times_called, 2);
+
+  // check, if the expected_call_time for next call is as expected, and skips a period
+  next_expected_call_time = call_info.expected_call_time + period + period;
+
+  EXPECT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &next_call_start));
+  std::this_thread::sleep_for(std::chrono::nanoseconds(next_call_start));
+
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_call_with_info(&timer, &call_info)) << rcl_get_error_string().str;
+  ASSERT_EQ(next_expected_call_time, call_info.expected_call_time);
+  ASSERT_GE(call_info.actual_call_time, call_info.expected_call_time);
+  EXPECT_EQ(times_called, 3);
+
+  EXPECT_EQ(RCL_RET_OK, rcl_timer_cancel(&timer)) << rcl_get_error_string().str;
+  EXPECT_EQ(RCL_RET_TIMER_CANCELED, rcl_timer_call(&timer));
+  rcl_reset_error();
+  EXPECT_EQ(times_called, 3);
+}
+
 TEST_F(TestPreInitTimer, test_time_since_last_call) {
   rcl_time_point_value_t time_sice_next_call_start = 0u;
   rcl_time_point_value_t time_sice_next_call_end = 0u;