diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index e9e0b4c50..273c8315d 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -53,6 +53,7 @@ set(${PROJECT_NAME}_sources src/rcl/node_options.c src/rcl/publisher.c src/rcl/remap.c + src/rcl/node_resolve_name.c src/rcl/rmw_implementation_identifier_check.c src/rcl/security.c src/rcl/service.c diff --git a/rcl/include/rcl/expand_topic_name.h b/rcl/include/rcl/expand_topic_name.h index bfcc288de..4816cbfbe 100644 --- a/rcl/include/rcl/expand_topic_name.h +++ b/rcl/include/rcl/expand_topic_name.h @@ -29,7 +29,7 @@ extern "C" /// Expand a given topic name into a fully-qualified topic name. /** * The input_topic_name, node_name, and node_namespace arguments must all be - * vaid, null terminated c strings. + * valid, null terminated c strings. * The output_topic_name will not be assigned a value in the event of an error. * * The output_topic_name will be null terminated. diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h index 900edb50e..011347fd3 100644 --- a/rcl/include/rcl/node.h +++ b/rcl/include/rcl/node.h @@ -490,6 +490,47 @@ RCL_WARN_UNUSED const char * rcl_node_get_logger_name(const rcl_node_t * node); +/// Expand a given name into a fully-qualified topic name and apply remapping rules. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node Node object. Its name, namespace, local/global command line arguments are used. + * \param[in] input_name Topic name to be expanded and remapped. + * \param[in] allocator The allocator to be used when creating the output topic. + * \param[in] is_service For services use `true`, for topics use `false`. + * \param[in] only_expand When `true`, remapping rules are ignored. + * \param[out] output_name Output char * pointer. + * \return `RCL_RET_OK` if the topic name was expanded successfully, or + * \return `RCL_RET_INVALID_ARGUMENT` if any of input_name, node_name, node_namespace + * or output_name are NULL, or + * \return `RCL_RET_INVALID_ARGUMENT` if both local_args and global_args are NULL, or + * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or + * \return `RCL_RET_TOPIC_NAME_INVALID` if the given topic name is invalid + * (see \ref rcl_validate_topic_name()), or + * \return `RCL_RET_NODE_INVALID_NAME` if the given node name is invalid + * (see \ref rmw_validate_node_name()), or + * \return `RCL_RET_NODE_INVALID_NAMESPACE` if the given node namespace is invalid + * (see \ref rmw_validate_namespace()), or + * \return `RCL_RET_UNKNOWN_SUBSTITUTION` for unknown substitutions in name, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_node_resolve_name( + const rcl_node_t * node, + const char * input_name, + rcl_allocator_t allocator, + bool is_service, + bool only_expand, + char ** output_name); + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 875236390..53dc77ca1 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -23,14 +23,12 @@ extern "C" #include #include "rcl/error_handling.h" -#include "rcl/expand_topic_name.h" -#include "rcl/remap.h" +#include "rcl/node.h" #include "rcutils/logging_macros.h" #include "rcutils/macros.h" #include "rcutils/stdatomic_helper.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "rmw/validate_full_topic_name.h" #include "tracetools/tracetools.h" #include "./common.h" @@ -75,89 +73,27 @@ rcl_client_init( RCL_SET_ERROR_MSG("client already initialized, or memory was unintialized"); return RCL_RET_ALREADY_INIT; } + // Expand the given service name. - rcutils_allocator_t rcutils_allocator = *allocator; // implicit conversion to rcutils version - rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map(); - rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions_map, 0, rcutils_allocator); - if (rcutils_ret != RCUTILS_RET_OK) { - RCL_SET_ERROR_MSG(rcutils_get_error_string().str); - if (rcutils_ret == RCUTILS_RET_BAD_ALLOC) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map); - if (ret != RCL_RET_OK) { - rcutils_ret = rcutils_string_map_fini(&substitutions_map); - if (rcutils_ret != RCUTILS_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, - "failed to fini string_map (%d) during error handling: %s\n", - rcutils_ret, - rcutils_get_error_string().str); - } - if (ret == RCL_RET_BAD_ALLOC) { - return ret; - } - return RCL_RET_ERROR; - } - char * expanded_service_name = NULL; char * remapped_service_name = NULL; - ret = rcl_expand_topic_name( + rcl_ret_t ret = rcl_node_resolve_name( + node, service_name, - rcl_node_get_name(node), - rcl_node_get_namespace(node), - &substitutions_map, *allocator, - &expanded_service_name); - rcutils_ret = rcutils_string_map_fini(&substitutions_map); - if (rcutils_ret != RCUTILS_RET_OK) { - RCL_SET_ERROR_MSG(rcutils_get_error_string().str); - allocator->deallocate(expanded_service_name, allocator->state); - return RCL_RET_ERROR; - } + true, + false, + &remapped_service_name); if (ret != RCL_RET_OK) { - if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { + if (ret == RCL_RET_SERVICE_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { ret = RCL_RET_SERVICE_NAME_INVALID; - } else { + } else if (RCL_RET_BAD_ALLOC != ret) { ret = RCL_RET_ERROR; } goto cleanup; } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded service name '%s'", expanded_service_name); - - const rcl_node_options_t * node_options = rcl_node_get_options(node); - if (NULL == node_options) { - ret = RCL_RET_ERROR; - goto cleanup; - } - rcl_arguments_t * global_args = NULL; - if (node_options->use_global_arguments) { - global_args = &(node->context->global_arguments); - } - ret = rcl_remap_service_name( - &(node_options->arguments), global_args, expanded_service_name, - rcl_node_get_name(node), rcl_node_get_namespace(node), *allocator, &remapped_service_name); - if (RCL_RET_OK != ret) { - goto fail; - } else if (NULL == remapped_service_name) { - remapped_service_name = expanded_service_name; - expanded_service_name = NULL; - } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Expanded and remapped service name '%s'", remapped_service_name); - // Validate the expanded service name. - int validation_result; - rmw_ret_t rmw_ret = rmw_validate_full_topic_name(remapped_service_name, &validation_result, NULL); - if (rmw_ret != RMW_RET_OK) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - ret = RCL_RET_ERROR; - goto cleanup; - } - if (validation_result != RMW_TOPIC_VALID) { - RCL_SET_ERROR_MSG(rmw_full_topic_name_validation_result_string(validation_result)); - ret = RCL_RET_SERVICE_NAME_INVALID; - goto cleanup; - } // Allocate space for the implementation struct. client->impl = (rcl_client_impl_t *)allocator->allocate( sizeof(rcl_client_impl_t), allocator->state); @@ -195,12 +131,7 @@ rcl_client_init( ret = fail_ret; // Fall through to cleanup cleanup: - if (NULL != expanded_service_name) { - allocator->deallocate(expanded_service_name, allocator->state); - } - if (NULL != remapped_service_name) { - allocator->deallocate(remapped_service_name, allocator->state); - } + allocator->deallocate(remapped_service_name, allocator->state); return ret; } diff --git a/rcl/src/rcl/node_resolve_name.c b/rcl/src/rcl/node_resolve_name.c new file mode 100644 index 000000000..9c2667669 --- /dev/null +++ b/rcl/src/rcl/node_resolve_name.c @@ -0,0 +1,162 @@ +// Copyright 2020 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 "rcl/node.h" + +#include "rcutils/error_handling.h" +#include "rcutils/logging_macros.h" +#include "rcutils/types/string_map.h" + +#include "rmw/error_handling.h" +#include "rmw/validate_full_topic_name.h" + +#include "rcl/error_handling.h" +#include "rcl/expand_topic_name.h" +#include "rcl/remap.h" + +#include "./remap_impl.h" + +static +rcl_ret_t +rcl_resolve_name( + const rcl_arguments_t * local_args, + const rcl_arguments_t * global_args, + const char * input_topic_name, + const char * node_name, + const char * node_namespace, + rcl_allocator_t allocator, + bool is_service, + bool only_expand, + char ** output_topic_name) +{ + // the other arguments are checked by rcl_expand_topic_name() and rcl_remap_name() + RCL_CHECK_ARGUMENT_FOR_NULL(output_topic_name, RCL_RET_INVALID_ARGUMENT); + // Create default topic name substitutions map + rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions_map, 0, allocator); + if (rcutils_ret != RCUTILS_RET_OK) { + rcutils_error_string_t error = rcutils_get_error_string(); + rcutils_reset_error(); + RCL_SET_ERROR_MSG(error.str); + if (RCUTILS_RET_BAD_ALLOC == rcutils_ret) { + return RCL_RET_BAD_ALLOC; + } + return RCL_RET_ERROR; + } + char * expanded_topic_name = NULL; + char * remapped_topic_name = NULL; + rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map); + if (ret != RCL_RET_OK) { + if (RCL_RET_BAD_ALLOC != ret) { + ret = RCL_RET_ERROR; + } + goto cleanup; + } + // expand topic name + ret = rcl_expand_topic_name( + input_topic_name, + node_name, + node_namespace, + &substitutions_map, + allocator, + &expanded_topic_name); + if (RCL_RET_OK != ret) { + goto cleanup; + } + // remap topic name + if (!only_expand) { + ret = rcl_remap_name( + local_args, global_args, is_service ? RCL_SERVICE_REMAP : RCL_TOPIC_REMAP, + expanded_topic_name, node_name, node_namespace, &substitutions_map, allocator, + &remapped_topic_name); + if (RCL_RET_OK != ret) { + goto cleanup; + } + } + if (NULL == remapped_topic_name) { + remapped_topic_name = expanded_topic_name; + expanded_topic_name = NULL; + } + // validate the result + int validation_result; + rmw_ret_t rmw_ret = rmw_validate_full_topic_name(remapped_topic_name, &validation_result, NULL); + if (rmw_ret != RMW_RET_OK) { + const char * error = rmw_get_error_string().str; + rmw_reset_error(); + RCL_SET_ERROR_MSG(error); + ret = RCL_RET_ERROR; + goto cleanup; + } + if (validation_result != RMW_TOPIC_VALID) { + RCL_SET_ERROR_MSG(rmw_full_topic_name_validation_result_string(validation_result)); + ret = RCL_RET_TOPIC_NAME_INVALID; + goto cleanup; + } + *output_topic_name = remapped_topic_name; + remapped_topic_name = NULL; + +cleanup: + rcutils_ret = rcutils_string_map_fini(&substitutions_map); + if (rcutils_ret != RCUTILS_RET_OK) { + rcutils_error_string_t error = rcutils_get_error_string(); + rcutils_reset_error(); + if (RCL_RET_OK == ret) { + RCL_SET_ERROR_MSG(error.str); + ret = RCL_RET_ERROR; + } else { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "failed to fini string_map (%d) during error handling: %s", + rcutils_ret, + error.str); + } + } + allocator.deallocate(expanded_topic_name, allocator.state); + allocator.deallocate(remapped_topic_name, allocator.state); + if (is_service && RCL_RET_TOPIC_NAME_INVALID == ret) { + ret = RCL_RET_SERVICE_NAME_INVALID; + } + return ret; +} + +rcl_ret_t +rcl_node_resolve_name( + const rcl_node_t * node, + const char * input_topic_name, + rcl_allocator_t allocator, + bool is_service, + bool only_expand, + char ** output_topic_name) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + const rcl_node_options_t * node_options = rcl_node_get_options(node); + if (NULL == node_options) { + return RCL_RET_ERROR; + } + rcl_arguments_t * global_args = NULL; + if (node_options->use_global_arguments) { + global_args = &(node->context->global_arguments); + } + + return rcl_resolve_name( + &(node_options->arguments), + global_args, + input_topic_name, + rcl_node_get_name(node), + rcl_node_get_namespace(node), + allocator, + is_service, + only_expand, + output_topic_name); +} diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 6662aca0f..3046598a3 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -24,12 +24,10 @@ extern "C" #include "rcl/allocator.h" #include "rcl/error_handling.h" -#include "rcl/expand_topic_name.h" -#include "rcl/remap.h" +#include "rcl/node.h" #include "rcutils/logging_macros.h" #include "rcutils/macros.h" #include "rmw/error_handling.h" -#include "rmw/validate_full_topic_name.h" #include "tracetools/tracetools.h" #include "./common.h" @@ -78,90 +76,26 @@ rcl_publisher_init( RCUTILS_LOG_DEBUG_NAMED( ROS_PACKAGE_NAME, "Initializing publisher for topic name '%s'", topic_name); - - // Expand the given topic name. - rcutils_allocator_t rcutils_allocator = *allocator; // implicit conversion to rcutils version - rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map(); - rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions_map, 0, rcutils_allocator); - if (rcutils_ret != RCUTILS_RET_OK) { - RCL_SET_ERROR_MSG(rcutils_get_error_string().str); - if (rcutils_ret == RCUTILS_RET_BAD_ALLOC) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map); - if (ret != RCL_RET_OK) { - rcutils_ret = rcutils_string_map_fini(&substitutions_map); - if (rcutils_ret != RCUTILS_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, - "failed to fini string_map (%d) during error handling: %s", - rcutils_ret, - rcutils_get_error_string().str); - } - if (ret == RCL_RET_BAD_ALLOC) { - return ret; - } - return RCL_RET_ERROR; - } - char * expanded_topic_name = NULL; + // Expand and remap the given topic name. char * remapped_topic_name = NULL; - ret = rcl_expand_topic_name( + rcl_ret_t ret = rcl_node_resolve_name( + node, topic_name, - rcl_node_get_name(node), - rcl_node_get_namespace(node), - &substitutions_map, *allocator, - &expanded_topic_name); - rcutils_ret = rcutils_string_map_fini(&substitutions_map); - if (rcutils_ret != RCUTILS_RET_OK) { - RCL_SET_ERROR_MSG(rcutils_get_error_string().str); - ret = RCL_RET_ERROR; - goto cleanup; - } + false, + false, + &remapped_topic_name); if (ret != RCL_RET_OK) { if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { ret = RCL_RET_TOPIC_NAME_INVALID; - } else { + } else if (ret != RCL_RET_BAD_ALLOC) { ret = RCL_RET_ERROR; } goto cleanup; } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded topic name '%s'", expanded_topic_name); - - const rcl_node_options_t * node_options = rcl_node_get_options(node); - if (NULL == node_options) { - ret = RCL_RET_ERROR; - goto cleanup; - } - rcl_arguments_t * global_args = NULL; - if (node_options->use_global_arguments) { - global_args = &(node->context->global_arguments); - } - ret = rcl_remap_topic_name( - &(node_options->arguments), global_args, expanded_topic_name, - rcl_node_get_name(node), rcl_node_get_namespace(node), *allocator, &remapped_topic_name); - if (RCL_RET_OK != ret) { - goto fail; - } else if (NULL == remapped_topic_name) { - remapped_topic_name = expanded_topic_name; - expanded_topic_name = NULL; - } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Expanded and remapped topic name '%s'", remapped_topic_name); - // Validate the expanded topic name. - int validation_result; - rmw_ret_t rmw_ret = rmw_validate_full_topic_name(remapped_topic_name, &validation_result, NULL); - if (rmw_ret != RMW_RET_OK) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - ret = RCL_RET_ERROR; - goto cleanup; - } - if (validation_result != RMW_TOPIC_VALID) { - RCL_SET_ERROR_MSG(rmw_full_topic_name_validation_result_string(validation_result)); - ret = RCL_RET_TOPIC_NAME_INVALID; - goto cleanup; - } // Allocate space for the implementation struct. publisher->impl = (rcl_publisher_impl_t *)allocator->allocate( sizeof(rcl_publisher_impl_t), allocator->state); @@ -180,7 +114,7 @@ rcl_publisher_init( RCL_CHECK_FOR_NULL_WITH_MSG( publisher->impl->rmw_handle, rmw_get_error_string().str, goto fail); // get actual qos, and store it - rmw_ret = rmw_publisher_get_actual_qos( + rmw_ret_t rmw_ret = rmw_publisher_get_actual_qos( publisher->impl->rmw_handle, &publisher->impl->actual_qos); if (RMW_RET_OK != rmw_ret) { @@ -221,12 +155,7 @@ rcl_publisher_init( ret = fail_ret; // Fall through to cleanup cleanup: - if (NULL != expanded_topic_name) { - allocator->deallocate(expanded_topic_name, allocator->state); - } - if (NULL != remapped_topic_name) { - allocator->deallocate(remapped_topic_name, allocator->state); - } + allocator->deallocate(remapped_topic_name, allocator->state); return ret; } diff --git a/rcl/src/rcl/remap.c b/rcl/src/rcl/remap.c index d8af63f87..5db08623c 100644 --- a/rcl/src/rcl/remap.c +++ b/rcl/src/rcl/remap.c @@ -99,9 +99,9 @@ rcl_remap_copy( /// Get the first matching rule in a chain. /// \return RCL_RET_OK if no errors occurred while searching for a rule -RCL_LOCAL +static rcl_ret_t -_rcl_remap_first_match( +rcl_remap_first_match( rcl_remap_t * remap_rules, int num_rules, rcl_remap_type_t type_bitmask, @@ -159,7 +159,7 @@ _rcl_remap_first_match( /// Remap from one name to another using rules matching a given type bitmask. RCL_LOCAL rcl_ret_t -_rcl_remap_name( +rcl_remap_name( const rcl_arguments_t * local_arguments, const rcl_arguments_t * global_arguments, rcl_remap_type_t type_bitmask, @@ -188,7 +188,7 @@ _rcl_remap_name( // Look at local rules first if (NULL != local_arguments) { - rcl_ret_t ret = _rcl_remap_first_match( + rcl_ret_t ret = rcl_remap_first_match( local_arguments->impl->remap_rules, local_arguments->impl->num_remap_rules, type_bitmask, name, node_name, node_namespace, substitutions, allocator, &rule); if (ret != RCL_RET_OK) { @@ -197,7 +197,7 @@ _rcl_remap_name( } // Check global rules if no local rule matched if (NULL == rule && NULL != global_arguments) { - rcl_ret_t ret = _rcl_remap_first_match( + rcl_ret_t ret = rcl_remap_first_match( global_arguments->impl->remap_rules, global_arguments->impl->num_remap_rules, type_bitmask, name, node_name, node_namespace, substitutions, allocator, &rule); if (ret != RCL_RET_OK) { @@ -244,7 +244,7 @@ rcl_remap_topic_name( if (RCUTILS_RET_OK == rcutils_ret) { ret = rcl_get_default_topic_name_substitutions(&substitutions); if (RCL_RET_OK == ret) { - ret = _rcl_remap_name( + ret = rcl_remap_name( local_arguments, global_arguments, RCL_TOPIC_REMAP, topic_name, node_name, node_namespace, &substitutions, allocator, output_name); } @@ -274,7 +274,7 @@ rcl_remap_service_name( if (rcutils_ret == RCUTILS_RET_OK) { ret = rcl_get_default_topic_name_substitutions(&substitutions); if (ret == RCL_RET_OK) { - ret = _rcl_remap_name( + ret = rcl_remap_name( local_arguments, global_arguments, RCL_SERVICE_REMAP, service_name, node_name, node_namespace, &substitutions, allocator, output_name); } @@ -299,7 +299,7 @@ rcl_remap_node_name( RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_INVALID_ARGUMENT); - return _rcl_remap_name( + return rcl_remap_name( local_arguments, global_arguments, RCL_NODENAME_REMAP, NULL, node_name, NULL, NULL, allocator, output_name); } @@ -318,7 +318,7 @@ rcl_remap_node_namespace( RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_INVALID_ARGUMENT); - return _rcl_remap_name( + return rcl_remap_name( local_arguments, global_arguments, RCL_NAMESPACE_REMAP, NULL, node_name, NULL, NULL, allocator, output_namespace); } diff --git a/rcl/src/rcl/remap_impl.h b/rcl/src/rcl/remap_impl.h index 898e58cb7..770672592 100644 --- a/rcl/src/rcl/remap_impl.h +++ b/rcl/src/rcl/remap_impl.h @@ -51,6 +51,19 @@ typedef struct rcl_remap_impl_t rcl_allocator_t allocator; } rcl_remap_impl_t; +RCL_LOCAL +rcl_ret_t +rcl_remap_name( + const rcl_arguments_t * local_arguments, + const rcl_arguments_t * global_arguments, + rcl_remap_type_t type_bitmask, + const char * name, + const char * node_name, + const char * node_namespace, + const rcutils_string_map_t * substitutions, + rcl_allocator_t allocator, + char ** output_name); + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index 196e4aab3..251ab66b1 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -23,13 +23,11 @@ extern "C" #include #include "rcl/error_handling.h" -#include "rcl/expand_topic_name.h" -#include "rcl/remap.h" +#include "rcl/node.h" #include "rcutils/logging_macros.h" #include "rcutils/macros.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "rmw/validate_full_topic_name.h" #include "tracetools/tracetools.h" typedef struct rcl_service_impl_t @@ -79,89 +77,27 @@ rcl_service_init( RCL_SET_ERROR_MSG("service already initialized, or memory was unintialized"); return RCL_RET_ALREADY_INIT; } - // Expand the given service name. - rcutils_allocator_t rcutils_allocator = *allocator; // implicit conversion to rcutils version - rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map(); - rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions_map, 0, rcutils_allocator); - if (rcutils_ret != RCUTILS_RET_OK) { - RCL_SET_ERROR_MSG(rcutils_get_error_string().str); - if (RCUTILS_RET_BAD_ALLOC == rcutils_ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map); - if (ret != RCL_RET_OK) { - rcutils_ret = rcutils_string_map_fini(&substitutions_map); - if (rcutils_ret != RCUTILS_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, - "failed to fini string_map (%d) during error handling: %s", - rcutils_ret, - rcutils_get_error_string().str); - } - if (RCL_RET_BAD_ALLOC == ret) { - return ret; - } - return RCL_RET_ERROR; - } - char * expanded_service_name = NULL; + + // Expand and remap the given service name. char * remapped_service_name = NULL; - ret = rcl_expand_topic_name( + rcl_ret_t ret = rcl_node_resolve_name( + node, service_name, - rcl_node_get_name(node), - rcl_node_get_namespace(node), - &substitutions_map, *allocator, - &expanded_service_name); - rcutils_ret = rcutils_string_map_fini(&substitutions_map); - if (rcutils_ret != RCUTILS_RET_OK) { - RCL_SET_ERROR_MSG(rcutils_get_error_string().str); - ret = RCL_RET_ERROR; - goto cleanup; - } + true, + false, + &remapped_service_name); if (ret != RCL_RET_OK) { - if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { + if (ret == RCL_RET_SERVICE_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { ret = RCL_RET_SERVICE_NAME_INVALID; - } else { + } else if (ret != RCL_RET_BAD_ALLOC) { ret = RCL_RET_ERROR; } goto cleanup; } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded service name '%s'", expanded_service_name); - - const rcl_node_options_t * node_options = rcl_node_get_options(node); - if (NULL == node_options) { - ret = RCL_RET_ERROR; - goto cleanup; - } - rcl_arguments_t * global_args = NULL; - if (node_options->use_global_arguments) { - global_args = &(node->context->global_arguments); - } - ret = rcl_remap_service_name( - &(node_options->arguments), global_args, expanded_service_name, - rcl_node_get_name(node), rcl_node_get_namespace(node), *allocator, &remapped_service_name); - if (RCL_RET_OK != ret) { - goto fail; - } else if (NULL == remapped_service_name) { - remapped_service_name = expanded_service_name; - expanded_service_name = NULL; - } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Expanded and remapped service name '%s'", remapped_service_name); - // Validate the expanded service name. - int validation_result; - rmw_ret_t rmw_ret = rmw_validate_full_topic_name(remapped_service_name, &validation_result, NULL); - if (rmw_ret != RMW_RET_OK) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - ret = RCL_RET_ERROR; - goto cleanup; - } - if (validation_result != RMW_TOPIC_VALID) { - RCL_SET_ERROR_MSG(rmw_full_topic_name_validation_result_string(validation_result)); - ret = RCL_RET_SERVICE_NAME_INVALID; - goto cleanup; - } // Allocate space for the implementation struct. service->impl = (rcl_service_impl_t *)allocator->allocate( sizeof(rcl_service_impl_t), allocator->state); @@ -205,12 +141,7 @@ rcl_service_init( ret = fail_ret; // Fall through to clean up cleanup: - if (NULL != expanded_service_name) { - allocator->deallocate(expanded_service_name, allocator->state); - } - if (NULL != remapped_service_name) { - allocator->deallocate(remapped_service_name, allocator->state); - } + allocator->deallocate(remapped_service_name, allocator->state); return ret; } diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index e9c2748aa..5b122e415 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -22,8 +22,7 @@ extern "C" #include #include "rcl/error_handling.h" -#include "rcl/expand_topic_name.h" -#include "rcl/remap.h" +#include "rcl/node.h" #include "rcutils/logging_macros.h" #include "rmw/error_handling.h" #include "rmw/validate_full_topic_name.h" @@ -67,89 +66,27 @@ rcl_subscription_init( RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninitialized"); return RCL_RET_ALREADY_INIT; } - // Expand the given topic name. - rcutils_allocator_t rcutils_allocator = *allocator; // implicit conversion to rcutils version - rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map(); - rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions_map, 0, rcutils_allocator); - if (rcutils_ret != RCUTILS_RET_OK) { - RCL_SET_ERROR_MSG(rcutils_get_error_string().str); - if (RCUTILS_RET_BAD_ALLOC == rcutils_ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map); - if (ret != RCL_RET_OK) { - rcutils_ret = rcutils_string_map_fini(&substitutions_map); - if (rcutils_ret != RCUTILS_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, - "failed to fini string_map (%d) during error handling: %s", - rcutils_ret, - rcutils_get_error_string().str); - } - if (RCL_RET_BAD_ALLOC == ret) { - return ret; - } - return RCL_RET_ERROR; - } - char * expanded_topic_name = NULL; + + // Expand and remap the given topic name. char * remapped_topic_name = NULL; - ret = rcl_expand_topic_name( + rcl_ret_t ret = rcl_node_resolve_name( + node, topic_name, - rcl_node_get_name(node), - rcl_node_get_namespace(node), - &substitutions_map, *allocator, - &expanded_topic_name); - rcutils_ret = rcutils_string_map_fini(&substitutions_map); - if (rcutils_ret != RCUTILS_RET_OK) { - RCL_SET_ERROR_MSG(rcutils_get_error_string().str); - ret = RCL_RET_ERROR; - goto cleanup; - } + false, + false, + &remapped_topic_name); if (ret != RCL_RET_OK) { if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { ret = RCL_RET_TOPIC_NAME_INVALID; - } else { + } else if (ret != RCL_RET_BAD_ALLOC) { ret = RCL_RET_ERROR; } goto cleanup; } - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Expanded topic name '%s'", expanded_topic_name); - - const rcl_node_options_t * node_options = rcl_node_get_options(node); - if (NULL == node_options) { - ret = RCL_RET_ERROR; - goto cleanup; - } - rcl_arguments_t * global_args = NULL; - if (node_options->use_global_arguments) { - global_args = &(node->context->global_arguments); - } - ret = rcl_remap_topic_name( - &(node_options->arguments), global_args, expanded_topic_name, - rcl_node_get_name(node), rcl_node_get_namespace(node), *allocator, &remapped_topic_name); - if (RCL_RET_OK != ret) { - goto fail; - } else if (NULL == remapped_topic_name) { - remapped_topic_name = expanded_topic_name; - expanded_topic_name = NULL; - } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Expanded and remapped topic name '%s'", remapped_topic_name); - // Validate the expanded topic name. - int validation_result; - rmw_ret_t rmw_ret = rmw_validate_full_topic_name(remapped_topic_name, &validation_result, NULL); - if (rmw_ret != RMW_RET_OK) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - ret = RCL_RET_ERROR; - goto cleanup; - } - if (validation_result != RMW_TOPIC_VALID) { - RCL_SET_ERROR_MSG(rmw_full_topic_name_validation_result_string(validation_result)); - ret = RCL_RET_TOPIC_NAME_INVALID; - goto cleanup; - } // Allocate memory for the implementation struct. subscription->impl = (rcl_subscription_impl_t *)allocator->allocate( sizeof(rcl_subscription_impl_t), allocator->state); @@ -169,7 +106,7 @@ rcl_subscription_init( goto fail; } // get actual qos, and store it - rmw_ret = rmw_subscription_get_actual_qos( + rmw_ret_t rmw_ret = rmw_subscription_get_actual_qos( subscription->impl->rmw_handle, &subscription->impl->actual_qos); if (RMW_RET_OK != rmw_ret) { @@ -208,12 +145,7 @@ rcl_subscription_init( ret = fail_ret; // Fall through to cleanup cleanup: - if (NULL != expanded_topic_name) { - allocator->deallocate(expanded_topic_name, allocator->state); - } - if (NULL != remapped_topic_name) { - allocator->deallocate(remapped_topic_name, allocator->state); - } + allocator->deallocate(remapped_topic_name, allocator->state); return ret; } diff --git a/rcl/test/rcl/test_node.cpp b/rcl/test/rcl/test_node.cpp index 2d20306fd..c21576126 100644 --- a/rcl/test/rcl/test_node.cpp +++ b/rcl/test/rcl/test_node.cpp @@ -33,6 +33,7 @@ #include "rcl/logging_rosout.h" #include "../mocking_utils/patch.hpp" +#include "./arg_macros.hpp" #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX @@ -900,3 +901,99 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_options_fai EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&prev_ini_options.arguments)); } + +/* Tests special case node_options + */ +TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_resolve_name) { + rcl_allocator_t default_allocator = rcl_get_default_allocator(); + char * final_name = NULL; + rcl_node_t node = rcl_get_zero_initialized_node(); + // Invalid node + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_node_resolve_name(NULL, "my_topic", default_allocator, false, false, &final_name)); + EXPECT_EQ( + RCL_RET_ERROR, + rcl_node_resolve_name(&node, "my_topic", default_allocator, false, false, &final_name)); + + // Initialize rcl with rcl_init(). + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + rcl_context_t context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &context); + ASSERT_EQ(RCL_RET_OK, ret); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ASSERT_EQ(RCL_RET_OK, rcl_shutdown(&context)); + ASSERT_EQ(RCL_RET_OK, rcl_context_fini(&context)); + }); + + // Initialize node with default options + rcl_node_options_t options = rcl_node_get_default_options(); + rcl_arguments_t local_arguments = rcl_get_zero_initialized_arguments(); + const char * argv[] = {"process_name", "--ros-args", "-r", "/bar/foo:=/foo/local_args"}; + unsigned int argc = (sizeof(argv) / sizeof(const char *)); + ret = rcl_parse_arguments( + argc, argv, default_allocator, &local_arguments); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + options.arguments = local_arguments; // transfer ownership + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_node_options_fini(&options); + }); + ret = rcl_node_init(&node, "node", "/ns", &context, &options); + ASSERT_EQ(RCL_RET_OK, ret); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ASSERT_EQ(RCL_RET_OK, rcl_node_fini(&node)); + }); + + // Invalid arguments + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_node_resolve_name(&node, NULL, default_allocator, false, false, &final_name)); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_node_resolve_name(&node, "my_topic", default_allocator, false, false, NULL)); + + // Some valid options, test_remap and test_expand_topic_name already have good coverage + EXPECT_EQ( + RCL_RET_OK, + rcl_node_resolve_name(&node, "my_topic", default_allocator, false, false, &final_name)); + ASSERT_TRUE(final_name); + EXPECT_STREQ("/ns/my_topic", final_name); + default_allocator.deallocate(final_name, default_allocator.state); + + EXPECT_EQ( + RCL_RET_OK, + rcl_node_resolve_name(&node, "my_service", default_allocator, true, false, &final_name)); + ASSERT_TRUE(final_name); + EXPECT_STREQ("/ns/my_service", final_name); + default_allocator.deallocate(final_name, default_allocator.state); + + EXPECT_EQ( + RCL_RET_OK, + rcl_node_resolve_name(&node, "/bar/foo", default_allocator, false, false, &final_name)); + ASSERT_TRUE(final_name); + EXPECT_STREQ("/foo/local_args", final_name); + default_allocator.deallocate(final_name, default_allocator.state); + + EXPECT_EQ( + RCL_RET_OK, + rcl_node_resolve_name(&node, "/bar/foo", default_allocator, false, true, &final_name)); + ASSERT_TRUE(final_name); + EXPECT_STREQ("/bar/foo", final_name); + default_allocator.deallocate(final_name, default_allocator.state); + + EXPECT_EQ( + RCL_RET_OK, + rcl_node_resolve_name(&node, "relative_ns/foo", default_allocator, true, false, &final_name)); + ASSERT_TRUE(final_name); + EXPECT_STREQ("/ns/relative_ns/foo", final_name); + default_allocator.deallocate(final_name, default_allocator.state); +} diff --git a/rcl/test/rcl/test_publisher.cpp b/rcl/test/rcl/test_publisher.cpp index 3fcbe0227..cf28011f7 100644 --- a/rcl/test/rcl/test_publisher.cpp +++ b/rcl/test/rcl/test_publisher.cpp @@ -788,23 +788,6 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_mocks_fail_publ EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; rcl_reset_error(); } - { - // Internal failure when fini rcutils_string_map returns error, targets rcl_remap_topic_name - auto mock = mocking_utils::patch( - "lib:rcl", rcutils_string_map_init, [](auto...) { - static int counter = 1; - if (counter == 1) { - counter++; - return RCUTILS_RET_OK; - } else { - // This makes rcl_remap_topic_name fail - return RCUTILS_RET_ERROR; - } - }); - ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); - EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; - rcl_reset_error(); - } { // Internal rmw failure validating topic name auto mock = mocking_utils::patch_and_return(