Add rcl guard condition on_trigger_callback

rcl/include/rcl/guard_condition.h

@@ -24,6 +24,7 @@ extern "C"
 
 #include "rcl/allocator.h"
 #include "rcl/context.h"
+#include "rcl/event_callback.h"
 #include "rcl/macros.h"
 #include "rcl/types.h"
 #include "rcl/visibility_control.h"
@@ -48,6 +49,14 @@ typedef struct rcl_guard_condition_options_s
   rcl_allocator_t allocator;
 } rcl_guard_condition_options_t;
 
+/// On trigger callback data
+typedef struct rcl_guard_condition_callback_data_s
+{
+  rcl_event_callback_t on_trigger_callback;
+  const void * user_data;
+  size_t trigger_count;
+} rcl_guard_condition_callback_data_t;
+
 /// Return a rcl_guard_condition_t struct with members set to `NULL`.
 RCL_PUBLIC
 RCL_WARN_UNUSED
@@ -262,6 +271,34 @@ RCL_WARN_UNUSED
 rmw_guard_condition_t *
 rcl_guard_condition_get_rmw_handle(const rcl_guard_condition_t * guard_condition);
 
+/// Set the on trigger callback function for the guard_condition.
+/**
+ * This API sets the callback function to be called whenever the
+ * guard_condition is triggered.
+ *
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | No
+ *
+ * \param[in] guard_condition The guard_condition on which to set the callback
+ * \param[in] on_trigger_callback The callback to be called when guard condition is triggered
+ * \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 `guard_condition` is NULL
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_guard_condition_set_on_trigger_callback(
+  const rcl_guard_condition_t * guard_condition,
+  rcl_event_callback_t on_trigger_callback,
+  const void * user_data);
+
 #ifdef __cplusplus
 }
 #endif

rcl/src/rcl/guard_condition.c

@@ -31,6 +31,7 @@ struct rcl_guard_condition_impl_s
   rmw_guard_condition_t * rmw_handle;
   bool allocated_rmw_guard_condition;
   rcl_guard_condition_options_t options;
+  rcl_guard_condition_callback_data_t callback_data;
 };
 
 rcl_guard_condition_t
@@ -105,7 +106,19 @@ rcl_guard_condition_init(
   const rcl_guard_condition_options_t options)
 {
   // NULL indicates "create a new rmw guard condition".
-  return __rcl_guard_condition_init_from_rmw_impl(guard_condition, NULL, context, options);
+  rcl_ret_t ret = __rcl_guard_condition_init_from_rmw_impl(
+    guard_condition, NULL, context, options);
+
+  if (ret != RCL_RET_OK) {
+    return ret;
+  }
+
+  // Empty init callback data
+  guard_condition->impl->callback_data.on_trigger_callback = NULL;
+  guard_condition->impl->callback_data.user_data = NULL;
+  guard_condition->impl->callback_data.trigger_count = 0;
+
+  return RCL_RET_OK;
 }
 
 rcl_ret_t
@@ -161,6 +174,14 @@ rcl_trigger_guard_condition(rcl_guard_condition_t * guard_condition)
     RCL_SET_ERROR_MSG(rmw_get_error_string().str);
     return RCL_RET_ERROR;
   }
+
+  rcl_guard_condition_callback_data_t * cb_data = &guard_condition->impl->callback_data;
+
+  if (cb_data->on_trigger_callback) {
+    cb_data->on_trigger_callback(cb_data->user_data, 1);
+  } else {
+    cb_data->trigger_count++;
+  }
   return RCL_RET_OK;
 }
 
@@ -186,6 +207,31 @@ rcl_guard_condition_get_rmw_handle(const rcl_guard_condition_t * guard_condition
   return guard_condition->impl->rmw_handle;
 }
 
+rcl_ret_t
+rcl_guard_condition_set_on_trigger_callback(
+  const rcl_guard_condition_t * guard_condition,
+  rcl_event_callback_t on_trigger_callback,
+  const void * user_data)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT);
+
+  rcl_guard_condition_callback_data_t * cb_data = &guard_condition->impl->callback_data;
+
+  if (on_trigger_callback) {
+    cb_data->on_trigger_callback = on_trigger_callback;
+    cb_data->user_data = user_data;
+    if (cb_data->trigger_count) {
+      cb_data->on_trigger_callback(user_data, cb_data->trigger_count);
+      cb_data->trigger_count = 0;
+    }
+  } else {
+    cb_data->on_trigger_callback = NULL;
+    cb_data->user_data = NULL;
+  }
+
+  return RCL_RET_OK;
+}
+
 #ifdef __cplusplus
 }
 #endif

rcl/test/rcl/test_guard_condition.cpp

@@ -251,3 +251,61 @@ TEST_F(
   EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_trigger_guard_condition(&zero_guard_condition));
   rcl_reset_error();
 }
+
+size_t global_trigger_count_ = 0;
+void guard_condition_on_trigger_callback(
+  const void * user_data,
+  size_t trigger_count)
+{
+  (void)user_data;
+  global_trigger_count_ = trigger_count;
+}
+
+/* Test rcl_guard_condition_set_on_trigger_callback
+ */
+TEST_F(
+  CLASSNAME(
+    TestGuardConditionFixture,
+    RMW_IMPLEMENTATION), test_rcl_guard_condition_on_trigger_callback) {
+  rcl_context_t context = rcl_get_zero_initialized_context();
+  rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition();
+  rcl_guard_condition_options_t default_options = rcl_guard_condition_get_default_options();
+  rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
+  rcl_init_options_init(&init_options, rcl_get_default_allocator());
+  rcl_init(0, nullptr, &init_options, &context);
+  rcl_guard_condition_init(&guard_condition, &context, default_options);
+  // Set on trigger callback to null guard condition
+  EXPECT_EQ(
+    RCL_RET_INVALID_ARGUMENT,
+    rcl_guard_condition_set_on_trigger_callback(nullptr, nullptr, nullptr));
+  // Trigger 3 times, set callback and check global counter
+  size_t trigger_times = 3;
+  for (size_t i = 0; i < trigger_times; i++) {
+    rcl_trigger_guard_condition(&guard_condition);
+  }
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_guard_condition_set_on_trigger_callback(
+      &guard_condition,
+      guard_condition_on_trigger_callback,
+      nullptr));
+  EXPECT_EQ(global_trigger_count_, trigger_times);
+  // Trigger 1 time and check global counter
+  rcl_trigger_guard_condition(&guard_condition);
+  EXPECT_EQ(global_trigger_count_, static_cast<size_t>(1));
+  // Unset callback
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_guard_condition_set_on_trigger_callback(&guard_condition, nullptr, nullptr));
+  // Trigger 3 times, reset callback and check counter
+  for (size_t i = 0; i < trigger_times; i++) {
+    rcl_trigger_guard_condition(&guard_condition);
+  }
+  EXPECT_EQ(
+    RCL_RET_OK,
+    rcl_guard_condition_set_on_trigger_callback(
+      &guard_condition,
+      guard_condition_on_trigger_callback,
+      nullptr));
+  EXPECT_EQ(global_trigger_count_, trigger_times);
+}