Add timer on reset callback

rcl/include/rcl/timer.h

@@ -26,6 +26,7 @@ extern "C"
 
 #include "rcl/allocator.h"
 #include "rcl/context.h"
+#include "rcl/event_callback.h"
 #include "rcl/guard_condition.h"
 #include "rcl/macros.h"
 #include "rcl/time.h"
@@ -41,6 +42,14 @@ typedef struct rcl_timer_s
   rcl_timer_impl_t * impl;
 } rcl_timer_t;
 
+/// Structure which encapsulates the on reset callback data
+typedef struct rcl_timer_on_reset_callback_data_s
+{
+  rcl_event_callback_t on_reset_callback;
+  const void * user_data;
+  size_t reset_counter;
+} rcl_timer_on_reset_callback_data_t;
+
 /// User callback signature for timers.
 /**
  * The first argument the callback gets is a pointer to the timer.
@@ -590,6 +599,34 @@ RCL_WARN_UNUSED
 rcl_guard_condition_t *
 rcl_timer_get_guard_condition(const rcl_timer_t * timer);
 
+/// Set the on reset callback function for the timer.
+/**
+ * This API sets the callback function to be called whenever the
+ * timer is reset.
+ * If the timer has already been reset, the callback will be called.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | No
+ *
+ * \param[in] timer The handle to the timer on which to set the callback
+ * \param[in] on_reset_callback The callback to be called when timer is reset
+ * \param[in] user_data Given to the callback when called later, may be NULL
+ * \return `RCL_RET_OK` if successful, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if `timer` is NULL
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_timer_set_on_reset_callback(
+  const rcl_timer_t * timer,
+  rcl_event_callback_t on_reset_callback,
+  const void * user_data);
+
 #ifdef __cplusplus
 }
 #endif

rcl/src/rcl/timer.c

@@ -50,6 +50,8 @@ struct rcl_timer_impl_s
   atomic_bool canceled;
   // The user supplied allocator.
   rcl_allocator_t allocator;
+  // The user supplied on reset callback data.
+  rcl_timer_on_reset_callback_data_t callback_data;
 };
 
 rcl_timer_t
@@ -181,6 +183,12 @@ rcl_timer_init(
   atomic_init(&impl.next_call_time, now + period);
   atomic_init(&impl.canceled, false);
   impl.allocator = allocator;
+
+  // Empty init on reset callback data
+  impl.callback_data.on_reset_callback = NULL;
+  impl.callback_data.user_data = NULL;
+  impl.callback_data.reset_counter = 0;
+
   timer->impl = (rcl_timer_impl_t *)allocator.allocate(sizeof(rcl_timer_impl_t), allocator.state);
   if (NULL == timer->impl) {
     if (RCL_RET_OK != rcl_guard_condition_fini(&(impl.guard_condition))) {
@@ -428,6 +436,15 @@ rcl_timer_reset(rcl_timer_t * timer)
   rcutils_atomic_store(&timer->impl->next_call_time, now + period);
   rcutils_atomic_store(&timer->impl->canceled, false);
   rcl_ret_t ret = rcl_trigger_guard_condition(&timer->impl->guard_condition);
+
+  rcl_timer_on_reset_callback_data_t * cb_data = &timer->impl->callback_data;
+
+  if (cb_data->on_reset_callback) {
+    cb_data->on_reset_callback(cb_data->user_data, 1);
+  } else {
+    cb_data->reset_counter++;
+  }
+
   if (ret != RCL_RET_OK) {
     RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to trigger timer guard condition");
   }
@@ -452,6 +469,31 @@ rcl_timer_get_guard_condition(const rcl_timer_t * timer)
   return &timer->impl->guard_condition;
 }
 
+rcl_ret_t
+rcl_timer_set_on_reset_callback(
+  const rcl_timer_t * timer,
+  rcl_event_callback_t on_reset_callback,
+  const void * user_data)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+
+  rcl_timer_on_reset_callback_data_t * cb_data = &timer->impl->callback_data;
+
+  if (on_reset_callback) {
+    cb_data->on_reset_callback = on_reset_callback;
+    cb_data->user_data = user_data;
+    if (cb_data->reset_counter) {
+      cb_data->on_reset_callback(user_data, cb_data->reset_counter);
+      cb_data->reset_counter = 0;
+    }
+  } else {
+    cb_data->on_reset_callback = NULL;
+    cb_data->user_data = NULL;
+  }
+
+  return RCL_RET_OK;
+}
+
 #ifdef __cplusplus
 }
 #endif

rcl/test/rcl/test_timer.cpp

@@ -81,6 +81,18 @@ static void callback_function_changed(rcl_timer_t * timer, int64_t last_call)
   times_called--;
 }
 
+static size_t times_reset = 0;
+static void on_reset_callback_function(const void * timer, size_t n)
+{
+  (void) timer;
+  times_reset += n;
+}
+static void on_reset_callback_function_changed(const void * timer, size_t n)
+{
+  (void) timer;
+  times_reset -= n;
+}
+
 class TestPreInitTimer : public TestTimerFixture
 {
 public:
@@ -860,6 +872,34 @@ TEST_F(TestPreInitTimer, test_timer_exchange_callback) {
   EXPECT_EQ(times_called, 0);
 }
 
+TEST_F(TestPreInitTimer, test_on_reset_timer_callback) {
+  // Set callback to an invalid timer
+  EXPECT_EQ(
+    RCL_RET_INVALID_ARGUMENT,
+    rcl_timer_set_on_reset_callback(nullptr, nullptr, nullptr));
+
+  // Set a null on reset callback to a valid timer
+  ASSERT_EQ(
+    RCL_RET_OK, rcl_timer_set_on_reset_callback(
+      &timer, nullptr, nullptr)) << rcl_get_error_string().str;
+
+  // Reset 2 times, then set callback and check times_reset
+  times_reset = 0;
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_reset(&timer)) << rcl_get_error_string().str;
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_reset(&timer)) << rcl_get_error_string().str;
+  ASSERT_EQ(
+    RCL_RET_OK, rcl_timer_set_on_reset_callback(
+      &timer, on_reset_callback_function, nullptr)) << rcl_get_error_string().str;
+  EXPECT_EQ(times_reset, static_cast<size_t>(2));
+
+  // Assign a new on_reset callback
+  ASSERT_EQ(
+    RCL_RET_OK, rcl_timer_set_on_reset_callback(
+      &timer, on_reset_callback_function_changed, nullptr)) << rcl_get_error_string().str;
+  ASSERT_EQ(RCL_RET_OK, rcl_timer_reset(&timer)) << rcl_get_error_string().str;
+  EXPECT_EQ(times_reset, static_cast<size_t>(1));
+}
+
 TEST_F(TestPreInitTimer, test_invalid_get_guard) {
   ASSERT_EQ(NULL, rcl_timer_get_guard_condition(nullptr));
 }