From 16205873ea7eca3038addf80975d0b5da34beb5b Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 31 Oct 2018 14:12:08 -0300 Subject: [PATCH 01/11] [rcl action] Action client first implementation. --- rcl_action/CMakeLists.txt | 12 + rcl_action/include/rcl_action/action_client.h | 13 +- rcl_action/src/rcl_action/action_client.c | 723 ++++++++++++++++++ 3 files changed, 740 insertions(+), 8 deletions(-) create mode 100644 rcl_action/src/rcl_action/action_client.c diff --git a/rcl_action/CMakeLists.txt b/rcl_action/CMakeLists.txt index 83c9931ce..1d4f17b2a 100644 --- a/rcl_action/CMakeLists.txt +++ b/rcl_action/CMakeLists.txt @@ -32,6 +32,7 @@ add_executable(test_compile_headers ) set(rcl_action_sources + src/${PROJECT_NAME}/action_client.c src/${PROJECT_NAME}/goal_handle.c src/${PROJECT_NAME}/goal_state_machine.c src/${PROJECT_NAME}/names.c @@ -72,6 +73,17 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() ament_find_gtest() # Gtests + # ament_add_gtest(test_action_client + # test/rcl_action/test_action_client.cpp + # ) + # if(TARGET test_action_client) + # target_include_directories(test_action_client PUBLIC + # ${rcl_INCLUDE_DIRS} + # ) + # target_link_libraries(test_action_client + # ${PROJECT_NAME} + # ) + # endif() ament_add_gtest(test_goal_handle test/rcl_action/test_goal_handle.cpp ) diff --git a/rcl_action/include/rcl_action/action_client.h b/rcl_action/include/rcl_action/action_client.h index 37ef690f5..c065603f0 100644 --- a/rcl_action/include/rcl_action/action_client.h +++ b/rcl_action/include/rcl_action/action_client.h @@ -176,7 +176,7 @@ RCL_WARN_UNUSED rcl_ret_t rcl_action_client_init( rcl_action_client_t * action_client, - const rcl_node_t * node, + rcl_node_t * node, const rosidl_action_type_support_t * type_support, const char * action_name, const rcl_action_client_options_t * options); @@ -355,9 +355,7 @@ rcl_action_take_goal_response( * Lock-Free | Yes * [1] only if required when filling the feedback message, avoided for fixed sizes * - * \param[in] action_client handle to the client that will take the goal response - * \param[out] goal_info pointer to a struct for meta-data about the goal associated - * with taken feedback + * \param[in] action_client handle to the client that will take action feedback * \param[out] ros_feedback pointer to the ROS feedback message. * \return `RCL_RET_OK` if the response was taken successfully, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or @@ -420,6 +418,7 @@ 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. @@ -661,7 +660,7 @@ RCL_WARN_UNUSED const rcl_action_client_options_t * rcl_action_client_get_options(const rcl_action_client_t * action_client); -/// Check that a rcl_action_clieint_t is valid. +/// Check that a rcl_action_client_t is valid. /** * The bool returned is `false` if `action_client` is invalid. * The bool returned is `true` otherwise. @@ -677,14 +676,12 @@ rcl_action_client_get_options(const rcl_action_client_t * action_client); * Lock-Free | Yes * * \param[in] action_client pointer to the rcl action client - * \param[in] error_msg_allocator a valid allocator or `NULL` * \return `true` if `action_client` is valid, otherwise `false` */ RCL_ACTION_PUBLIC bool rcl_action_client_is_valid( - const rcl_action_client_t * action_client, - rcl_allocator_t * error_msg_allocator); + const rcl_action_client_t * action_client); #ifdef __cplusplus } diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c new file mode 100644 index 000000000..7ec4816a7 --- /dev/null +++ b/rcl_action/src/rcl_action/action_client.c @@ -0,0 +1,723 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl_action/action_client.h" +#include "rcl_action/default_qos.h" +#include "rcl_action/names.h" +#include "rcl_action/types.h" + +#include "rcl/client.h" +#include "rcl/error_handling.h" +#include "rcl/subscription.h" +#include "rcl/types.h" + +#include "rcutils/logging_macros.h" +#include "rcutils/strdup.h" + +#include "rmw/qos_profiles.h" +#include "rmw/types.h" + + +typedef struct rcl_action_client_impl_t +{ + rcl_client_t goal_client; + rcl_client_t cancel_client; + rcl_client_t result_client; + rcl_subscription_t feedback_subscription; + rcl_subscription_t status_subscription; + rcl_action_client_options_t options; + char * action_name; +} rcl_action_client_impl_t; + +rcl_action_client_t +rcl_action_get_zero_initialized_client(void) +{ + static rcl_action_client_t null_action_client = {0}; + return null_action_client; +} + +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) { + ret = RCL_RET_ERROR; + } + return ret; + } + + *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) { + ret = RCL_RET_ACTION_NAME_INVALID; + } else if (RCL_RET_BAD_ALLOC != ret) { + ret = RCL_RET_ERROR; + } + } + return ret; +} + +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) { + ret = RCL_RET_ERROR; + } + return ret; + } + + *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) { + ret = RCL_RET_ACTION_NAME_INVALID; + } else if (RCL_RET_BAD_ALLOC != ret) { + ret = RCL_RET_ERROR; + } + } + return ret; +} + +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) { + ret = RCL_RET_ERROR; + } + return ret; + } + + *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) { + ret = RCL_RET_ACTION_NAME_INVALID; + } else if (RCL_RET_BAD_ALLOC != ret) { + ret = RCL_RET_ERROR; + } + } + return ret; +} + +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) { + ret = RCL_RET_ERROR; + } + return ret; + } + + *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_SERVICE_NAME_INVALID == ret) { + ret = RCL_RET_ACTION_NAME_INVALID; + } else if (RCL_RET_BAD_ALLOC != ret) { + ret = RCL_RET_ERROR; + } + } + return ret; +} + +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) { + ret = RCL_RET_ERROR; + } + return ret; + } + + *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_SERVICE_NAME_INVALID == ret) { + ret = RCL_RET_ACTION_NAME_INVALID; + } else if (RCL_RET_BAD_ALLOC != ret) { + ret = RCL_RET_ERROR; + } + } + return ret; +} + +rcl_ret_t +rcl_action_client_init( + rcl_action_client_t * action_client, + rcl_node_t * node, + const rosidl_action_type_support_t * type_support, + const char * action_name, + const rcl_action_client_options_t * options) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Initializing action client"); + 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_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(action_name, RCL_RET_INVALID_ARGUMENT); + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Initializing client for action name '%s'", action_name); + // Allocate space for the implementation struct. + rcl_action_client_impl_t *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); + + impl->action_name = rcutils_strdup(action_name, *((rcutils_allocator_t *)allocator)); + if (NULL == impl->action_name) { + allocator->deallocate(impl, allocator->state); + return RCL_RET_BAD_ALLOC; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Initializing action goal client"); + rcl_client_options_t goal_client_options = { + .qos = options->goal_service_qos, .allocator = *allocator + }; + rcl_ret_t ret, critical_ret; + 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, "Initializing action cancel 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, "Action cancel client initialized"); + rcl_client_options_t result_client_options = { + .qos = options->result_service_qos, .allocator = *allocator + }; + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Initializing action result client"); + 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, "Action cancel result initialized"); + rcl_subscription_options_t feedback_subscription_options = { + .qos = options->feedback_topic_qos, + .ignore_local_publications = false, + .allocator = *allocator + }; + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Initializing action feedback subscription"); + ret = rcl_action_feedback_subscription_init( + &impl->feedback_subscription, node, + &type_support->feedback_topic_type_support, + impl->action_name, &feedback_subscription_options); + if (RCL_RET_OK == ret) { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Action feedback subscription initialized"); + rcl_subscription_options_t status_subscription_options = { + .qos = options->status_topic_qos, + .ignore_local_publications = false, + .allocator = *allocator + }; + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Initializing action status subscription"); + ret = rcl_action_status_subscription_init( + &impl->status_subscription, node, + &type_support->status_topic_type_support, + impl->action_name, &status_subscription_options); + if (RCL_RET_OK == ret) { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Action status subscription initialized"); + action_client->impl->options = *options; + action_client->impl = impl; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client initialized"); + return RCL_RET_OK; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Finalizing action feedback subscription"); + critical_ret = rcl_subscription_fini(&impl->feedback_subscription, node); + if (RCL_RET_OK != critical_ret) { + RCL_SET_ERROR_MSG("cleanup after error failed, " + "system left inconsistent\n"); + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Action feedback subscription finalized"); + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Finalizing action result client"); + critical_ret = rcl_client_fini(&impl->result_client, node); + if (RCL_RET_OK != critical_ret) { + RCL_SET_ERROR_MSG("cleanup after error failed, " + "system left inconsistent\n"); + + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Action result client finalized"); + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Finalizing action cancel client"); + critical_ret = rcl_client_fini(&impl->cancel_client, node); + if (RCL_RET_OK != critical_ret) { + RCL_SET_ERROR_MSG("cleanup after error failed, " + "system left inconsistent\n"); + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Action cancel client finalized"); + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Finalizing action goal client"); + critical_ret = rcl_client_fini(&impl->goal_client, node); + if (RCL_RET_OK != critical_ret) { + RCL_SET_ERROR_MSG("cleanup after error failed, " + "system left inconsistent\n"); + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Action goal client finalized"); + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Failed to initialize action client"); + allocator->deallocate(impl->action_name, allocator->state); + allocator->deallocate(impl, allocator->state); + return ret; +} + +rcl_ret_t +rcl_action_client_fini(rcl_action_client_t * action_client, rcl_node_t * node) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing action client"); + if (!rcl_action_client_is_valid(action_client)) { + return RCL_RET_ACTION_CLIENT_INVALID; + } + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; + } + rcl_ret_t ret = RCL_RET_OK; + if (action_client->impl) { + // TODO(hidmic): ideally we should rollback to a valid state if any + // finalization failed, but it seems that's currently + // not possible. + rcl_action_client_impl_t * impl = action_client->impl; + if (rcl_client_is_valid(&impl->goal_client)) { + ret = rcl_client_fini(&impl->goal_client, node); + if (RCL_RET_OK != ret) { + return RCL_RET_ERROR; + } + } + if (rcl_client_is_valid(&impl->cancel_client)) { + ret = rcl_client_fini(&impl->cancel_client, node); + if (RCL_RET_OK != ret) { + return RCL_RET_ERROR; + } + } + if (rcl_client_is_valid(&impl->result_client)) { + ret = rcl_client_fini(&impl->result_client, node); + if (ret != RCL_RET_OK) { + return RCL_RET_ERROR; + } + } + if (rcl_subscription_is_valid(&impl->feedback_subscription)) { + ret = rcl_subscription_fini(&impl->feedback_subscription, node); + if (ret != RCL_RET_OK) { + return RCL_RET_ERROR; + } + } + if (rcl_subscription_is_valid(&impl->status_subscription)) { + ret = rcl_subscription_fini(&impl->status_subscription, node); + if (ret != RCL_RET_OK) { + return RCL_RET_ERROR; + } + } + rcl_allocator_t * allocator = &impl->options.allocator; + allocator->deallocate(action_client->impl, allocator->state); + action_client->impl = NULL; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client finalized"); + return RCL_RET_OK; +} + +rcl_action_client_options_t +rcl_action_client_get_default_options(void) +{ + static rcl_action_client_options_t default_options; + default_options.goal_service_qos = rmw_qos_profile_services_default; + default_options.cancel_service_qos = rmw_qos_profile_services_default; + default_options.result_service_qos = rmw_qos_profile_services_default; + default_options.feedback_topic_qos = rmw_qos_profile_default; + default_options.status_topic_qos = rcl_action_qos_profile_status_default; + default_options.allocator = rcl_get_default_allocator(); + return default_options; +} + +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; + } + 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; +} + +rcl_ret_t +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; + } + 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; + } + 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) { + ret = RCL_RET_ACTION_CLIENT_TAKE_FAILED; + } else if (RCL_RET_BAD_ALLOC != ret) { + ret = RCL_RET_ERROR; + } + return ret; + } + 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; + } + 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) { + ret = RCL_RET_ACTION_CLIENT_TAKE_FAILED; + } else if (RCL_RET_BAD_ALLOC != ret) { + ret = RCL_RET_ERROR; + } + return ret; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action status taken"); + return RCL_RET_OK; +} + +rcl_ret_t +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; + } + 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; +} + +rcl_ret_t +rcl_action_take_result_response( + const rcl_action_client_t * action_client, + void * ros_result) +{ + 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; + } + 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) { + ret = RCL_RET_ACTION_CLIENT_TAKE_FAILED; + } else if (RCL_RET_BAD_ALLOC != ret) { + ret = RCL_RET_ERROR; + } + return ret; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action result response taken"); + return RCL_RET_OK; +} + +rcl_ret_t +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; + } + 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; +} + +rcl_ret_t +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; + } + 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) { + ret = RCL_RET_ACTION_CLIENT_TAKE_FAILED; + } else if (RCL_RET_BAD_ALLOC != ret) { + ret = RCL_RET_ERROR; + } + return ret; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action cancel response taken"); + return RCL_RET_OK; +} + +const char * +rcl_action_client_get_action_name(const rcl_action_client_t * action_client) +{ + if (!rcl_action_client_is_valid(action_client)) { + return NULL; + } + return action_client->impl->action_name; +} + +const rcl_action_client_options_t * +rcl_action_client_get_options(const rcl_action_client_t * action_client) { + if (!rcl_action_client_is_valid(action_client)) { + return NULL; + } + return &action_client->impl->options; +} + +bool +rcl_action_client_is_valid(const rcl_action_client_t * action_client) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(action_client, false); + RCL_CHECK_FOR_NULL_WITH_MSG( + action_client->impl, + "action client's implementation is invalid", + return false); + return true; +} + +#ifdef __cplusplus +} +#endif From d1c3990f5d6fbfd424cd3f040d310551a83d81e4 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 5 Nov 2018 10:50:15 -0300 Subject: [PATCH 02/11] Addresses peer review comments (#319) --- rcl_action/src/rcl_action/action_client.c | 319 ++++++++++------------ 1 file changed, 151 insertions(+), 168 deletions(-) diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index 7ec4816a7..b36641cd2 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -52,6 +52,7 @@ 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, @@ -69,8 +70,7 @@ static rcl_ret_t rcl_action_goal_service_client_init( 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); + ret = rcl_action_get_goal_service_name(action_name, allocator, &goal_service_name); if (RCL_RET_OK != ret) { if (RCL_RET_BAD_ALLOC != ret) { ret = RCL_RET_ERROR; @@ -98,6 +98,7 @@ static rcl_ret_t rcl_action_goal_service_client_init( return ret; } +// \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, @@ -115,8 +116,7 @@ static rcl_ret_t rcl_action_cancel_service_client_init( 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); + ret = rcl_action_get_cancel_service_name(action_name, allocator, &cancel_service_name); if (RCL_RET_OK != ret) { if (RCL_RET_BAD_ALLOC != ret) { ret = RCL_RET_ERROR; @@ -144,6 +144,7 @@ static rcl_ret_t rcl_action_cancel_service_client_init( return ret; } +// \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, @@ -161,8 +162,7 @@ static rcl_ret_t rcl_action_result_client_init( 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); + ret = rcl_action_get_result_service_name(action_name, allocator, &result_service_name); if (RCL_RET_OK != ret) { if (RCL_RET_BAD_ALLOC != ret) { ret = RCL_RET_ERROR; @@ -190,6 +190,7 @@ static rcl_ret_t rcl_action_result_client_init( return ret; } +// \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, @@ -207,9 +208,7 @@ static rcl_ret_t rcl_action_feedback_subscription_init( 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); + ret = rcl_action_get_feedback_topic_name(action_name, allocator, &feedback_topic_name); if (RCL_RET_OK != ret) { if (RCL_RET_BAD_ALLOC != ret) { ret = RCL_RET_ERROR; @@ -228,7 +227,7 @@ static rcl_ret_t rcl_action_feedback_subscription_init( allocator.deallocate(feedback_topic_name, allocator.state); if (RCL_RET_OK != ret) { - if (RCL_RET_SERVICE_NAME_INVALID == ret) { + if (RCL_RET_TOPIC_NAME_INVALID == ret) { ret = RCL_RET_ACTION_NAME_INVALID; } else if (RCL_RET_BAD_ALLOC != ret) { ret = RCL_RET_ERROR; @@ -237,6 +236,7 @@ static rcl_ret_t rcl_action_feedback_subscription_init( return ret; } +// \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, @@ -253,8 +253,7 @@ static rcl_ret_t rcl_action_status_subscription_init( 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); + ret = rcl_action_get_status_topic_name(action_name, allocator, &status_topic_name); if (RCL_RET_OK != ret) { if (RCL_RET_BAD_ALLOC != ret) { ret = RCL_RET_ERROR; @@ -273,7 +272,7 @@ static rcl_ret_t rcl_action_status_subscription_init( allocator.deallocate(status_topic_name, allocator.state); if (RCL_RET_OK != ret) { - if (RCL_RET_SERVICE_NAME_INVALID == ret) { + if (RCL_RET_TOPIC_NAME_INVALID == ret) { ret = RCL_RET_ACTION_NAME_INVALID; } else if (RCL_RET_BAD_ALLOC != ret) { ret = RCL_RET_ERROR; @@ -290,11 +289,6 @@ rcl_action_client_init( const char * action_name, const rcl_action_client_options_t * options) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Initializing action client"); - 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_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); if (!rcl_node_is_valid(node)) { @@ -302,131 +296,107 @@ 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_ret_t ret = RCL_RET_OK; + rcl_ret_t fini_ret = RCL_RET_OK; RCUTILS_LOG_DEBUG_NAMED( ROS_PACKAGE_NAME, "Initializing client for action name '%s'", action_name); + if (NULL != action_client->impl) { + RCL_SET_ERROR_MSG("action client already initialized, or memory was uninitialized"); + return RCL_RET_INVALID_ARGUMENT; + } // Allocate space for the implementation struct. - rcl_action_client_impl_t *impl = allocator->allocate( + 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); - + // 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) { - allocator->deallocate(impl, allocator->state); - return RCL_RET_BAD_ALLOC; + ret = RCL_RET_BAD_ALLOC; + goto fail; } - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Initializing action goal client"); + // Initialize action goal service client. rcl_client_options_t goal_client_options = { .qos = options->goal_service_qos, .allocator = *allocator }; - rcl_ret_t ret, critical_ret; ret = rcl_action_goal_service_client_init( - &impl->goal_client, node, + &impl->goal_client, node, &type_support->goal_service_type_support, impl->action_name, &goal_client_options); - if (RCL_RET_OK == ret) { + if (RCL_RET_OK != ret) { RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Initializing action cancel 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, "Action cancel client initialized"); - rcl_client_options_t result_client_options = { - .qos = options->result_service_qos, .allocator = *allocator - }; - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Initializing action result client"); - 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, "Action cancel result initialized"); - rcl_subscription_options_t feedback_subscription_options = { - .qos = options->feedback_topic_qos, - .ignore_local_publications = false, - .allocator = *allocator - }; - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Initializing action feedback subscription"); - ret = rcl_action_feedback_subscription_init( - &impl->feedback_subscription, node, - &type_support->feedback_topic_type_support, - impl->action_name, &feedback_subscription_options); - if (RCL_RET_OK == ret) { - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Action feedback subscription initialized"); - rcl_subscription_options_t status_subscription_options = { - .qos = options->status_topic_qos, - .ignore_local_publications = false, - .allocator = *allocator - }; - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Initializing action status subscription"); - ret = rcl_action_status_subscription_init( - &impl->status_subscription, node, - &type_support->status_topic_type_support, - impl->action_name, &status_subscription_options); - if (RCL_RET_OK == ret) { - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Action status subscription initialized"); - action_client->impl->options = *options; - action_client->impl = impl; - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client initialized"); - return RCL_RET_OK; - } - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Finalizing action feedback subscription"); - critical_ret = rcl_subscription_fini(&impl->feedback_subscription, node); - if (RCL_RET_OK != critical_ret) { - RCL_SET_ERROR_MSG("cleanup after error failed, " - "system left inconsistent\n"); - } - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Action feedback subscription finalized"); - } - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Finalizing action result client"); - critical_ret = rcl_client_fini(&impl->result_client, node); - if (RCL_RET_OK != critical_ret) { - RCL_SET_ERROR_MSG("cleanup after error failed, " - "system left inconsistent\n"); - - } - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Action result client finalized"); - } - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Finalizing action cancel client"); - critical_ret = rcl_client_fini(&impl->cancel_client, node); - if (RCL_RET_OK != critical_ret) { - RCL_SET_ERROR_MSG("cleanup after error failed, " - "system left inconsistent\n"); - } - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Action cancel client finalized"); - } + 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, "Finalizing action goal client"); - critical_ret = rcl_client_fini(&impl->goal_client, node); - if (RCL_RET_OK != critical_ret) { - RCL_SET_ERROR_MSG("cleanup after error failed, " - "system left inconsistent\n"); - } + 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_topic_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_topic_type_support, + impl->action_name, &status_subscription_options); + if (RCL_RET_OK != ret) { RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Action goal client finalized"); + ROS_PACKAGE_NAME, "Failed to initialize status subscription"); + goto fail; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client initialized"); + return ret; +fail: + fini_ret = rcl_action_client_fini(action_client, node); + if (RCL_RET_OK != fini_ret) { + RCL_SET_ERROR_MSG("failed to cleanup action client"); + ret = RCL_RET_ERROR; } - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "Failed to initialize action client"); - allocator->deallocate(impl->action_name, allocator->state); - allocator->deallocate(impl, allocator->state); return ret; } @@ -434,54 +404,58 @@ rcl_ret_t rcl_action_client_fini(rcl_action_client_t * action_client, rcl_node_t * node) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing action client"); + RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { return RCL_RET_ACTION_CLIENT_INVALID; } if (!rcl_node_is_valid(node)) { return RCL_RET_NODE_INVALID; } + // TODO(hidmic): ideally we should rollback to a valid state if any + // finalization failed, but it seems that's currently + // not possible. rcl_ret_t ret = RCL_RET_OK; - if (action_client->impl) { - // TODO(hidmic): ideally we should rollback to a valid state if any - // finalization failed, but it seems that's currently - // not possible. - rcl_action_client_impl_t * impl = action_client->impl; - if (rcl_client_is_valid(&impl->goal_client)) { - ret = rcl_client_fini(&impl->goal_client, node); - if (RCL_RET_OK != ret) { - return RCL_RET_ERROR; - } + 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_client_is_valid(&impl->cancel_client)) { - ret = rcl_client_fini(&impl->cancel_client, node); - if (RCL_RET_OK != ret) { - return 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_client_is_valid(&impl->result_client)) { - ret = rcl_client_fini(&impl->result_client, node); - if (ret != RCL_RET_OK) { - return 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_subscription_is_valid(&impl->feedback_subscription)) { - ret = rcl_subscription_fini(&impl->feedback_subscription, node); - if (ret != RCL_RET_OK) { - return 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_subscription_is_valid(&impl->status_subscription)) { - ret = rcl_subscription_fini(&impl->status_subscription, node); - if (ret != RCL_RET_OK) { - return 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 != ret) { rcl_allocator_t * allocator = &impl->options.allocator; + allocator->deallocate(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"); } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client finalized"); - return RCL_RET_OK; + return ret; } rcl_action_client_options_t @@ -503,10 +477,11 @@ rcl_action_send_goal_request( const void * ros_goal_request) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action goal request"); + RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_goal_request, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { return RCL_RET_ACTION_CLIENT_INVALID; } - 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, @@ -524,10 +499,11 @@ rcl_action_take_goal_response( void * ros_goal_response) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action goal response"); + RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_goal_response, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { return RCL_RET_ACTION_CLIENT_INVALID; } - 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, @@ -550,10 +526,11 @@ rcl_action_take_feedback( void * ros_feedback) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action feedback"); + RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_feedback, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { return RCL_RET_ACTION_CLIENT_INVALID; } - 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, @@ -576,10 +553,11 @@ rcl_action_take_status( void * ros_status_array) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action status"); + RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_status_array, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { return RCL_RET_ACTION_CLIENT_INVALID; } - 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, @@ -602,10 +580,12 @@ rcl_action_send_result_request( const void * ros_result_request) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action result request"); + RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_result_request, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { return RCL_RET_ACTION_CLIENT_INVALID; } - 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, @@ -613,7 +593,7 @@ rcl_action_send_result_request( if (RCL_RET_OK != ret) { return RCL_RET_ERROR; } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action result request sent"); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action result request sent"); return RCL_RET_OK; } @@ -623,10 +603,11 @@ rcl_action_take_result_response( void * ros_result) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action result response"); + RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_result, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { return RCL_RET_ACTION_CLIENT_INVALID; } - 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, @@ -649,10 +630,11 @@ rcl_action_send_cancel_request( const void * ros_cancel_request) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action cancel request"); + RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_cancel_request, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { return RCL_RET_ACTION_CLIENT_INVALID; } - 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, @@ -660,7 +642,7 @@ rcl_action_send_cancel_request( if (RCL_RET_OK != ret) { return RCL_RET_ERROR; } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action cancel request sent"); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action cancel request sent"); return RCL_RET_OK; } @@ -670,10 +652,11 @@ rcl_action_take_cancel_response( void * ros_cancel_response) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action cancel response"); + RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_cancel_response, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { return RCL_RET_ACTION_CLIENT_INVALID; } - 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, @@ -686,7 +669,7 @@ rcl_action_take_cancel_response( } return ret; } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action cancel response taken"); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action cancel response taken"); return RCL_RET_OK; } From 89563623f72ebd584616cee54e25dfd687a712cc Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 6 Nov 2018 11:32:15 -0300 Subject: [PATCH 03/11] [rcl action] Refactors action client error checking. --- rcl_action/include/rcl_action/action_client.h | 1 + rcl_action/src/rcl_action/action_client.c | 172 +++++++++--------- 2 files changed, 88 insertions(+), 85 deletions(-) diff --git a/rcl_action/include/rcl_action/action_client.h b/rcl_action/include/rcl_action/action_client.h index c065603f0..c70638d44 100644 --- a/rcl_action/include/rcl_action/action_client.h +++ b/rcl_action/include/rcl_action/action_client.h @@ -167,6 +167,7 @@ rcl_action_get_zero_initialized_client(void); * \return `RCL_RET_OK` if action_client was initialized successfully, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_NODE_INVALID` if the node is invalid, or + * \return `RCL_RET_ALREADY_INIT` if the action client is already initialized, or * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or * \return `RCL_RET_ACTION_NAME_INVALID` if the given action name is invalid, or * \return `RCL_RET_ERROR` if an unspecified error occurs. diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index b36641cd2..41bd42d07 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -72,10 +72,10 @@ static rcl_ret_t rcl_action_goal_service_client_init( 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) { - ret = RCL_RET_ERROR; + if (RCL_RET_BAD_ALLOC == ret) { + return RCL_RET_BAD_ALLOC; } - return ret; + return RCL_RET_ERROR; } *goal_client = rcl_get_zero_initialized_client(); @@ -90,12 +90,14 @@ static rcl_ret_t rcl_action_goal_service_client_init( if (RCL_RET_OK != ret) { if (RCL_RET_SERVICE_NAME_INVALID == ret) { - ret = RCL_RET_ACTION_NAME_INVALID; - } else if (RCL_RET_BAD_ALLOC != ret) { - ret = RCL_RET_ERROR; + return RCL_RET_ACTION_NAME_INVALID; + } + if (RCL_RET_BAD_ALLOC == ret) { + return RCL_RET_BAD_ALLOC; } + return RCL_RET_ERROR; } - return ret; + return RCL_RET_OK; } // \internal Initialize client for the given action's goal cancel service. @@ -118,10 +120,10 @@ static rcl_ret_t rcl_action_cancel_service_client_init( 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) { - ret = RCL_RET_ERROR; + if (RCL_RET_BAD_ALLOC == ret) { + return RCL_RET_BAD_ALLOC; } - return ret; + return RCL_RET_ERROR; } *cancel_client = rcl_get_zero_initialized_client(); @@ -136,12 +138,14 @@ static rcl_ret_t rcl_action_cancel_service_client_init( if (RCL_RET_OK != ret) { if (RCL_RET_SERVICE_NAME_INVALID == ret) { - ret = RCL_RET_ACTION_NAME_INVALID; - } else if (RCL_RET_BAD_ALLOC != ret) { - ret = RCL_RET_ERROR; + return RCL_RET_ACTION_NAME_INVALID; } + if (RCL_RET_BAD_ALLOC == ret) { + return RCL_RET_BAD_ALLOC; + } + return RCL_RET_ERROR; } - return ret; + return RCL_RET_OK; } // \internal Initialize client for the given action's goal result service. @@ -164,10 +168,10 @@ static rcl_ret_t rcl_action_result_client_init( 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) { - ret = RCL_RET_ERROR; + if (RCL_RET_BAD_ALLOC == ret) { + return RCL_RET_BAD_ALLOC; } - return ret; + return RCL_RET_ERROR; } *result_client = rcl_get_zero_initialized_client(); @@ -182,12 +186,14 @@ static rcl_ret_t rcl_action_result_client_init( if (RCL_RET_OK != ret) { if (RCL_RET_SERVICE_NAME_INVALID == ret) { - ret = RCL_RET_ACTION_NAME_INVALID; - } else if (RCL_RET_BAD_ALLOC != ret) { - ret = RCL_RET_ERROR; + return RCL_RET_ACTION_NAME_INVALID; + } + if (RCL_RET_BAD_ALLOC == ret) { + return RCL_RET_BAD_ALLOC; } + return RCL_RET_ERROR; } - return ret; + return RCL_RET_OK; } // \internal Initialize subscription to the given action's feedback topic. @@ -210,10 +216,10 @@ static rcl_ret_t rcl_action_feedback_subscription_init( 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) { - ret = RCL_RET_ERROR; + if (RCL_RET_BAD_ALLOC == ret) { + return RCL_RET_BAD_ALLOC; } - return ret; + return RCL_RET_ERROR; } *feedback_subscription = rcl_get_zero_initialized_subscription(); @@ -228,12 +234,14 @@ static rcl_ret_t rcl_action_feedback_subscription_init( if (RCL_RET_OK != ret) { if (RCL_RET_TOPIC_NAME_INVALID == ret) { - ret = RCL_RET_ACTION_NAME_INVALID; - } else if (RCL_RET_BAD_ALLOC != ret) { - ret = RCL_RET_ERROR; + return RCL_RET_ACTION_NAME_INVALID; + } + if (RCL_RET_BAD_ALLOC == ret) { + return RCL_RET_BAD_ALLOC; } + return RCL_RET_ERROR; } - return ret; + return RCL_RET_OK; } // \internal Initialize subscription to the given action's status topic. @@ -255,10 +263,10 @@ static rcl_ret_t rcl_action_status_subscription_init( 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) { - ret = RCL_RET_ERROR; + if (RCL_RET_BAD_ALLOC == ret) { + return RCL_RET_BAD_ALLOC; } - return ret; + return RCL_RET_ERROR; } *status_subscription = rcl_get_zero_initialized_subscription(); @@ -273,12 +281,14 @@ static rcl_ret_t rcl_action_status_subscription_init( if (RCL_RET_OK != ret) { if (RCL_RET_TOPIC_NAME_INVALID == ret) { - ret = RCL_RET_ACTION_NAME_INVALID; - } else if (RCL_RET_BAD_ALLOC != ret) { - ret = RCL_RET_ERROR; + return RCL_RET_ACTION_NAME_INVALID; + } + if (RCL_RET_BAD_ALLOC == ret) { + return RCL_RET_BAD_ALLOC; } + return RCL_RET_ERROR; } - return ret; + return RCL_RET_OK; } rcl_ret_t @@ -307,7 +317,7 @@ rcl_action_client_init( ROS_PACKAGE_NAME, "Initializing client for action name '%s'", action_name); if (NULL != action_client->impl) { RCL_SET_ERROR_MSG("action client already initialized, or memory was uninitialized"); - return RCL_RET_INVALID_ARGUMENT; + return RCL_RET_ALREADY_INIT; } // Allocate space for the implementation struct. rcl_action_client_impl_t *impl = action_client->impl = allocator->allocate( @@ -404,13 +414,11 @@ rcl_ret_t rcl_action_client_fini(rcl_action_client_t * action_client, rcl_node_t * node) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing action client"); - RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; + return RCL_RET_ACTION_CLIENT_INVALID; // error already set } if (!rcl_node_is_valid(node)) { - return RCL_RET_NODE_INVALID; + return RCL_RET_NODE_INVALID; // error already set } // TODO(hidmic): ideally we should rollback to a valid state if any // finalization failed, but it seems that's currently @@ -477,11 +485,10 @@ rcl_action_send_goal_request( const void * ros_goal_request) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action goal request"); - RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(ros_goal_request, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; + 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, @@ -499,11 +506,10 @@ rcl_action_take_goal_response( void * ros_goal_response) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action goal response"); - RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(ros_goal_response, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; + 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, @@ -526,22 +532,22 @@ rcl_action_take_feedback( void * ros_feedback) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action feedback"); - RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(ros_feedback, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; + 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) { - ret = RCL_RET_ACTION_CLIENT_TAKE_FAILED; - } else if (RCL_RET_BAD_ALLOC != ret) { - ret = RCL_RET_ERROR; + return RCL_RET_ACTION_CLIENT_TAKE_FAILED; } - return ret; + 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; @@ -553,22 +559,22 @@ rcl_action_take_status( void * ros_status_array) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action status"); - RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(ros_status_array, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; + 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) { - ret = RCL_RET_ACTION_CLIENT_TAKE_FAILED; - } else if (RCL_RET_BAD_ALLOC != ret) { - ret = RCL_RET_ERROR; + return RCL_RET_ACTION_CLIENT_TAKE_FAILED; } - return ret; + 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; @@ -580,12 +586,10 @@ rcl_action_send_result_request( const void * ros_result_request) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action result request"); - RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(ros_result_request, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; + 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, @@ -603,22 +607,22 @@ rcl_action_take_result_response( void * ros_result) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action result response"); - RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(ros_result, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; + 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) { - ret = RCL_RET_ACTION_CLIENT_TAKE_FAILED; - } else if (RCL_RET_BAD_ALLOC != ret) { - ret = RCL_RET_ERROR; + return RCL_RET_ACTION_CLIENT_TAKE_FAILED; } - return ret; + 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; @@ -630,11 +634,10 @@ rcl_action_send_cancel_request( const void * ros_cancel_request) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action cancel request"); - RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(ros_cancel_request, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; + 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, @@ -652,22 +655,22 @@ rcl_action_take_cancel_response( void * ros_cancel_response) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action cancel response"); - RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(ros_cancel_response, RCL_RET_INVALID_ARGUMENT); if (!rcl_action_client_is_valid(action_client)) { - return RCL_RET_ACTION_CLIENT_INVALID; + 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) { - ret = RCL_RET_ACTION_CLIENT_TAKE_FAILED; - } else if (RCL_RET_BAD_ALLOC != ret) { - ret = RCL_RET_ERROR; + return RCL_RET_ACTION_CLIENT_TAKE_FAILED; } - return ret; + 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"); return RCL_RET_OK; @@ -693,11 +696,10 @@ rcl_action_client_get_options(const rcl_action_client_t * action_client) { bool rcl_action_client_is_valid(const rcl_action_client_t * action_client) { - RCL_CHECK_ARGUMENT_FOR_NULL(action_client, false); RCL_CHECK_FOR_NULL_WITH_MSG( - action_client->impl, - "action client's implementation is invalid", - return false); + 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); return true; } From e2ee3c5475c4e032d4908f3f9c6fe0820fb87347 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 6 Nov 2018 12:01:48 -0300 Subject: [PATCH 04/11] [rcl action] Wait set support for action clients (restrictive). --- rcl_action/include/rcl_action/wait.h | 8 +- rcl_action/src/rcl_action/action_client.c | 101 ++++++++++++++++++++++ 2 files changed, 106 insertions(+), 3 deletions(-) diff --git a/rcl_action/include/rcl_action/wait.h b/rcl_action/include/rcl_action/wait.h index 3ea8b55b5..37d13d2cf 100644 --- a/rcl_action/include/rcl_action/wait.h +++ b/rcl_action/include/rcl_action/wait.h @@ -27,13 +27,13 @@ extern "C" /// Add a rcl_action_client_t to a wait set. /** - * This function will add the underlying service clients and subscriber to the wait set. + * This function will add the underlying service clients and subscribers to the wait set. * * This function behaves similar to adding subscriptions to the wait set, but will add - * four elements: + * five elements: * * - Three service clients - * - One subscriber + * - Two subscribers * * \see rcl_wait_set_add_subscription * @@ -199,6 +199,8 @@ rcl_action_server_wait_set_get_num_entities( * \param[out] is_result_response_ready `true` if there is a result response message ready * to take, `false` otherwise * \return `RCL_RET_OK` if call is successful, or + * \return `RCL_RET_WAIT_SET_INVALID` if the wait set is zero initialized or not used + * for the action client alone, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_ACTION_CLIENT_INVALID` if the action client is invalid, or * \return `RCL_RET_ERROR` if an unspecified error occurs. diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index 41bd42d07..9f258ee48 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -21,11 +21,13 @@ extern "C" #include "rcl_action/default_qos.h" #include "rcl_action/names.h" #include "rcl_action/types.h" +#include "rcl_action/wait.h" #include "rcl/client.h" #include "rcl/error_handling.h" #include "rcl/subscription.h" #include "rcl/types.h" +#include "rcl/wait.h" #include "rcutils/logging_macros.h" #include "rcutils/strdup.h" @@ -703,6 +705,105 @@ rcl_action_client_is_valid(const rcl_action_client_t * action_client) return true; } +rcl_ret_t +rcl_action_wait_set_add_action_client( + rcl_wait_set_t * wait_set, + const rcl_action_client_t * action_client) { + rcl_ret_t ret; + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_WAIT_SET_INVALID); + if (!rcl_action_client_is_valid(action_client)) { + return RCL_RET_ACTION_CLIENT_INVALID; // error already set + } + // Wait on action goal service response messages. + ret = rcl_wait_set_add_client( + wait_set, &action_client->impl->goal_client); + if (RCL_RET_OK != ret) { + return ret; + } + // Wait on action cancel service response messages. + ret = rcl_wait_set_add_client( + wait_set, &action_client->impl->cancel_client); + if (RCL_RET_OK != ret) { + return ret; + } + // Wait on action result service response messages. + ret = rcl_wait_set_add_client( + wait_set, &action_client->impl->result_client); + if (RCL_RET_OK != ret) { + return ret; + } + // Wait on action feedback messages. + ret = rcl_wait_set_add_subscription( + wait_set, &action_client->impl->feedback_subscription); + if (RCL_RET_OK != ret) { + return ret; + } + return RCL_RET_OK; + // Wait on action status messages. + ret = rcl_wait_set_add_subscription( + wait_set, &action_client->impl->status_subscription); + if (RCL_RET_OK != ret) { + return ret; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_action_client_wait_set_get_num_entities( + const rcl_action_client_t * action_client, + size_t * num_subscriptions, + size_t * num_guard_conditions, + size_t * num_timers, + size_t * num_clients, + size_t * num_services) +{ + if (!rcl_action_client_is_valid(action_client)) { + return RCL_RET_ACTION_CLIENT_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(num_subscriptions, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(num_guard_conditions, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(num_timers, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(num_clients, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(num_services, RCL_RET_INVALID_ARGUMENT); + *num_subscriptions = 2; + *num_guard_conditions = 0; + *num_timers = 0; + *num_clients = 3; + *num_services = 0; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_action_client_wait_set_get_entities_ready( + const rcl_wait_set_t * wait_set, + const rcl_action_client_t * action_client, + bool * is_feedback_ready, + bool * is_status_ready, + bool * is_goal_response_ready, + bool * is_cancel_response_ready, + bool * is_result_response_ready) +{ + if (!rcl_action_client_is_valid(action_client)) { + return RCL_RET_ACTION_CLIENT_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(is_feedback_ready, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(is_status_ready, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(is_goal_response_ready, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(is_cancel_response_ready, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(is_result_response_ready, RCL_RET_INVALID_ARGUMENT); + if (2 != wait_set->size_of_subscriptions || 3 != wait_set->size_of_clients) { + RCL_SET_ERROR_MSG("wait set not initialized or not used by the action client alone"); + return RCL_RET_WAIT_SET_INVALID; + } + *is_feedback_ready = !!wait_set->subscriptions[0]; + *is_status_ready = !!wait_set->subscriptions[1]; + *is_goal_response_ready = !!wait_set->clients[0]; + *is_cancel_response_ready = !!wait_set->clients[1]; + *is_result_response_ready = !!wait_set->clients[2]; + return RCL_RET_OK; +} + + #ifdef __cplusplus } #endif From cca18daad26bd14c28f327ea99e86cc6ae856ad3 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 7 Nov 2018 17:48:32 -0300 Subject: [PATCH 05/11] [rcl action] Fixes after changes introduced by (#310) --- rcl_action/include/rcl_action/action_client.h | 4 ---- rcl_action/include/rcl_action/action_server.h | 4 ---- rcl_action/include/rcl_action/types.h | 2 ++ rcl_action/src/rcl_action/action_client.c | 10 +++++----- 4 files changed, 7 insertions(+), 13 deletions(-) diff --git a/rcl_action/include/rcl_action/action_client.h b/rcl_action/include/rcl_action/action_client.h index c70638d44..f90065d31 100644 --- a/rcl_action/include/rcl_action/action_client.h +++ b/rcl_action/include/rcl_action/action_client.h @@ -20,10 +20,6 @@ extern "C" { #endif -// TODO(jacobperron): replace type support typedef with one defined in rosdl_generator_c -// #include "rosidl_generator_c/action_type_support_struct.h" -typedef struct rosidl_action_type_support_t rosidl_action_type_support_t; - #include "rcl_action/types.h" #include "rcl_action/visibility_control.h" #include "rcl/macros.h" diff --git a/rcl_action/include/rcl_action/action_server.h b/rcl_action/include/rcl_action/action_server.h index 80c14e1ad..4195b61f2 100644 --- a/rcl_action/include/rcl_action/action_server.h +++ b/rcl_action/include/rcl_action/action_server.h @@ -20,10 +20,6 @@ extern "C" { #endif -// TODO(jacobperron): replace type support typedef with one defined in rosdl_generator_c -// #include "rosidl_generator_c/action_type_support_struct.h" -typedef struct rosidl_action_type_support_t rosidl_action_type_support_t; - #include "rcl_action/goal_handle.h" #include "rcl_action/types.h" #include "rcl_action/visibility_control.h" diff --git a/rcl_action/include/rcl_action/types.h b/rcl_action/include/rcl_action/types.h index f9a86ad5f..52a379a5e 100644 --- a/rcl_action/include/rcl_action/types.h +++ b/rcl_action/include/rcl_action/types.h @@ -31,6 +31,8 @@ extern "C" #include "rcl/macros.h" #include "rcl/types.h" +#include "rosidl_generator_c/action_type_support_struct.h" +typedef struct rosidl_action_type_support_t rosidl_action_type_support_t; // rcl action specific ret codes in 2XXX /// Action name does not pass validation return code. diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index 9f258ee48..4734788e2 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -338,7 +338,7 @@ rcl_action_client_init( }; ret = rcl_action_goal_service_client_init( &impl->goal_client, node, - &type_support->goal_service_type_support, + type_support->goal_service_type_support, impl->action_name, &goal_client_options); if (RCL_RET_OK != ret) { RCUTILS_LOG_DEBUG_NAMED( @@ -351,7 +351,7 @@ rcl_action_client_init( }; ret = rcl_action_cancel_service_client_init( &impl->cancel_client, node, - &type_support->cancel_service_type_support, + type_support->cancel_service_type_support, impl->action_name, &cancel_client_options); if (RCL_RET_OK != ret) { RCUTILS_LOG_DEBUG_NAMED( @@ -364,7 +364,7 @@ rcl_action_client_init( }; ret = rcl_action_result_client_init( &impl->result_client, node, - &type_support->result_service_type_support, + type_support->result_service_type_support, impl->action_name, &result_client_options); if (RCL_RET_OK != ret) { RCUTILS_LOG_DEBUG_NAMED( @@ -379,7 +379,7 @@ rcl_action_client_init( }; ret = rcl_action_feedback_subscription_init( &impl->feedback_subscription, node, - &type_support->feedback_topic_type_support, + type_support->feedback_message_type_support, impl->action_name, &feedback_subscription_options); if (RCL_RET_OK != ret) { RCUTILS_LOG_DEBUG_NAMED( @@ -394,7 +394,7 @@ rcl_action_client_init( }; ret = rcl_action_status_subscription_init( &impl->status_subscription, node, - &type_support->status_topic_type_support, + type_support->status_message_type_support, impl->action_name, &status_subscription_options); if (RCL_RET_OK != ret) { RCUTILS_LOG_DEBUG_NAMED( From 7efd1a15ae349a2054b0f36158c307eb407002e2 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 12 Nov 2018 13:58:28 -0300 Subject: [PATCH 06/11] [rcl action] Adds action client tests. --- rcl_action/CMakeLists.txt | 25 +- rcl_action/package.xml | 1 + rcl_action/src/rcl_action/action_client.c | 11 +- .../test/rcl_action/test_action_client.cpp | 213 ++++++++++++++++++ 4 files changed, 235 insertions(+), 15 deletions(-) create mode 100644 rcl_action/test/rcl_action/test_action_client.cpp diff --git a/rcl_action/CMakeLists.txt b/rcl_action/CMakeLists.txt index 1d4f17b2a..820eac25f 100644 --- a/rcl_action/CMakeLists.txt +++ b/rcl_action/CMakeLists.txt @@ -70,20 +70,23 @@ install(TARGETS ${PROJECT_NAME} if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) + find_package(test_msgs REQUIRED) ament_lint_auto_find_test_dependencies() ament_find_gtest() # Gtests - # ament_add_gtest(test_action_client - # test/rcl_action/test_action_client.cpp - # ) - # if(TARGET test_action_client) - # target_include_directories(test_action_client PUBLIC - # ${rcl_INCLUDE_DIRS} - # ) - # target_link_libraries(test_action_client - # ${PROJECT_NAME} - # ) - # endif() + ament_add_gtest(test_action_client + test/rcl_action/test_action_client.cpp + ) + if(TARGET test_action_client) + target_include_directories(test_action_client PUBLIC + ${rcl_INCLUDE_DIRS} + ${test_msgs_INCLUDE_DIRS} + ) + target_link_libraries(test_action_client + ${PROJECT_NAME} + ${test_msgs_LIBRARIES} + ) + endif() ament_add_gtest(test_goal_handle test/rcl_action/test_goal_handle.cpp ) diff --git a/rcl_action/package.xml b/rcl_action/package.xml index c5eab74f5..da9d64c59 100644 --- a/rcl_action/package.xml +++ b/rcl_action/package.xml @@ -18,6 +18,7 @@ ament_cmake_gtest ament_lint_common ament_lint_auto + test_msgs ament_cmake diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index 4734788e2..2b2ac1522 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -18,6 +18,7 @@ extern "C" #endif #include "rcl_action/action_client.h" + #include "rcl_action/default_qos.h" #include "rcl_action/names.h" #include "rcl_action/types.h" @@ -322,7 +323,7 @@ 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( + 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); // Copy action client name and options. @@ -688,7 +689,8 @@ rcl_action_client_get_action_name(const rcl_action_client_t * action_client) } const rcl_action_client_options_t * -rcl_action_client_get_options(const rcl_action_client_t * action_client) { +rcl_action_client_get_options(const rcl_action_client_t * action_client) +{ if (!rcl_action_client_is_valid(action_client)) { return NULL; } @@ -699,7 +701,7 @@ bool rcl_action_client_is_valid(const rcl_action_client_t * action_client) { RCL_CHECK_FOR_NULL_WITH_MSG( - action_client,"action client pointer is invalid", return false); + 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); return true; @@ -708,7 +710,8 @@ rcl_action_client_is_valid(const rcl_action_client_t * action_client) rcl_ret_t rcl_action_wait_set_add_action_client( rcl_wait_set_t * wait_set, - const rcl_action_client_t * action_client) { + const rcl_action_client_t * action_client) +{ rcl_ret_t ret; RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_WAIT_SET_INVALID); if (!rcl_action_client_is_valid(action_client)) { diff --git a/rcl_action/test/rcl_action/test_action_client.cpp b/rcl_action/test/rcl_action/test_action_client.cpp new file mode 100644 index 000000000..b10ad2b39 --- /dev/null +++ b/rcl_action/test/rcl_action/test_action_client.cpp @@ -0,0 +1,213 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl_action/action_client.h" + +#include "rcl/error_handling.h" +#include "rcl/rcl.h" + +#include "test_msgs/action/fibonacci.h" + +class TestActionClientBaseFixture : public ::testing::Test +{ +protected: + void SetUp() override + { + rcl_ret_t ret = rcl_init(0, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + this->node = rcl_get_zero_initialized_node(); + rcl_node_options_t node_options = rcl_node_get_default_options(); + const char * node_name = "test_action_client_node"; + ret = rcl_node_init(&this->node, node_name, "", &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() override + { + rcl_ret_t ret = rcl_node_fini(&this->node); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + rcl_node_t node; +}; + + +TEST_F(TestActionClientBaseFixture, test_action_client_init_fini) { + rcl_ret_t ret = RCL_RET_OK; + rcl_action_client_t invalid_action_client = + rcl_action_get_zero_initialized_client(); + rcl_node_t invalid_node = rcl_get_zero_initialized_node(); + const char * action_name = "test_action_client_name"; + const rosidl_action_type_support_t * action_typesupport = + ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci); + const rcl_action_client_options_t action_client_options = + rcl_action_client_get_default_options(); + rcl_action_client_options_t invalid_action_client_options = + rcl_action_client_get_default_options(); + invalid_action_client_options.allocator = + (rcl_allocator_t)rcutils_get_zero_initialized_allocator(); + rcl_action_client_t action_client = rcl_action_get_zero_initialized_client(); + + ret = rcl_action_client_init( + nullptr, &this->node, action_typesupport, + action_name, &action_client_options); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_client_init( + &action_client, nullptr, action_typesupport, + action_name, &action_client_options); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_client_init( + &action_client, &invalid_node, action_typesupport, + action_name, &action_client_options); + EXPECT_EQ(ret, RCL_RET_NODE_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_client_init( + &action_client, &this->node, nullptr, + action_name, &action_client_options); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_client_init( + &action_client, &this->node, action_typesupport, + nullptr, &action_client_options); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_client_init( + &action_client, &this->node, + action_typesupport, action_name, + nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_client_init( + &action_client, &this->node, + action_typesupport, action_name, + &invalid_action_client_options); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_client_init( + &action_client, &this->node, action_typesupport, + action_name, &action_client_options); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_client_init( + &action_client, &this->node, action_typesupport, + action_name, &action_client_options); + EXPECT_EQ(ret, RCL_RET_ALREADY_INIT) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_client_fini(nullptr, &this->node); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_client_fini(&invalid_action_client, &this->node); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_client_fini(&action_client, nullptr); + EXPECT_EQ(ret, RCL_RET_NODE_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_client_fini(&action_client, &invalid_node); + EXPECT_EQ(ret, RCL_RET_NODE_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_client_fini(&action_client, &this->node); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); +} + +class TestActionClientFixture : public TestActionClientBaseFixture +{ +protected: + void SetUp() override + { + TestActionClientBaseFixture::SetUp(); + this->action_client = rcl_action_get_zero_initialized_client(); + const rosidl_action_type_support_t * action_typesupport = + ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci); + this->action_client_options = rcl_action_client_get_default_options(); + rcl_ret_t ret = rcl_action_client_init( + &this->action_client, &this->node, action_typesupport, + this->action_name, &this->action_client_options); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + this->invalid_action_client = rcl_action_get_zero_initialized_client(); + } + + void TearDown() override + { + rcl_ret_t ret = rcl_action_client_fini(&this->action_client, &this->node); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + TestActionClientBaseFixture::TearDown(); + } + + const char * const action_name = "test_action_client_name"; + rcl_action_client_options_t action_client_options; + rcl_action_client_t invalid_action_client; + rcl_action_client_t action_client; +}; + +TEST_F(TestActionClientFixture, test_action_client_is_valid) { + bool is_valid = rcl_action_client_is_valid(nullptr); + EXPECT_FALSE(is_valid) << rcl_get_error_string().str; + rcl_reset_error(); + + is_valid = rcl_action_client_is_valid(&this->invalid_action_client); + EXPECT_FALSE(is_valid) << rcl_get_error_string().str; + rcl_reset_error(); + + is_valid = rcl_action_client_is_valid(&this->action_client); + EXPECT_TRUE(is_valid) << rcl_get_error_string().str; + rcl_reset_error(); +} + +TEST_F(TestActionClientFixture, test_action_client_get_action_name) { + const char * name = rcl_action_client_get_action_name(nullptr); + EXPECT_EQ(name, nullptr) << rcl_get_error_string().str; + rcl_reset_error(); + + name = rcl_action_client_get_action_name(&this->invalid_action_client); + EXPECT_EQ(name, nullptr) << rcl_get_error_string().str; + rcl_reset_error(); + + name = rcl_action_client_get_action_name(&this->action_client); + ASSERT_NE(name, nullptr) << rcl_get_error_string().str; + EXPECT_STREQ(name, this->action_name); +} + +TEST_F(TestActionClientFixture, test_action_client_get_options) { + const rcl_action_client_options_t * options = + rcl_action_client_get_options(nullptr); + EXPECT_EQ(options, nullptr) << rcl_get_error_string().str; + rcl_reset_error(); + + options = rcl_action_client_get_options(&this->invalid_action_client); + EXPECT_EQ(options, nullptr) << rcl_get_error_string().str; + rcl_reset_error(); + + options = rcl_action_client_get_options(&this->action_client); + ASSERT_NE(options, nullptr) << rcl_get_error_string().str; +} From 0c313821659b90f03650dfcefcbff453acba82f6 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 13 Nov 2018 10:50:39 -0300 Subject: [PATCH 07/11] [rcl action] Addresses peer review comments (#319). * Remove redundant null check for given rcl_node_t pointer. * Always deallocate rcl_action_client_t pimpl on finalization. * Minor formatting fixes. --- rcl_action/src/rcl_action/action_client.c | 31 ++++++++--------------- 1 file changed, 10 insertions(+), 21 deletions(-) diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index 2b2ac1522..23fb9cc29 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -303,7 +303,6 @@ rcl_action_client_init( const rcl_action_client_options_t * options) { RCL_CHECK_ARGUMENT_FOR_NULL(action_client, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); if (!rcl_node_is_valid(node)) { return RCL_RET_NODE_INVALID; } @@ -423,9 +422,6 @@ rcl_action_client_fini(rcl_action_client_t * action_client, rcl_node_t * node) if (!rcl_node_is_valid(node)) { return RCL_RET_NODE_INVALID; // error already set } - // TODO(hidmic): ideally we should rollback to a valid state if any - // finalization failed, but it seems that's currently - // not possible. rcl_ret_t ret = RCL_RET_OK; rcl_ret_t fini_ret = RCL_RET_OK; rcl_action_client_impl_t * impl = action_client->impl; @@ -459,13 +455,11 @@ rcl_action_client_fini(rcl_action_client_t * action_client, rcl_node_t * node) ret = RCL_RET_ERROR; } } - if (RCL_RET_OK != ret) { - rcl_allocator_t * allocator = &impl->options.allocator; - allocator->deallocate(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"); - } + rcl_allocator_t * allocator = &impl->options.allocator; + allocator->deallocate(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"); return ret; } @@ -718,33 +712,28 @@ rcl_action_wait_set_add_action_client( return RCL_RET_ACTION_CLIENT_INVALID; // error already set } // Wait on action goal service response messages. - ret = rcl_wait_set_add_client( - wait_set, &action_client->impl->goal_client); + ret = rcl_wait_set_add_client(wait_set, &action_client->impl->goal_client); if (RCL_RET_OK != ret) { return ret; } // Wait on action cancel service response messages. - ret = rcl_wait_set_add_client( - wait_set, &action_client->impl->cancel_client); + ret = rcl_wait_set_add_client(wait_set, &action_client->impl->cancel_client); if (RCL_RET_OK != ret) { return ret; } // Wait on action result service response messages. - ret = rcl_wait_set_add_client( - wait_set, &action_client->impl->result_client); + ret = rcl_wait_set_add_client(wait_set, &action_client->impl->result_client); if (RCL_RET_OK != ret) { return ret; } // Wait on action feedback messages. - ret = rcl_wait_set_add_subscription( - wait_set, &action_client->impl->feedback_subscription); + ret = rcl_wait_set_add_subscription(wait_set, &action_client->impl->feedback_subscription); if (RCL_RET_OK != ret) { return ret; } return RCL_RET_OK; // Wait on action status messages. - ret = rcl_wait_set_add_subscription( - wait_set, &action_client->impl->status_subscription); + ret = rcl_wait_set_add_subscription(wait_set, &action_client->impl->status_subscription); if (RCL_RET_OK != ret) { return ret; } From f2afddc91031edbee352a712c1017bff0af6547e Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 13 Nov 2018 12:03:05 -0300 Subject: [PATCH 08/11] [rcl action] Cleanup CMake code for test_action_client. --- rcl_action/CMakeLists.txt | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rcl_action/CMakeLists.txt b/rcl_action/CMakeLists.txt index 820eac25f..79c997bf5 100644 --- a/rcl_action/CMakeLists.txt +++ b/rcl_action/CMakeLists.txt @@ -79,13 +79,12 @@ if(BUILD_TESTING) ) if(TARGET test_action_client) target_include_directories(test_action_client PUBLIC - ${rcl_INCLUDE_DIRS} - ${test_msgs_INCLUDE_DIRS} + include ) target_link_libraries(test_action_client ${PROJECT_NAME} - ${test_msgs_LIBRARIES} ) + ament_target_dependencies(test_action_client "rcl" "test_msgs") endif() ament_add_gtest(test_goal_handle test/rcl_action/test_goal_handle.cpp From 629403e29d7bf30232201a1998e2e82b2b62b2a5 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 13 Nov 2018 14:12:00 -0300 Subject: [PATCH 09/11] [rcl action] Fixes bad assertion in test_action_client. --- rcl_action/test/rcl_action/test_action_client.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl_action/test/rcl_action/test_action_client.cpp b/rcl_action/test/rcl_action/test_action_client.cpp index b10ad2b39..eb2508d54 100644 --- a/rcl_action/test/rcl_action/test_action_client.cpp +++ b/rcl_action/test/rcl_action/test_action_client.cpp @@ -72,7 +72,7 @@ TEST_F(TestActionClientBaseFixture, test_action_client_init_fini) { ret = rcl_action_client_init( &action_client, nullptr, action_typesupport, action_name, &action_client_options); - EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + EXPECT_EQ(ret, RCL_RET_NODE_INVALID) << rcl_get_error_string().str; rcl_reset_error(); ret = rcl_action_client_init( From 78168bb52afa82b2b16db36108fd8de254b74c01 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 14 Nov 2018 15:54:32 -0300 Subject: [PATCH 10/11] [rcl action] Removes duplicate typedef in action_client header. --- rcl_action/include/rcl_action/types.h | 1 - 1 file changed, 1 deletion(-) diff --git a/rcl_action/include/rcl_action/types.h b/rcl_action/include/rcl_action/types.h index 52a379a5e..cb686db00 100644 --- a/rcl_action/include/rcl_action/types.h +++ b/rcl_action/include/rcl_action/types.h @@ -32,7 +32,6 @@ extern "C" #include "rcl/types.h" #include "rosidl_generator_c/action_type_support_struct.h" -typedef struct rosidl_action_type_support_t rosidl_action_type_support_t; // rcl action specific ret codes in 2XXX /// Action name does not pass validation return code. From c7e61834b4ac29d1d087b773c9cc976b0f996f95 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 14 Nov 2018 16:56:05 -0300 Subject: [PATCH 11/11] [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 f90065d31..7b680d7b7 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 23fb9cc29..48d4049ae 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; - } - - *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 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; \ + } + +// \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,175 +285,77 @@ 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; + TAKE_SERVICE_RESPONSE(goal); } rcl_ret_t -rcl_action_take_feedback( +rcl_action_send_result_request( const rcl_action_client_t * action_client, - void * ros_feedback) + const void * ros_result_request) { - 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; + SEND_SERVICE_REQUEST(result); } rcl_ret_t -rcl_action_take_status( +rcl_action_take_result_response( const rcl_action_client_t * action_client, - void * ros_status_array) + void * ros_result_response) { - 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(result); } rcl_ret_t -rcl_action_send_result_request( +rcl_action_send_cancel_request( const rcl_action_client_t * action_client, - const void * ros_result_request) + const void * ros_cancel_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(cancel); } rcl_ret_t -rcl_action_take_result_response( +rcl_action_take_cancel_response( const rcl_action_client_t * action_client, - void * ros_result) + void * ros_cancel_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(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_send_cancel_request( +rcl_action_take_feedback( const rcl_action_client_t * action_client, - const void * ros_cancel_request) + void * ros_feedback) { - 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; + TAKE_MESSAGE(feedback); } rcl_ret_t -rcl_action_take_cancel_response( +rcl_action_take_status( const rcl_action_client_t * action_client, - void * ros_cancel_response) + void * ros_status) { - 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"); - return RCL_RET_OK; + 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; }