Add convenient node method to get a final topic/service name (#835)

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

Author: ivanpauno

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

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.

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.
+/**
+ * <hr>
+ * 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

rcl/src/rcl/client.c

@@ -23,14 +23,12 @@ extern "C"
 #include <string.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 "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;
 }
 

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);
+}