From d975e4f66fc522ca6a4362673aaf7302312b4b87 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Mon, 14 Mar 2022 11:06:21 +0000 Subject: [PATCH] Add guard condition on trigger callback Signed-off-by: Mauro Passerino --- rcl/include/rcl/guard_condition.h | 37 +++++++++++++++++ rcl/src/rcl/guard_condition.c | 48 +++++++++++++++++++++- rcl/test/rcl/test_guard_condition.cpp | 58 +++++++++++++++++++++++++++ 3 files changed, 142 insertions(+), 1 deletion(-) diff --git a/rcl/include/rcl/guard_condition.h b/rcl/include/rcl/guard_condition.h index 368a9a6b4..87d8b670b 100644 --- a/rcl/include/rcl/guard_condition.h +++ b/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. + * + * + *
+ * 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 diff --git a/rcl/src/rcl/guard_condition.c b/rcl/src/rcl/guard_condition.c index 62974347d..83e5088fc 100644 --- a/rcl/src/rcl/guard_condition.c +++ b/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 diff --git a/rcl/test/rcl/test_guard_condition.cpp b/rcl/test/rcl/test_guard_condition.cpp index 48d4ede4d..71d11d3d5 100644 --- a/rcl/test/rcl/test_guard_condition.cpp +++ b/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(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); +}