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(