From f8a630cfb5a2c6cab2fd6b0b6037bc09c80d6096 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Wed, 28 Jul 2021 12:28:24 +0200 Subject: [PATCH] refactor #116 remove callback_type (#154) (#163) * refactor #116 remove callback_type Signed-off-by: BrettRD * rename subscription callback Signed-off-by: BrettRD * deduplicate remove functions Signed-off-by: BrettRD (cherry picked from commit 700ea24671d731e610e7c33ec0bd830e4c8eb27d) Co-authored-by: Brett Downing --- rclc/README.md | 2 +- rclc/include/rclc/executor.h | 2 +- rclc/include/rclc/executor_handle.h | 29 ++- rclc/src/rclc/executor.c | 297 ++++++++++++------------ rclc/src/rclc/executor_handle.c | 18 +- rclc/test/rclc/test_executor_handle.cpp | 2 +- 6 files changed, 184 insertions(+), 166 deletions(-) diff --git a/rclc/README.md b/rclc/README.md index df810fa7..df35f1a2 100644 --- a/rclc/README.md +++ b/rclc/README.md @@ -308,7 +308,7 @@ With `rclc_executor_trigger_any` being the default trigger condition, the curren With the `rclc_executor_trigger_one` trigger, the handle to trigger is specified with `trigger_object`. In the other cases of the trigger conditions this parameter shall be `NULL`. -**rclc_executor_add_subscription(rclc_executor_t * executor, rcl_subscription_t * subscription, void * msg, rclc_callback_t callback, rclc_executor_handle_invocation_t invocation)** +**rclc_executor_add_subscription(rclc_executor_t * executor, rcl_subscription_t * subscription, void * msg, rclc_subscription_callback_t callback, rclc_executor_handle_invocation_t invocation)** **rclc_executor_add_timer( rclc_executor_t * executor, rcl_timer_t * timer)** diff --git a/rclc/include/rclc/executor.h b/rclc/include/rclc/executor.h index 4dda219e..2e8e6a1c 100644 --- a/rclc/include/rclc/executor.h +++ b/rclc/include/rclc/executor.h @@ -233,7 +233,7 @@ rclc_executor_add_subscription( rclc_executor_t * executor, rcl_subscription_t * subscription, void * msg, - rclc_callback_t callback, + rclc_subscription_callback_t callback, rclc_executor_handle_invocation_t invocation); /** diff --git a/rclc/include/rclc/executor_handle.h b/rclc/include/rclc/executor_handle.h index f9825c71..20003511 100644 --- a/rclc/include/rclc/executor_handle.h +++ b/rclc/include/rclc/executor_handle.h @@ -31,10 +31,17 @@ extern "C" typedef enum { SUBSCRIPTION, + SUBSCRIPTION_WITH_CONTEXT, TIMER, + // TIMER_WITH_CONTEXT, // TODO CLIENT, + CLIENT_WITH_REQUEST_ID, + // CLIENT_WITH_CONTEXT, // TODO SERVICE, + SERVICE_WITH_REQUEST_ID, + SERVICE_WITH_CONTEXT, GUARD_CONDITION, + // GUARD_CONDITION_WITH_CONTEXT, //TODO NONE } rclc_executor_handle_type_t; @@ -46,21 +53,13 @@ typedef enum ALWAYS } rclc_executor_handle_invocation_t; -typedef enum -{ - CB_UNDEFINED, - CB_WITHOUT_REQUEST_ID, - CB_WITH_REQUEST_ID, - CB_WITH_CONTEXT, -} rclc_executor_handle_callback_type_t; - - -/// Type definition for callback function. -typedef void (* rclc_callback_t)(const void *); - /// Type definition for subscription callback function /// - incoming message -// typedef void (* rclc_subscription_callback_t)(const void *); +typedef void (* rclc_subscription_callback_t)(const void *); + +/// Type definition (duplicate) for subscription callback function (alias for foxy and galactic). +/// - incoming message +typedef rclc_subscription_callback_t rclc_callback_t; /// Type definition for subscription callback function /// - incoming message @@ -137,7 +136,7 @@ typedef struct /// Storage for callbacks union { - rclc_callback_t callback; + rclc_subscription_callback_t subscription_callback; rclc_subscription_callback_with_context_t subscription_callback_with_context; rclc_service_callback_t service_callback; rclc_service_callback_with_request_id_t service_callback_with_reqid; @@ -159,8 +158,6 @@ typedef struct /// Interval variable. Flag, which is true, if new data is available from DDS queue /// (is set after calling rcl_take) bool data_available; - /// callback type for service/client - rclc_executor_handle_callback_type_t callback_type; } rclc_executor_handle_t; /// Information about total number of subscriptions, guard_conditions, timers, subscription etc. diff --git a/rclc/src/rclc/executor.c b/rclc/src/rclc/executor.c index d1d94b25..63906411 100644 --- a/rclc/src/rclc/executor.c +++ b/rclc/src/rclc/executor.c @@ -203,7 +203,7 @@ rclc_executor_add_subscription( rclc_executor_t * executor, rcl_subscription_t * subscription, void * msg, - rclc_callback_t callback, + rclc_subscription_callback_t callback, rclc_executor_handle_invocation_t invocation) { RCL_CHECK_ARGUMENT_FOR_NULL(executor, RCL_RET_INVALID_ARGUMENT); @@ -221,10 +221,10 @@ rclc_executor_add_subscription( executor->handles[executor->index].type = SUBSCRIPTION; executor->handles[executor->index].subscription = subscription; executor->handles[executor->index].data = msg; - executor->handles[executor->index].callback = callback; - executor->handles[executor->index].callback_type = CB_WITHOUT_REQUEST_ID; + executor->handles[executor->index].subscription_callback = callback; executor->handles[executor->index].invocation = invocation; executor->handles[executor->index].initialized = true; + executor->handles[executor->index].callback_context = NULL; // increase index of handle array executor->index++; @@ -267,11 +267,10 @@ rclc_executor_add_subscription_with_context( } // assign data fields - executor->handles[executor->index].type = SUBSCRIPTION; + executor->handles[executor->index].type = SUBSCRIPTION_WITH_CONTEXT; executor->handles[executor->index].subscription = subscription; executor->handles[executor->index].data = msg; executor->handles[executor->index].subscription_callback_with_context = callback; - executor->handles[executor->index].callback_type = CB_WITH_CONTEXT; executor->handles[executor->index].invocation = invocation; executor->handles[executor->index].initialized = true; executor->handles[executor->index].callback_context = context; @@ -317,6 +316,7 @@ rclc_executor_add_timer( executor->handles[executor->index].timer = timer; executor->handles[executor->index].invocation = ON_NEW_DATA; // i.e. when timer elapsed executor->handles[executor->index].initialized = true; + executor->handles[executor->index].callback_context = NULL; // increase index of handle array executor->index++; @@ -359,10 +359,9 @@ rclc_executor_add_client( executor->handles[executor->index].client = client; executor->handles[executor->index].data = response_msg; executor->handles[executor->index].client_callback = callback; - executor->handles[executor->index].callback_type = CB_WITHOUT_REQUEST_ID; executor->handles[executor->index].invocation = ON_NEW_DATA; // i.e. when request came in executor->handles[executor->index].initialized = true; - + executor->handles[executor->index].callback_context = NULL; // increase index of handle array executor->index++; @@ -402,13 +401,13 @@ rclc_executor_add_client_with_request_id( } // assign data fields - executor->handles[executor->index].type = CLIENT; + executor->handles[executor->index].type = CLIENT_WITH_REQUEST_ID; executor->handles[executor->index].client = client; executor->handles[executor->index].data = response_msg; executor->handles[executor->index].client_callback_with_reqid = callback; - executor->handles[executor->index].callback_type = CB_WITH_REQUEST_ID; executor->handles[executor->index].invocation = ON_NEW_DATA; // i.e. when request came in executor->handles[executor->index].initialized = true; + executor->handles[executor->index].callback_context = NULL; // increase index of handle array executor->index++; @@ -456,9 +455,9 @@ rclc_executor_add_service( // TODO(jst3si) new type with req and resp message in data field. executor->handles[executor->index].data_response_msg = response_msg; executor->handles[executor->index].service_callback = callback; - executor->handles[executor->index].callback_type = CB_WITHOUT_REQUEST_ID; executor->handles[executor->index].invocation = ON_NEW_DATA; // invoce when request came in executor->handles[executor->index].initialized = true; + executor->handles[executor->index].callback_context = NULL; // increase index of handle array executor->index++; @@ -492,7 +491,6 @@ rclc_executor_add_service_with_context( RCL_CHECK_ARGUMENT_FOR_NULL(request_msg, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(response_msg, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(callback, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(context, RCL_RET_INVALID_ARGUMENT); //? rcl_ret_t ret = RCL_RET_OK; // array bound check if (executor->index >= executor->max_handles) { @@ -502,13 +500,12 @@ rclc_executor_add_service_with_context( } // assign data fields - executor->handles[executor->index].type = SERVICE; + executor->handles[executor->index].type = SERVICE_WITH_CONTEXT; executor->handles[executor->index].service = service; executor->handles[executor->index].data = request_msg; // TODO(jst3si) new type with req and resp message in data field. executor->handles[executor->index].data_response_msg = response_msg; executor->handles[executor->index].service_callback_with_context = callback; - executor->handles[executor->index].callback_type = CB_WITH_CONTEXT; executor->handles[executor->index].invocation = ON_NEW_DATA; // invoce when request came in executor->handles[executor->index].initialized = true; executor->handles[executor->index].callback_context = context; @@ -553,15 +550,15 @@ rclc_executor_add_service_with_request_id( } // assign data fields - executor->handles[executor->index].type = SERVICE; + executor->handles[executor->index].type = SERVICE_WITH_REQUEST_ID; executor->handles[executor->index].service = service; executor->handles[executor->index].data = request_msg; // TODO(jst3si) new type with req and resp message in data field. executor->handles[executor->index].data_response_msg = response_msg; executor->handles[executor->index].service_callback_with_reqid = callback; - executor->handles[executor->index].callback_type = CB_WITH_REQUEST_ID; executor->handles[executor->index].invocation = ON_NEW_DATA; // invoce when request came in executor->handles[executor->index].initialized = true; + executor->handles[executor->index].callback_context = NULL; // increase index of handle array executor->index++; @@ -604,6 +601,7 @@ rclc_executor_add_guard_condition( executor->handles[executor->index].gc_callback = callback; executor->handles[executor->index].invocation = ON_NEW_DATA; // invoce when request came in executor->handles[executor->index].initialized = true; + executor->handles[executor->index].callback_context = NULL; // increase index of handle array executor->index++; @@ -623,14 +621,23 @@ rclc_executor_add_guard_condition( return ret; } - +static rcl_ret_t -_rclc_executor_remove_handle(rclc_executor_t * executor, size_t handle_index) +_rclc_executor_remove_handle(rclc_executor_t * executor, rclc_executor_handle_t * handle) { RCL_CHECK_ARGUMENT_FOR_NULL(executor, RCL_RET_INVALID_ARGUMENT); + rcl_ret_t ret = RCL_RET_OK; - if (handle_index >= executor->index) { + // NULL will be returned by _rclc_executor_find_handle if the handle is not found + if (NULL == handle) { + RCL_SET_ERROR_MSG("handle not found in rclc_executor_remove_handle"); + return RCL_RET_ERROR; + } + + if ((handle >= &executor->handles[executor->index]) || + (handle < executor->handles)) + { RCL_SET_ERROR_MSG("Handle out of bounds"); return RCL_RET_ERROR; } @@ -641,8 +648,11 @@ _rclc_executor_remove_handle(rclc_executor_t * executor, size_t handle_index) // shorten the list of handles without changing the order of remaining handles executor->index--; - for (size_t i = handle_index; i < executor->index; i++) { - executor->handles[i] = executor->handles[i + 1]; + for (rclc_executor_handle_t * handle_dest = handle; + handle_dest < &executor->handles[executor->index]; + handle_dest++) + { + *handle_dest = *(handle_dest + 1); } ret = rclc_executor_handle_init(&executor->handles[executor->index], executor->max_handles); @@ -659,6 +669,23 @@ _rclc_executor_remove_handle(rclc_executor_t * executor, size_t handle_index) return ret; } +static +rclc_executor_handle_t * +_rclc_executor_find_handle( + rclc_executor_t * executor, + const void * rcl_handle) +{ + for (rclc_executor_handle_t * test_handle = executor->handles; + test_handle < &executor->handles[executor->index]; + test_handle++) + { + if (rcl_handle == rclc_executor_handle_get_ptr(test_handle)) { + return test_handle; + } + } + return NULL; +} + rcl_ret_t rclc_executor_remove_subscription( @@ -669,22 +696,15 @@ rclc_executor_remove_subscription( RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT); rcl_ret_t ret = RCL_RET_OK; - for (size_t i = 0; (i < executor->max_handles && executor->handles[i].initialized); i++) { - if (SUBSCRIPTION == executor->handles[i].type) { - if (subscription == executor->handles[i].subscription) { - ret = _rclc_executor_remove_handle(executor, i); - if (RCL_RET_OK != ret) { - RCL_SET_ERROR_MSG("Failed to remove handle in rclc_executor_remove_subscription."); - return ret; - } - executor->info.number_of_subscriptions--; - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Removed a subscription."); - return ret; - } - } + rclc_executor_handle_t * handle = _rclc_executor_find_handle(executor, subscription); + ret = _rclc_executor_remove_handle(executor, handle); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG("Failed to remove handle in rclc_executor_remove_subscription."); + return ret; } - RCL_SET_ERROR_MSG("Subscription not found in rclc_executor_remove_subscription"); - return RCL_RET_ERROR; + executor->info.number_of_subscriptions--; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Removed a subscription."); + return ret; } rcl_ret_t @@ -696,22 +716,15 @@ rclc_executor_remove_timer( RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); rcl_ret_t ret = RCL_RET_OK; - for (size_t i = 0; (i < executor->max_handles && executor->handles[i].initialized); i++) { - if (TIMER == executor->handles[i].type) { - if (timer == executor->handles[i].timer) { - _rclc_executor_remove_handle(executor, i); - if (RCL_RET_OK != ret) { - RCL_SET_ERROR_MSG("Failed to remove handle in rclc_executor_remove_timer."); - return ret; - } - executor->info.number_of_timers--; - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Removed a timer."); - return ret; - } - } + rclc_executor_handle_t * handle = _rclc_executor_find_handle(executor, timer); + ret = _rclc_executor_remove_handle(executor, handle); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG("Failed to remove handle in rclc_executor_remove_timer."); + return ret; } - RCL_SET_ERROR_MSG("Timer not found in rclc_executor_remove_timer"); - return RCL_RET_ERROR; + executor->info.number_of_timers--; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Removed a timer."); + return ret; } rcl_ret_t @@ -723,22 +736,15 @@ rclc_executor_remove_client( RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT); rcl_ret_t ret = RCL_RET_OK; - for (size_t i = 0; (i < executor->max_handles && executor->handles[i].initialized); i++) { - if (CLIENT == executor->handles[i].type) { - if (client == executor->handles[i].client) { - _rclc_executor_remove_handle(executor, i); - if (RCL_RET_OK != ret) { - RCL_SET_ERROR_MSG("Failed to remove handle in rclc_executor_remove_client."); - return ret; - } - executor->info.number_of_clients--; - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Removed a client."); - return ret; - } - } + rclc_executor_handle_t * handle = _rclc_executor_find_handle(executor, client); + ret = _rclc_executor_remove_handle(executor, handle); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG("Failed to remove handle in rclc_executor_remove_client."); + return ret; } - RCL_SET_ERROR_MSG("Client not found in rclc_executor_remove_client"); - return RCL_RET_ERROR; + executor->info.number_of_clients--; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Removed a client."); + return ret; } rcl_ret_t @@ -750,22 +756,15 @@ rclc_executor_remove_service( RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT); rcl_ret_t ret = RCL_RET_OK; - for (size_t i = 0; (i < executor->max_handles && executor->handles[i].initialized); i++) { - if (SERVICE == executor->handles[i].type) { - if (service == executor->handles[i].service) { - _rclc_executor_remove_handle(executor, i); - if (RCL_RET_OK != ret) { - RCL_SET_ERROR_MSG("Failed to remove handle in rclc_executor_remove_service."); - return ret; - } - executor->info.number_of_services--; - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Removed a service."); - return ret; - } - } + rclc_executor_handle_t * handle = _rclc_executor_find_handle(executor, service); + ret = _rclc_executor_remove_handle(executor, handle); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG("Failed to remove handle in rclc_executor_remove_service."); + return ret; } - RCL_SET_ERROR_MSG("Service not found in rclc_executor_remove_service"); - return RCL_RET_ERROR; + executor->info.number_of_services--; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Removed a service."); + return ret; } @@ -778,22 +777,15 @@ rclc_executor_remove_guard_condition( RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT); rcl_ret_t ret = RCL_RET_OK; - for (size_t i = 0; (i < executor->max_handles && executor->handles[i].initialized); i++) { - if (GUARD_CONDITION == executor->handles[i].type) { - if (guard_condition == executor->handles[i].gc) { - _rclc_executor_remove_handle(executor, i); - if (RCL_RET_OK != ret) { - RCL_SET_ERROR_MSG("Failed to remove handle in rclc_executor_remove_guard_condition."); - return ret; - } - executor->info.number_of_guard_conditions--; - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Removed a guard condition."); - return ret; - } - } + rclc_executor_handle_t * handle = _rclc_executor_find_handle(executor, guard_condition); + ret = _rclc_executor_remove_handle(executor, handle); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG("Failed to remove handle in rclc_executor_remove_guard_condition."); + return ret; } - RCL_SET_ERROR_MSG("Guard Condition not found in rclc_executor_remove_guard_condition"); - return RCL_RET_ERROR; + executor->info.number_of_guard_conditions--; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Removed a guard condition."); + return ret; } @@ -815,12 +807,14 @@ _rclc_check_for_new_data(rclc_executor_handle_t * handle, rcl_wait_set_t * wait_ switch (handle->type) { case SUBSCRIPTION: + case SUBSCRIPTION_WITH_CONTEXT: if (wait_set->subscriptions[handle->index]) { handle->data_available = true; } break; case TIMER: + // case TIMER_WITH_CONTEXT: if (wait_set->timers[handle->index]) { bool timer_is_ready = false; rc = rcl_timer_is_ready(handle->timer, &timer_is_ready); @@ -841,18 +835,23 @@ _rclc_check_for_new_data(rclc_executor_handle_t * handle, rcl_wait_set_t * wait_ break; case SERVICE: + case SERVICE_WITH_REQUEST_ID: + case SERVICE_WITH_CONTEXT: if (wait_set->services[handle->index]) { handle->data_available = true; } break; case CLIENT: + case CLIENT_WITH_REQUEST_ID: + // case CLIENT_WITH_CONTEXT: if (wait_set->clients[handle->index]) { handle->data_available = true; } break; case GUARD_CONDITION: + // case GUARD_CONDITION_WITH_CONTEXT: if (wait_set->guard_conditions[handle->index]) { handle->data_available = true; } @@ -880,6 +879,7 @@ _rclc_take_new_data(rclc_executor_handle_t * handle, rcl_wait_set_t * wait_set) switch (handle->type) { case SUBSCRIPTION: + case SUBSCRIPTION_WITH_CONTEXT: if (wait_set->subscriptions[handle->index]) { rmw_message_info_t messageInfo; rc = rcl_take( @@ -897,11 +897,14 @@ _rclc_take_new_data(rclc_executor_handle_t * handle, rcl_wait_set_t * wait_set) break; case TIMER: + // case TIMER_WITH_CONTEXT: // nothing to do // notification, that timer is ready already done in _rclc_evaluate_data_availability() break; case SERVICE: + case SERVICE_WITH_REQUEST_ID: + case SERVICE_WITH_CONTEXT: if (wait_set->services[handle->index]) { rc = rcl_take_request( handle->service, &handle->req_id, handle->data); @@ -917,6 +920,8 @@ _rclc_take_new_data(rclc_executor_handle_t * handle, rcl_wait_set_t * wait_set) break; case CLIENT: + case CLIENT_WITH_REQUEST_ID: + // case CLIENT_WITH_CONTEXT: if (wait_set->clients[handle->index]) { rc = rcl_take_response( handle->client, &handle->req_id, handle->data); @@ -932,6 +937,7 @@ _rclc_take_new_data(rclc_executor_handle_t * handle, rcl_wait_set_t * wait_set) break; case GUARD_CONDITION: + // case GUARD_CONDITION_WITH_CONTEXT: // nothing to do break; @@ -974,32 +980,27 @@ _rclc_execute(rclc_executor_handle_t * handle) if (invoke_callback) { switch (handle->type) { case SUBSCRIPTION: - switch (handle->callback_type) { - case CB_WITHOUT_REQUEST_ID: - if (handle->data_available) { - handle->callback(handle->data); - } else { - handle->callback(NULL); - } - break; - case CB_WITH_CONTEXT: - if (handle->data_available) { - handle->subscription_callback_with_context( - handle->data, - handle->callback_context); - } else { - handle->subscription_callback_with_context( - NULL, - handle->callback_context); - } - break; - default: - PRINT_RCLC_ERROR(rclc_execute, unknown_callback_type); - break; + if (handle->data_available) { + handle->subscription_callback(handle->data); + } else { + handle->subscription_callback(NULL); + } + break; + + case SUBSCRIPTION_WITH_CONTEXT: + if (handle->data_available) { + handle->subscription_callback_with_context( + handle->data, + handle->callback_context); + } else { + handle->subscription_callback_with_context( + NULL, + handle->callback_context); } break; case TIMER: + // case TIMER_WITH_CONTEXT: rc = rcl_timer_call(handle->timer); if (rc != RCL_RET_OK) { PRINT_RCLC_ERROR(rclc_execute, rcl_timer_call); @@ -1008,29 +1009,31 @@ _rclc_execute(rclc_executor_handle_t * handle) break; case SERVICE: - switch (handle->callback_type) { - case CB_WITHOUT_REQUEST_ID: + case SERVICE_WITH_REQUEST_ID: + case SERVICE_WITH_CONTEXT: + // differentiate user-side service types + switch (handle->type) { + case SERVICE: handle->service_callback( handle->data, handle->data_response_msg); break; - case CB_WITH_REQUEST_ID: + case SERVICE_WITH_REQUEST_ID: handle->service_callback_with_reqid( handle->data, &handle->req_id, handle->data_response_msg); break; - case CB_WITH_CONTEXT: + case SERVICE_WITH_CONTEXT: handle->service_callback_with_context( handle->data, handle->data_response_msg, handle->callback_context); break; default: - PRINT_RCLC_ERROR(rclc_execute, unknown_callback_type); - break; + break; // flow can't reach here } - + // handle rcl-side services rc = rcl_send_response(handle->service, &handle->req_id, handle->data_response_msg); if (rc != RCL_RET_OK) { PRINT_RCLC_ERROR(rclc_execute, rcl_send_response); @@ -1039,19 +1042,23 @@ _rclc_execute(rclc_executor_handle_t * handle) break; case CLIENT: - if (handle->callback_type == CB_WITHOUT_REQUEST_ID || - handle->callback_type == CB_WITH_CONTEXT) - { - handle->client_callback(handle->data); - } else if (handle->callback_type == CB_WITH_REQUEST_ID) { - handle->client_callback_with_reqid(handle->data, &handle->req_id); - } + handle->client_callback(handle->data); break; + case CLIENT_WITH_REQUEST_ID: + handle->client_callback_with_reqid(handle->data, &handle->req_id); + break; + + // case CLIENT_WITH_CONTEXT: //TODO + // break; + case GUARD_CONDITION: handle->gc_callback(); break; + // case GUARD_CONDITION_WITH_CONTEXT: //TODO + // break; + default: RCUTILS_LOG_DEBUG_NAMED( ROS_PACKAGE_NAME, "Error in _rclc_execute: unknwon handle type: %d", @@ -1209,6 +1216,7 @@ rclc_executor_spin_some(rclc_executor_t * executor, const uint64_t timeout_ns) RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "wait_set_add_* %d", executor->handles[i].type); switch (executor->handles[i].type) { case SUBSCRIPTION: + case SUBSCRIPTION_WITH_CONTEXT: // add subscription to wait_set and save index rc = rcl_wait_set_add_subscription( &executor->wait_set, executor->handles[i].subscription, @@ -1225,6 +1233,7 @@ rclc_executor_spin_some(rclc_executor_t * executor, const uint64_t timeout_ns) break; case TIMER: + // case TIMER_WITH_CONTEXT: // add timer to wait_set and save index rc = rcl_wait_set_add_timer( &executor->wait_set, executor->handles[i].timer, @@ -1240,6 +1249,8 @@ rclc_executor_spin_some(rclc_executor_t * executor, const uint64_t timeout_ns) break; case SERVICE: + case SERVICE_WITH_REQUEST_ID: + case SERVICE_WITH_CONTEXT: // add service to wait_set and save index rc = rcl_wait_set_add_service( &executor->wait_set, executor->handles[i].service, @@ -1256,6 +1267,8 @@ rclc_executor_spin_some(rclc_executor_t * executor, const uint64_t timeout_ns) case CLIENT: + case CLIENT_WITH_REQUEST_ID: + // case CLIENT_WITH_CONTEXT: // add client to wait_set and save index rc = rcl_wait_set_add_client( &executor->wait_set, executor->handles[i].client, @@ -1271,6 +1284,7 @@ rclc_executor_spin_some(rclc_executor_t * executor, const uint64_t timeout_ns) break; case GUARD_CONDITION: + // case GUARD_CONDITION_WITH_CONTEXT: // add guard_condition to wait_set and save index rc = rcl_wait_set_add_guard_condition( &executor->wait_set, executor->handles[i].gc, @@ -1434,20 +1448,13 @@ bool rclc_executor_trigger_one(rclc_executor_handle_t * handles, unsigned int si for (unsigned int i = 0; i < size; i++) { if (handles[i].initialized) { if (handles[i].data_available == true) { - switch (handles[i].type) { - case SUBSCRIPTION: - if (handles[i].subscription == obj) { - return true; - } - break; - case TIMER: - if (handles[i].timer == obj) { - return true; - } - break; - default: - // non-supported type - return false; + void * handle_obj_ptr = rclc_executor_handle_get_ptr(&handles[i]); + if (NULL == handle_obj_ptr) { + // rclc_executor_handle_get_ptr returns null for unsupported types + return false; + } + if (obj == handle_obj_ptr) { + return true; } } } else { diff --git a/rclc/src/rclc/executor_handle.c b/rclc/src/rclc/executor_handle.c index e87b6075..7b9225aa 100644 --- a/rclc/src/rclc/executor_handle.c +++ b/rclc/src/rclc/executor_handle.c @@ -50,7 +50,7 @@ rclc_executor_handle_init( handle->data_response_msg = NULL; handle->callback_context = NULL; - handle->callback = NULL; + handle->subscription_callback = NULL; // because of union structure: // handle->service_callback == NULL; // handle->client_callback == NULL; @@ -60,7 +60,6 @@ rclc_executor_handle_init( handle->index = max_handles; handle->initialized = false; handle->data_available = false; - handle->callback_type = CB_UNDEFINED; return RCL_RET_OK; } @@ -89,18 +88,25 @@ rclc_executor_handle_print(rclc_executor_handle_t * handle) typeName = "None"; break; case SUBSCRIPTION: + case SUBSCRIPTION_WITH_CONTEXT: typeName = "Sub"; break; case TIMER: + // case TIMER_WITH_CONTEXT: typeName = "Timer"; break; case CLIENT: + case CLIENT_WITH_REQUEST_ID: + // case CLIENT_WITH_CONTEXT: typeName = "Client"; break; case SERVICE: + case SERVICE_WITH_REQUEST_ID: + case SERVICE_WITH_CONTEXT: typeName = "Service"; break; case GUARD_CONDITION: + // case GUARD_CONDITION_WITH_CONTEXT: typeName = "GuardCondition"; break; default: @@ -123,20 +129,28 @@ rclc_executor_handle_get_ptr(rclc_executor_handle_t * handle) void * ptr; switch (handle->type) { case SUBSCRIPTION: + case SUBSCRIPTION_WITH_CONTEXT: ptr = handle->subscription; break; case TIMER: + // case TIMER_WITH_CONTEXT: ptr = handle->timer; break; case CLIENT: + case CLIENT_WITH_REQUEST_ID: + // case CLIENT_WITH_CONTEXT: ptr = handle->client; break; case SERVICE: + case SERVICE_WITH_REQUEST_ID: + case SERVICE_WITH_CONTEXT: ptr = handle->service; break; case GUARD_CONDITION: + // case GUARD_CONDITION_WITH_CONTEXT: ptr = handle->gc; break; + case NONE: default: ptr = NULL; } diff --git a/rclc/test/rclc/test_executor_handle.cpp b/rclc/test/rclc/test_executor_handle.cpp index 64742f12..586ef156 100644 --- a/rclc/test/rclc/test_executor_handle.cpp +++ b/rclc/test/rclc/test_executor_handle.cpp @@ -54,7 +54,7 @@ TEST(Test, executor_handle_init) { EXPECT_EQ(handle.gc, nullptr); EXPECT_EQ(handle.data, nullptr); - EXPECT_EQ(handle.callback, nullptr); + EXPECT_EQ(handle.subscription_callback, nullptr); EXPECT_EQ(handle.index, max_handles); EXPECT_EQ(handle.initialized, false); EXPECT_EQ(handle.data_available, false);