From c7e61834b4ac29d1d087b773c9cc976b0f996f95 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 14 Nov 2018 16:56:05 -0300 Subject: [PATCH] [rcl action] Use macros instead of static function for action client impl. --- rcl_action/include/rcl_action/action_client.h | 1 - rcl_action/src/rcl_action/action_client.c | 699 +++++------------- 2 files changed, 202 insertions(+), 498 deletions(-) diff --git a/rcl_action/include/rcl_action/action_client.h b/rcl_action/include/rcl_action/action_client.h index f90065d..7b680d7 100644 --- a/rcl_action/include/rcl_action/action_client.h +++ b/rcl_action/include/rcl_action/action_client.h @@ -415,7 +415,6 @@ rcl_action_take_status( const rcl_action_client_t * action_client, void * ros_status_array); - /// Send a request for the result of a completed goal associated with a rcl_action_client_t. /** * This is a non-blocking call. diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index 23fb9cc..48d4049 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -55,244 +55,77 @@ rcl_action_get_zero_initialized_client(void) return null_action_client; } -// \internal Initialize client for the given action's goal service. -static rcl_ret_t rcl_action_goal_service_client_init( - rcl_client_t * goal_client, - const rcl_node_t * node, - const rosidl_service_type_support_t * goal_service_type_support, - const char * action_name, - const rcl_client_options_t * goal_client_options) -{ - assert(NULL != goal_client); - assert(NULL != node); - assert(NULL != goal_service_type_support); - assert(NULL != action_name); - assert(NULL != goal_client_options); - - rcl_ret_t ret; - rcl_allocator_t allocator = goal_client_options->allocator; - - char * goal_service_name = NULL; - ret = rcl_action_get_goal_service_name(action_name, allocator, &goal_service_name); - if (RCL_RET_OK != ret) { - if (RCL_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; +// \internal Initializes an action client specific service client. +#define CLIENT_INIT(Type) \ + char * Type ## _service_name = NULL; \ + ret = rcl_action_get_ ## Type ## _service_name(action_name, allocator, &Type ## _service_name); \ + if (RCL_RET_OK != ret) { \ + RCL_SET_ERROR_MSG("failed to get " #Type " service name"); \ + if (RCL_RET_BAD_ALLOC == ret) { \ + ret = RCL_RET_BAD_ALLOC; \ + } else { \ + ret = RCL_RET_ERROR; \ + } \ + goto fail; \ + } \ + rcl_client_options_t Type ## _service_client_options = { \ + .qos = options->Type ## _service_qos, .allocator = allocator \ + }; \ + action_client->impl->Type ## _client = rcl_get_zero_initialized_client(); \ + ret = rcl_client_init( \ + &action_client->impl->Type ## _client, \ + node, \ + type_support->Type ## _service_type_support, \ + Type ## _service_name, \ + &Type ## _service_client_options); \ + allocator.deallocate(Type ## _service_name, allocator.state); \ + if (RCL_RET_OK != ret) { \ + if (RCL_RET_BAD_ALLOC == ret) { \ + ret = RCL_RET_BAD_ALLOC; \ + } else if (RCL_RET_SERVICE_NAME_INVALID == ret) { \ + ret = RCL_RET_ACTION_NAME_INVALID; \ + } else { \ + ret = RCL_RET_ERROR; \ + } \ + goto fail; \ } - *goal_client = rcl_get_zero_initialized_client(); - - ret = rcl_client_init( - goal_client, node, - goal_service_type_support, - goal_service_name, - goal_client_options); - - allocator.deallocate(goal_service_name, allocator.state); - - if (RCL_RET_OK != ret) { - if (RCL_RET_SERVICE_NAME_INVALID == ret) { - return RCL_RET_ACTION_NAME_INVALID; - } - if (RCL_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; +// \internal Initializes an action client specific topic subscription. +#define SUBSCRIPTION_INIT(Type) \ + char * Type ## _topic_name = NULL; \ + ret = rcl_action_get_ ## Type ## _topic_name(action_name, allocator, &Type ## _topic_name); \ + if (RCL_RET_OK != ret) { \ + RCL_SET_ERROR_MSG("failed to get " #Type " topic name"); \ + if (RCL_RET_BAD_ALLOC == ret) { \ + ret = RCL_RET_BAD_ALLOC; \ + } else { \ + ret = RCL_RET_ERROR; \ + } \ + goto fail; \ + } \ + rcl_subscription_options_t Type ## _topic_subscription_options = { \ + .qos = options->Type ## _topic_qos, \ + .ignore_local_publications = false, \ + .allocator = allocator \ + }; \ + action_client->impl->Type ## _subscription = rcl_get_zero_initialized_subscription(); \ + ret = rcl_subscription_init( \ + &action_client->impl->Type ## _subscription, \ + node, \ + type_support->Type ## _message_type_support, \ + Type ## _topic_name, \ + &Type ## _topic_subscription_options); \ + allocator.deallocate(Type ## _topic_name, allocator.state); \ + if (RCL_RET_OK != ret) { \ + if (RCL_RET_BAD_ALLOC == ret) { \ + ret = RCL_RET_BAD_ALLOC; \ + } else if (RCL_RET_TOPIC_NAME_INVALID == ret) { \ + ret = RCL_RET_ACTION_NAME_INVALID; \ + } else { \ + ret = RCL_RET_ERROR; \ + } \ + goto fail; \ } - return RCL_RET_OK; -} - -// \internal Initialize client for the given action's goal cancel service. -static rcl_ret_t rcl_action_cancel_service_client_init( - rcl_client_t * cancel_client, - const rcl_node_t * node, - const rosidl_service_type_support_t * cancel_service_type_support, - const char * action_name, - const rcl_client_options_t * cancel_client_options) -{ - assert(NULL != cancel_client); - assert(NULL != node); - assert(NULL != cancel_service_type_support); - assert(NULL != action_name); - assert(NULL != cancel_client_options); - - rcl_ret_t ret; - rcl_allocator_t allocator = cancel_client_options->allocator; - - char * cancel_service_name = NULL; - ret = rcl_action_get_cancel_service_name(action_name, allocator, &cancel_service_name); - if (RCL_RET_OK != ret) { - if (RCL_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - - *cancel_client = rcl_get_zero_initialized_client(); - - ret = rcl_client_init( - cancel_client, node, - cancel_service_type_support, - cancel_service_name, - cancel_client_options); - - allocator.deallocate(cancel_service_name, allocator.state); - - if (RCL_RET_OK != ret) { - if (RCL_RET_SERVICE_NAME_INVALID == ret) { - return RCL_RET_ACTION_NAME_INVALID; - } - if (RCL_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - return RCL_RET_OK; -} - -// \internal Initialize client for the given action's goal result service. -static rcl_ret_t rcl_action_result_client_init( - rcl_client_t * result_client, - const rcl_node_t * node, - const rosidl_service_type_support_t * result_service_type_support, - const char * action_name, - const rcl_client_options_t * result_client_options) -{ - assert(NULL != result_client); - assert(NULL != node); - assert(NULL != result_service_type_support); - assert(NULL != action_name); - assert(NULL != result_client_options); - - rcl_ret_t ret; - rcl_allocator_t allocator = result_client_options->allocator; - - char * result_service_name = NULL; - ret = rcl_action_get_result_service_name(action_name, allocator, &result_service_name); - if (RCL_RET_OK != ret) { - if (RCL_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - - *result_client = rcl_get_zero_initialized_client(); - - ret = rcl_client_init( - result_client, node, - result_service_type_support, - result_service_name, - result_client_options); - - allocator.deallocate(result_service_name, allocator.state); - - if (RCL_RET_OK != ret) { - if (RCL_RET_SERVICE_NAME_INVALID == ret) { - return RCL_RET_ACTION_NAME_INVALID; - } - if (RCL_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - return RCL_RET_OK; -} - -// \internal Initialize subscription to the given action's feedback topic. -static rcl_ret_t rcl_action_feedback_subscription_init( - rcl_subscription_t * feedback_subscription, - const rcl_node_t * node, - const rosidl_message_type_support_t * feedback_message_type_support, - const char * action_name, - const rcl_subscription_options_t * feedback_subscription_options) -{ - assert(NULL != feedback_subscription); - assert(NULL != node); - assert(NULL != feedback_message_type_support); - assert(NULL != action_name); - assert(NULL != feedback_subscription_options); - - rcl_ret_t ret; - rcl_allocator_t allocator = feedback_subscription_options->allocator; - - char * feedback_topic_name = NULL; - ret = rcl_action_get_feedback_topic_name(action_name, allocator, &feedback_topic_name); - if (RCL_RET_OK != ret) { - if (RCL_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - - *feedback_subscription = rcl_get_zero_initialized_subscription(); - - ret = rcl_subscription_init( - feedback_subscription, node, - feedback_message_type_support, - feedback_topic_name, - feedback_subscription_options); - - allocator.deallocate(feedback_topic_name, allocator.state); - - if (RCL_RET_OK != ret) { - if (RCL_RET_TOPIC_NAME_INVALID == ret) { - return RCL_RET_ACTION_NAME_INVALID; - } - if (RCL_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - return RCL_RET_OK; -} - -// \internal Initialize subscription to the given action's status topic. -static rcl_ret_t rcl_action_status_subscription_init( - rcl_subscription_t * status_subscription, const rcl_node_t * node, - const rosidl_message_type_support_t * status_message_type_support, - const char * action_name, - const rcl_subscription_options_t * status_subscription_options) -{ - assert(NULL != status_subscription); - assert(NULL != node); - assert(NULL != status_message_type_support); - assert(NULL != action_name); - assert(NULL != status_subscription_options); - - rcl_ret_t ret; - rcl_allocator_t allocator = status_subscription_options->allocator; - - char * status_topic_name = NULL; - ret = rcl_action_get_status_topic_name(action_name, allocator, &status_topic_name); - if (RCL_RET_OK != ret) { - if (RCL_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - - *status_subscription = rcl_get_zero_initialized_subscription(); - - ret = rcl_subscription_init( - status_subscription, node, - status_message_type_support, - status_topic_name, - status_subscription_options); - - allocator.deallocate(status_topic_name, allocator.state); - - if (RCL_RET_OK != ret) { - if (RCL_RET_TOPIC_NAME_INVALID == ret) { - return RCL_RET_ACTION_NAME_INVALID; - } - if (RCL_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - return RCL_RET_OK; -} rcl_ret_t rcl_action_client_init( @@ -309,9 +142,8 @@ rcl_action_client_init( RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(action_name, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); - rcl_allocator_t * allocator = (rcl_allocator_t *)&options->allocator; - RCL_CHECK_ALLOCATOR_WITH_MSG( - allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t allocator = options->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); rcl_ret_t ret = RCL_RET_OK; rcl_ret_t fini_ret = RCL_RET_OK; @@ -322,85 +154,27 @@ rcl_action_client_init( return RCL_RET_ALREADY_INIT; } // Allocate space for the implementation struct. - rcl_action_client_impl_t * impl = action_client->impl = allocator->allocate( - sizeof(rcl_action_client_impl_t), allocator->state); - RCL_CHECK_FOR_NULL_WITH_MSG(impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); + action_client->impl = allocator.allocate(sizeof(rcl_action_client_impl_t), allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + action_client->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); + // Copy action client name and options. - impl->options = *options; - impl->action_name = rcutils_strdup(action_name, *((rcutils_allocator_t *)allocator)); - if (NULL == impl->action_name) { + action_client->impl->action_name = rcutils_strdup(action_name, allocator); + if (NULL == action_client->impl->action_name) { ret = RCL_RET_BAD_ALLOC; goto fail; } - // Initialize action goal service client. - rcl_client_options_t goal_client_options = { - .qos = options->goal_service_qos, .allocator = *allocator - }; - ret = rcl_action_goal_service_client_init( - &impl->goal_client, node, - type_support->goal_service_type_support, - impl->action_name, &goal_client_options); - if (RCL_RET_OK != ret) { - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Failed to initialize action goal client"); - goto fail; - } - // Initialize action cancel service client. - rcl_client_options_t cancel_client_options = { - .qos = options->cancel_service_qos, .allocator = *allocator - }; - ret = rcl_action_cancel_service_client_init( - &impl->cancel_client, node, - type_support->cancel_service_type_support, - impl->action_name, &cancel_client_options); - if (RCL_RET_OK != ret) { - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Failed to initialize action cancel client"); - goto fail; - } - // Initialize action result service client. - rcl_client_options_t result_client_options = { - .qos = options->result_service_qos, .allocator = *allocator - }; - ret = rcl_action_result_client_init( - &impl->result_client, node, - type_support->result_service_type_support, - impl->action_name, &result_client_options); - if (RCL_RET_OK != ret) { - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Failed to initialize action result client"); - goto fail; - } - // Initialize action feedback subscription client. - rcl_subscription_options_t feedback_subscription_options = { - .qos = options->feedback_topic_qos, - .ignore_local_publications = false, - .allocator = *allocator - }; - ret = rcl_action_feedback_subscription_init( - &impl->feedback_subscription, node, - type_support->feedback_message_type_support, - impl->action_name, &feedback_subscription_options); - if (RCL_RET_OK != ret) { - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Failed to initialize feedback subscription"); - goto fail; - } - // Initialize action status subscription client. - rcl_subscription_options_t status_subscription_options = { - .qos = options->status_topic_qos, - .ignore_local_publications = false, - .allocator = *allocator - }; - ret = rcl_action_status_subscription_init( - &impl->status_subscription, node, - type_support->status_message_type_support, - impl->action_name, &status_subscription_options); - if (RCL_RET_OK != ret) { - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Failed to initialize status subscription"); - goto fail; - } + action_client->impl->options = *options; + + // Initialize action service clients. + CLIENT_INIT(goal); + CLIENT_INIT(cancel); + CLIENT_INIT(result); + + // Initialize action topic subscriptions. + SUBSCRIPTION_INIT(feedback); + SUBSCRIPTION_INIT(status); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client initialized"); return ret; fail: @@ -423,40 +197,23 @@ rcl_action_client_fini(rcl_action_client_t * action_client, rcl_node_t * node) return RCL_RET_NODE_INVALID; // error already set } rcl_ret_t ret = RCL_RET_OK; - rcl_ret_t fini_ret = RCL_RET_OK; - rcl_action_client_impl_t * impl = action_client->impl; - if (rcl_client_is_valid(&impl->goal_client)) { - fini_ret = rcl_client_fini(&impl->goal_client, node); - if (RCL_RET_OK != fini_ret) { - ret = RCL_RET_ERROR; - } + if (RCL_RET_OK != rcl_client_fini(&action_client->impl->goal_client, node)) { + ret = RCL_RET_ERROR; } - if (rcl_client_is_valid(&impl->cancel_client)) { - fini_ret = rcl_client_fini(&impl->cancel_client, node); - if (RCL_RET_OK != fini_ret) { - ret = RCL_RET_ERROR; - } + if (RCL_RET_OK != rcl_client_fini(&action_client->impl->cancel_client, node)) { + ret = RCL_RET_ERROR; } - if (rcl_client_is_valid(&impl->result_client)) { - fini_ret = rcl_client_fini(&impl->result_client, node); - if (RCL_RET_OK != fini_ret) { - ret = RCL_RET_ERROR; - } + if (RCL_RET_OK != rcl_client_fini(&action_client->impl->result_client, node)) { + ret = RCL_RET_ERROR; } - if (rcl_subscription_is_valid(&impl->feedback_subscription)) { - fini_ret = rcl_subscription_fini(&impl->feedback_subscription, node); - if (RCL_RET_OK != fini_ret) { - ret = RCL_RET_ERROR; - } + if (RCL_RET_OK != rcl_subscription_fini(&action_client->impl->feedback_subscription, node)) { + ret = RCL_RET_ERROR; } - if (rcl_subscription_is_valid(&impl->status_subscription)) { - fini_ret = rcl_subscription_fini(&impl->status_subscription, node); - if (RCL_RET_OK != fini_ret) { - ret = RCL_RET_ERROR; - } + if (RCL_RET_OK != rcl_subscription_fini(&action_client->impl->status_subscription, node)) { + ret = RCL_RET_ERROR; } - rcl_allocator_t * allocator = &impl->options.allocator; - allocator->deallocate(impl->action_name, allocator->state); + rcl_allocator_t * allocator = &action_client->impl->options.allocator; + allocator->deallocate(action_client->impl->action_name, allocator->state); allocator->deallocate(action_client->impl, allocator->state); action_client->impl = NULL; RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client finalized"); @@ -476,25 +233,51 @@ rcl_action_client_get_default_options(void) return default_options; } +// \internal Sends an action client specific service request. +#define SEND_SERVICE_REQUEST(Type) \ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action " #Type " request"); \ + if (!rcl_action_client_is_valid(action_client)) { \ + return RCL_RET_ACTION_SERVER_INVALID; /* error already set */ \ + } \ + RCL_CHECK_ARGUMENT_FOR_NULL(ros_ ## Type ## _request, RCL_RET_INVALID_ARGUMENT); \ + int64_t sequence_number; /* ignored */ \ + rcl_ret_t ret = rcl_send_request( \ + &action_client->impl->Type ## _client, ros_ ## Type ## _request, &sequence_number); \ + if (RCL_RET_OK != ret) { \ + return RCL_RET_ERROR; /* error already set */ \ + } \ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action " #Type " request sent"); \ + return RCL_RET_OK; + +// \internal Takes an action client specific service response. +#define TAKE_SERVICE_RESPONSE(Type) \ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action " #Type " response"); \ + if (!rcl_action_client_is_valid(action_client)) { \ + return RCL_RET_ACTION_SERVER_INVALID; /* error already set */ \ + } \ + RCL_CHECK_ARGUMENT_FOR_NULL(ros_ ## Type ## _response, RCL_RET_INVALID_ARGUMENT); \ + rmw_request_id_t request_header; /* ignored */ \ + rcl_ret_t ret = rcl_take_response( \ + &action_client->impl->Type ## _client, &request_header, ros_ ## Type ## _response); \ + if (RCL_RET_OK != ret) { \ + if (RCL_RET_BAD_ALLOC == ret) { \ + return RCL_RET_BAD_ALLOC; /* error already set */ \ + } \ + if (RCL_RET_CLIENT_TAKE_FAILED == ret) { \ + return RCL_RET_ACTION_CLIENT_TAKE_FAILED; \ + } \ + return RCL_RET_ERROR; /* error already set */ \ + } \ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action " #Type " response taken"); \ + return RCL_RET_OK; + + rcl_ret_t rcl_action_send_goal_request( const rcl_action_client_t * action_client, const void * ros_goal_request) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action goal request"); - if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; // error already set - } - RCL_CHECK_ARGUMENT_FOR_NULL(ros_goal_request, RCL_RET_INVALID_ARGUMENT); - int64_t ignored_sequence_number; - rcl_ret_t ret = rcl_send_request( - &action_client->impl->goal_client, - ros_goal_request, &ignored_sequence_number); - if (RCL_RET_OK != ret) { - return RCL_RET_ERROR; - } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action goal request sent"); - return RCL_RET_OK; + SEND_SERVICE_REQUEST(goal) } rcl_ret_t @@ -502,79 +285,7 @@ rcl_action_take_goal_response( const rcl_action_client_t * action_client, void * ros_goal_response) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action goal response"); - if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; // error already set - } - RCL_CHECK_ARGUMENT_FOR_NULL(ros_goal_response, RCL_RET_INVALID_ARGUMENT); - rmw_request_id_t ignored_request_header; - rcl_ret_t ret = rcl_take_response( - &action_client->impl->goal_client, - &ignored_request_header, ros_goal_response); - if (RCL_RET_OK != ret) { - if (RCL_RET_CLIENT_TAKE_FAILED == ret) { - ret = RCL_RET_ACTION_CLIENT_TAKE_FAILED; - } else { - ret = RCL_RET_ERROR; - } - return ret; - } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action goal response taken"); - return RCL_RET_OK; -} - -rcl_ret_t -rcl_action_take_feedback( - const rcl_action_client_t * action_client, - void * ros_feedback) -{ - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action feedback"); - if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; // error already set - } - RCL_CHECK_ARGUMENT_FOR_NULL(ros_feedback, RCL_RET_INVALID_ARGUMENT); - rmw_message_info_t ignored_message_info; - rcl_ret_t ret = rcl_take( - &action_client->impl->feedback_subscription, - ros_feedback, &ignored_message_info); - if (RCL_RET_OK != ret) { - if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { - return RCL_RET_ACTION_CLIENT_TAKE_FAILED; - } - if (RCL_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action feedback taken"); - return RCL_RET_OK; -} - -rcl_ret_t -rcl_action_take_status( - const rcl_action_client_t * action_client, - void * ros_status_array) -{ - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action status"); - if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; // error already set - } - RCL_CHECK_ARGUMENT_FOR_NULL(ros_status_array, RCL_RET_INVALID_ARGUMENT); - rmw_message_info_t ignored_message_info; - rcl_ret_t ret = rcl_take( - &action_client->impl->status_subscription, - ros_status_array, &ignored_message_info); - if (RCL_RET_OK != ret) { - if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { - return RCL_RET_ACTION_CLIENT_TAKE_FAILED; - } - if (RCL_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action status taken"); - return RCL_RET_OK; + TAKE_SERVICE_RESPONSE(goal); } rcl_ret_t @@ -582,47 +293,15 @@ rcl_action_send_result_request( const rcl_action_client_t * action_client, const void * ros_result_request) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action result request"); - if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; // error already set - } - RCL_CHECK_ARGUMENT_FOR_NULL(ros_result_request, RCL_RET_INVALID_ARGUMENT); - int64_t ignored_sequence_number; - rcl_ret_t ret = rcl_send_request( - &action_client->impl->result_client, - ros_result_request, &ignored_sequence_number); - if (RCL_RET_OK != ret) { - return RCL_RET_ERROR; - } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action result request sent"); - return RCL_RET_OK; + SEND_SERVICE_REQUEST(result); } rcl_ret_t rcl_action_take_result_response( const rcl_action_client_t * action_client, - void * ros_result) + void * ros_result_response) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action result response"); - if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; // error already set - } - RCL_CHECK_ARGUMENT_FOR_NULL(ros_result, RCL_RET_INVALID_ARGUMENT); - rmw_request_id_t ignored_response_header; - rcl_ret_t ret = rcl_take_response( - &action_client->impl->result_client, - &ignored_response_header, ros_result); - if (RCL_RET_OK != ret) { - if (RCL_RET_CLIENT_TAKE_FAILED == ret) { - return RCL_RET_ACTION_CLIENT_TAKE_FAILED; - } - if (RCL_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action result response taken"); - return RCL_RET_OK; + TAKE_SERVICE_RESPONSE(result); } rcl_ret_t @@ -630,20 +309,7 @@ rcl_action_send_cancel_request( const rcl_action_client_t * action_client, const void * ros_cancel_request) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action cancel request"); - if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; // error already set - } - RCL_CHECK_ARGUMENT_FOR_NULL(ros_cancel_request, RCL_RET_INVALID_ARGUMENT); - int64_t ignored_sequence_number; - rcl_ret_t ret = rcl_send_request( - &action_client->impl->cancel_client, - ros_cancel_request, &ignored_sequence_number); - if (RCL_RET_OK != ret) { - return RCL_RET_ERROR; - } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action cancel request sent"); - return RCL_RET_OK; + SEND_SERVICE_REQUEST(cancel); } rcl_ret_t @@ -651,26 +317,45 @@ rcl_action_take_cancel_response( const rcl_action_client_t * action_client, void * ros_cancel_response) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action cancel response"); - if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; // error already set - } - RCL_CHECK_ARGUMENT_FOR_NULL(ros_cancel_response, RCL_RET_INVALID_ARGUMENT); - rmw_request_id_t ignored_response_header; - rcl_ret_t ret = rcl_take_response( - &action_client->impl->cancel_client, - &ignored_response_header, ros_cancel_response); - if (RCL_RET_OK != ret) { - if (RCL_RET_CLIENT_TAKE_FAILED == ret) { - return RCL_RET_ACTION_CLIENT_TAKE_FAILED; - } - if (RCL_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action cancel response taken"); + TAKE_SERVICE_RESPONSE(cancel); +} + +// \internal Takes an action client specific topic message. +#define TAKE_MESSAGE(Type) \ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action " #Type); \ + if (!rcl_action_client_is_valid(action_client)) { \ + return RCL_RET_ACTION_CLIENT_INVALID; /* error already set */ \ + } \ + RCL_CHECK_ARGUMENT_FOR_NULL(ros_ ## Type, RCL_RET_INVALID_ARGUMENT); \ + rmw_message_info_t message_info; /* ignored */ \ + rcl_ret_t ret = rcl_take( \ + &action_client->impl->Type ## _subscription, ros_ ## Type, &message_info); \ + if (RCL_RET_OK != ret) { \ + if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { \ + return RCL_RET_ACTION_CLIENT_TAKE_FAILED; \ + } \ + if (RCL_RET_BAD_ALLOC == ret) { \ + return RCL_RET_BAD_ALLOC; \ + } \ + return RCL_RET_ERROR; \ + } \ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action " #Type " taken"); \ return RCL_RET_OK; + +rcl_ret_t +rcl_action_take_feedback( + const rcl_action_client_t * action_client, + void * ros_feedback) +{ + TAKE_MESSAGE(feedback); +} + +rcl_ret_t +rcl_action_take_status( + const rcl_action_client_t * action_client, + void * ros_status) +{ + TAKE_MESSAGE(status); } const char * @@ -698,6 +383,26 @@ rcl_action_client_is_valid(const rcl_action_client_t * action_client) action_client, "action client pointer is invalid", return false); RCL_CHECK_FOR_NULL_WITH_MSG( action_client->impl, "action client implementation is invalid", return false); + if (!rcl_client_is_valid(&action_client->impl->goal_client)) { + RCL_SET_ERROR_MSG("goal client is invalid"); + return false; + } + if (!rcl_client_is_valid(&action_client->impl->cancel_client)) { + RCL_SET_ERROR_MSG("cancel client is invalid"); + return false; + } + if (!rcl_client_is_valid(&action_client->impl->result_client)) { + RCL_SET_ERROR_MSG("result client is invalid"); + return false; + } + if (!rcl_subscription_is_valid(&action_client->impl->feedback_subscription)) { + RCL_SET_ERROR_MSG("feedback subscription is invalid"); + return false; + } + if (!rcl_subscription_is_valid(&action_client->impl->status_subscription)) { + RCL_SET_ERROR_MSG("status subscription is invalid"); + return false; + } return true; }