diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt
index 634bf82..f253ed2 100644
--- a/rcl/CMakeLists.txt
+++ b/rcl/CMakeLists.txt
@@ -27,6 +27,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
endif()
set(${PROJECT_NAME}_sources
+ src/rcl/arguments.c
src/rcl/client.c
src/rcl/common.c
src/rcl/expand_topic_name.c
@@ -35,6 +36,7 @@ set(${PROJECT_NAME}_sources
src/rcl/node.c
src/rcl/publisher.c
src/rcl/rcl.c
+ src/rcl/remap.c
src/rcl/rmw_implementation_identifier_check.c
src/rcl/service.c
src/rcl/subscription.c
diff --git a/rcl/include/rcl/arguments.h b/rcl/include/rcl/arguments.h
new file mode 100644
index 0000000..1bf13a3
--- /dev/null
+++ b/rcl/include/rcl/arguments.h
@@ -0,0 +1,187 @@
+// 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.
+
+#ifndef RCL__ARGUMENTS_H_
+#define RCL__ARGUMENTS_H_
+
+#include "rcl/allocator.h"
+#include "rcl/macros.h"
+#include "rcl/types.h"
+#include "rcl/visibility_control.h"
+
+#if __cplusplus
+extern "C"
+{
+#endif
+
+struct rcl_arguments_impl_t;
+
+/// Hold output of parsing command line arguments.
+typedef struct rcl_arguments_t
+{
+ /// Private implementation pointer.
+ struct rcl_arguments_impl_t * impl;
+} rcl_arguments_t;
+
+/// Return a rcl_node_t struct with members initialized to `NULL`.
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_arguments_t
+rcl_get_zero_initialized_arguments(void);
+
+/// Parse command line arguments into a structure usable by code.
+/**
+ * If an argument does not appear to be a valid ROS argument then it is skipped and parsing
+ * continues with the next argument in `argv`.
+ * \sa rcl_arguments_get_count_unparsed()
+ * \sa rcl_arguments_get_unparsed()
+ *
+ * Successfully parsed remap rules are stored in the order they were given in `argv`.
+ * If given arguments `{"__ns:=/foo", "__ns:=/bar"}` then the namespace used by nodes in this
+ * process will be `/foo` and not `/bar`.
+ * \sa rcl_remap_topic_name()
+ * \sa rcl_remap_service_name()
+ * \sa rcl_remap_node_name()
+ * \sa rcl_remap_node_namespace()
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | Yes
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] argc The number of arguments in argv.
+ * \param[in] argv Whe values of the arguments.
+ * \param[in] allocator A valid allocator.
+ * \param[out] args_output A structure that will contain the result of parsing.
+ * \return `RCL_RET_OK` if the arguments were parsed successfully, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any function arguments are invalid, or
+ * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_parse_arguments(
+ int argc,
+ const char * const argv[],
+ rcl_allocator_t allocator,
+ rcl_arguments_t * args_output);
+
+/// Return the number of arguments that were not successfully parsed.
+/**
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | No
+ * Thread-Safe | Yes
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] args An arguments structure that has been parsed.
+ * \return number of unparsed arguments, or
+ * \return -1 if args is `NULL` or zero initialized.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+int
+rcl_arguments_get_count_unparsed(
+ rcl_arguments_t * args);
+
+/// Return a list of indexes that weren't successfully parsed.
+/**
+ * Some arguments may not have been successfully parsed, or were not intended as ROS arguments.
+ * This function populates an array of indexes to these arguments in the original argv array.
+ * Since the first argument is always assumed to be a process name, the list will always contain
+ * the index 0.
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | Yes
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] args An arguments structure that has been parsed.
+ * \param[in] allocator A valid allocator.
+ * \param[out] output_unparsed_indices An allocated array of indices into the original argv array.
+ * This array must be deallocated by the caller using the given allocator.
+ * If there are no unparsed args then the output will be set to NULL.
+ * \return `RCL_RET_OK` if everything goes correctly, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any function arguments are invalid, or
+ * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_arguments_get_unparsed(
+ rcl_arguments_t * args,
+ rcl_allocator_t allocator,
+ int ** output_unparsed_indices);
+
+/// Reclaim resources held inside rcl_arguments_t structure.
+/**
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | No
+ * Thread-Safe | Yes
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] args The structure to be deallocated.
+ * \param[in] allocator A valid allocator.
+ * \return `RCL_RET_OK` if the memory was successfully freed, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any function arguments are invalid, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_arguments_fini(
+ rcl_arguments_t * args);
+
+/// Get a global instance of command line arguments.
+/**
+ * \sa rcl_init(int, char **, rcl_allocator_t)
+ * \sa rcl_shutdown()
+ * This returns parsed command line arguments that were passed to `rcl_init()`.
+ * The value returned by this function is undefined before `rcl_init()` is called and after
+ * `rcl_shutdown()` is called.
+ * The return value must not be finalized.
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | No
+ * Thread-Safe | Yes
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \return a global instance of parsed command line arguments.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_arguments_t *
+rcl_get_global_arguments();
+
+#if __cplusplus
+}
+#endif
+
+#endif // RCL__ARGUMENTS_H_
diff --git a/rcl/include/rcl/node.h b/rcl/include/rcl/node.h
index 49ebb2f..d15b3d8 100644
--- a/rcl/include/rcl/node.h
+++ b/rcl/include/rcl/node.h
@@ -23,6 +23,7 @@ extern "C"
#include
#include "rcl/allocator.h"
+#include "rcl/arguments.h"
#include "rcl/macros.h"
#include "rcl/types.h"
#include "rcl/visibility_control.h"
@@ -65,6 +66,12 @@ typedef struct rcl_node_options_t
/// Custom allocator used for internal allocations.
rcl_allocator_t allocator;
+
+ /// If false then only use arguments in this struct, otherwise use global arguments also.
+ bool use_global_arguments;
+
+ /// Command line arguments that apply only to this node.
+ rcl_arguments_t arguments;
} rcl_node_options_t;
/// Return a rcl_node_t struct with members initialized to `NULL`.
diff --git a/rcl/include/rcl/remap.h b/rcl/include/rcl/remap.h
new file mode 100644
index 0000000..a8b68f6
--- /dev/null
+++ b/rcl/include/rcl/remap.h
@@ -0,0 +1,239 @@
+// 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.
+
+#ifndef RCL__REMAP_H_
+#define RCL__REMAP_H_
+
+#include "rcl/allocator.h"
+#include "rcl/arguments.h"
+#include "rcl/macros.h"
+#include "rcl/types.h"
+#include "rcl/visibility_control.h"
+
+#if __cplusplus
+extern "C"
+{
+#endif
+
+// TODO(sloretz) add documentation about rostopic:// when it is supported
+/// Remap a topic name based on given rules.
+/**
+ * The supplied topic name must have already been expanded to a fully qualified name.
+ * \sa rcl_expand_topic_name()
+ *
+ * If `local_arguments` is not NULL and not zero intialized then its remap rules are checked first.
+ * If no rules matched and `global_arguments` is not NULL and not zero intitialized then its rules
+ * are checked next.
+ * If both `local_arguments` and global_arguments are NULL or zero intialized then the function will
+ * return RCL_RET_INVALID_ARGUMENT.
+ *
+ * `global_arguments` is usually the arguments passed to `rcl_init()`.
+ * \sa rcl_init()
+ * \sa rcl_get_global_arguments()
+ *
+ * Remap rules are checked in the order they were given.
+ * For rules passed to `rcl_init` this usually is the order they were passed on the command line.
+ * \sa rcl_parse_arguments()
+ *
+ * Only the first remap rule that matches is used to remap a name.
+ * For example, if the command line arguments are `foo:=bar bar:=baz` the topic `foo` is remapped to
+ * `bar` and not `baz`.
+ *
+ * `node_name` and `node_namespace` are used to expand the match and replacement into fully
+ * qualified names.
+ * Given node_name `trudy`, namespace `/ns`, and rule `foo:=~/bar` the names in the rule are
+ * expanded to `/ns/foo:=/ns/trudy/bar`.
+ * The rule will only apply if the given topic name is `/ns/foo`.
+ *
+ * `node_name` is also used to match against node specific rules.
+ * Given rules `alice:foo:=bar foo:=baz`, node name `alice`, and topic `foo` the remapped topic
+ * name will be `bar`.
+ * If given the node name `bob` and topic `foo` the remaped topic name would be `baz` instead.
+ * Note that processing always stops at the first matching rule even if there is a more specific one
+ * later on.
+ * Given `foo:=bar alice:foo:=baz` and topic name `foo` the remapped topic name will always be
+ * `bar` regardless of the node name given.
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] local_arguments Command line arguments to be used before global arguments, or
+ * if NULL or zero-initialized then only global arguments are used.
+ * \param[in] global_arguments Command line arguments to use if no local rules matched, or
+ * `NULL` or zero-initialized to ignore global arguments.
+ * \param[in] topic_name A fully qualified and expanded topic name to be remapped.
+ * \param[in] node_name The name of the node to which name belongs.
+ * \param[in] node_namespace The namespace of a node to which name belongs.
+ * \param[in] allocator A valid allocator to use.
+ * \param[out] output_name Either an allocated string with the remapped name, or
+ * `NULL` if no remap rules matched the name.
+ * \return `RCL_RET_OK` if the topic name was remapped or no rules matched, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or
+ * \return `RCL_RET_TOPIC_NAME_INVALID` if the given topic name is invalid, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_remap_topic_name(
+ const rcl_arguments_t * local_arguments,
+ const rcl_arguments_t * global_arguments,
+ const char * topic_name,
+ const char * node_name,
+ const char * node_namespace,
+ rcl_allocator_t allocator,
+ char ** output_name);
+
+// TODO(sloretz) add documentation about rosservice:// when it is supported
+/// Remap a service name based on given rules.
+/**
+ * The supplied service name must have already been expanded to a fully qualified name.
+ *
+ * The behavior of this function is identical to rcl_expand_topic_name() except that it applies
+ * to service names instead of topic names.
+ * \sa rcl_expand_topic_name()
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] local_arguments Command line arguments to be used before global arguments, or
+ * if NULL or zero-initialized then only global arguments are used.
+ * \param[in] global_arguments Command line arguments to use if no local rules matched, or
+ * `NULL` or zero-initialized to ignore global arguments.
+ * \param[in] service_name A fully qualified and expanded service name to be remapped.
+ * \param[in] node_name The name of the node to which name belongs.
+ * \param[in] node_namespace The namespace of a node to which name belongs.
+ * \param[in] allocator A valid allocator to use.
+ * \param[out] output_name Either an allocated string with the remapped name, or
+ * `NULL` if no remap rules matched the name.
+ * \return `RCL_RET_OK` if the name was remapped or no rules matched, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or
+ * \return `RCL_RET_SERVICE_NAME_INVALID` if the given name is invalid, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_remap_service_name(
+ const rcl_arguments_t * local_arguments,
+ const rcl_arguments_t * global_arguments,
+ const char * serivice_name,
+ const char * node_name,
+ const char * node_namespace,
+ rcl_allocator_t allocator,
+ char ** output_name);
+
+/// Remap a node name based on given rules.
+/**
+ * This function returns the node name that a node with the given name would be remapped to.
+ * When a node's name is remapped it changes its logger name and the output of expanding relative
+ * topic and service names.
+ *
+ * When composing nodes make sure that the final node names used are unique per process.
+ * There is not currently a way to independently remap the names of two nodes that were created
+ * with the same node name and are manually composed into one process.
+ *
+ * The behavior of `local_arguments`, `global_arguments`, `node_name`, the order remap rules are
+ * applied, and node specific rules is identical to rcl_remap_topic_name().
+ * \sa rcl_remap_topic_name()
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] local_arguments Arguments to be used before global arguments.
+ * \param[in] global_arguments Command line arguments to use if no local rules matched, or
+ * `NULL` or zero-initialized to ignore global arguments.
+ * \param[in] node_name The current name of the node.
+ * \param[in] allocator A valid allocator to use.
+ * \param[out] output_name Either an allocated string with the remapped name, or
+ * `NULL` if no remap rules matched the name.
+ * \return `RCL_RET_OK` If the name was remapped or no rules matched, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or
+ * \return `RCL_RET_NODE_INVALID_NAME` if the name is invalid, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_remap_node_name(
+ const rcl_arguments_t * local_arguments,
+ const rcl_arguments_t * global_arguments,
+ const char * node_name,
+ rcl_allocator_t allocator,
+ char ** output_name);
+
+/// Remap a namespace based on given rules.
+/**
+ * This function returns the namespace that a node with the given name would be remapped to.
+ * When a node's namespace is remapped it changes its logger name and the output of expanding
+ * relative topic and service names.
+ *
+ * The behavior of `local_arguments`, `global_arguments`, `node_name`, the order remap rules are
+ * applied, and node specific rules is identical to rcl_remap_topic_name().
+ * \sa rcl_remap_topic_name()
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Yes
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] local_arguments Arguments to be used before global arguments.
+ * \param[in] global_arguments Command line arguments to use if no local rules matched, or
+ * `NULL` or zero-initialized to ignore global arguments.
+ * \param[in] node_name The name of the node whose namespace is being remapped.
+ * \param[in] allocator A valid allocator to be used.
+ * \param[out] output_namespace Either an allocated string with the remapped namespace, or
+ * `NULL` if no remap rules matched the name.
+ * \return `RCL_RET_OK` if the node name was remapped or no rules matched, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or
+ * \return `RCL_RET_NODE_INVALID_NAMESPACE` if the remapped namespace is invalid, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_remap_node_namespace(
+ const rcl_arguments_t * local_arguments,
+ const rcl_arguments_t * global_arguments,
+ const char * node_name,
+ rcl_allocator_t allocator,
+ char ** output_namespace);
+
+#if __cplusplus
+}
+#endif
+
+#endif // RCL__REMAP_H_
diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h
index 3cdf54e..93e9095 100644
--- a/rcl/include/rcl/types.h
+++ b/rcl/include/rcl/types.h
@@ -87,4 +87,8 @@ typedef rmw_ret_t rcl_ret_t;
/// Given rcl_wait_set_t is full return code.
#define RCL_RET_WAIT_SET_FULL 902
+// rcl argument parsing specific ret codes in 1XXX
+/// Argument is not a valid remap rule
+#define RCL_RET_INVALID_REMAP_RULE 1001
+
#endif // RCL__TYPES_H_
diff --git a/rcl/src/rcl/arguments.c b/rcl/src/rcl/arguments.c
new file mode 100644
index 0000000..4378ca1
--- /dev/null
+++ b/rcl/src/rcl/arguments.c
@@ -0,0 +1,403 @@
+// 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 "rcl/arguments.h"
+
+#include
+
+#include "./arguments_impl.h"
+#include "./remap_impl.h"
+#include "rcl/error_handling.h"
+#include "rcl/validate_topic_name.h"
+#include "rcutils/allocator.h"
+#include "rcutils/logging_macros.h"
+#include "rcutils/strdup.h"
+#include "rmw/validate_namespace.h"
+#include "rmw/validate_node_name.h"
+
+#if __cplusplus
+extern "C"
+{
+#endif
+
+// Instance of global arguments.
+static rcl_arguments_t __rcl_global_arguments;
+
+/// Parse an argument that may or may not be a remap rule.
+/// \param[in] arg the argument to parse
+/// \param[in] allocator an allocator to use
+/// \param[in,out] output_rule input a zero intialized rule, output a fully initialized one
+/// \return RCL_RET_OK if a valid rule was parsed, or
+/// \return RCL_RET_INVALID_REMAP_RULE if the argument is not a valid rule, or
+/// \return RCL_RET_BAD_ALLOC if an allocation failed, or
+/// \return RLC_RET_ERROR if an unspecified error occurred.
+/// \internal
+RCL_LOCAL
+rcl_ret_t
+_rcl_parse_remap_rule(
+ const char * arg,
+ rcl_allocator_t allocator,
+ rcl_remap_t * output_rule)
+{
+ size_t len_node_name = 0;
+ size_t len_match = 0;
+ size_t len_replacement = 0;
+
+ const char * separator = NULL;
+ const char * colon = NULL;
+ const char * match_begin = arg;
+ const char * replacement_begin = NULL;
+
+ // A valid rule has two parts separated by :=
+ separator = strstr(arg, ":=");
+ if (NULL == separator) {
+ RCL_SET_ERROR_MSG("missing :=", allocator);
+ return RCL_RET_INVALID_REMAP_RULE;
+ }
+
+ replacement_begin = separator + 2;
+
+ // must have characters on both sides of the separator
+ len_match = separator - arg;
+ len_replacement = strlen(replacement_begin);
+ if (0 == len_match) {
+ RCL_SET_ERROR_MSG("match is zero length", allocator);
+ return RCL_RET_INVALID_REMAP_RULE;
+ } else if (0 == len_replacement) {
+ RCL_SET_ERROR_MSG("replacement has zero length", allocator);
+ return RCL_RET_INVALID_REMAP_RULE;
+ }
+
+ colon = strchr(arg, ':');
+ if (NULL != colon) {
+ if (colon < separator) {
+ // If there is a : on the match side then there is a node-name prefix
+ match_begin = colon + 1;
+ len_node_name = colon - arg;
+ len_match = separator - match_begin;
+ // node name must have at least one character
+ if (len_node_name <= 0) {
+ RCL_SET_ERROR_MSG("node name previx has zero length", allocator);
+ return RCL_RET_INVALID_REMAP_RULE;
+ }
+ } else if (colon > separator) {
+ // If the colon is on the replacement side then this couldn't be a valid rule
+ RCL_SET_ERROR_MSG("replacement side cannot contain a :", allocator);
+ return RCL_RET_INVALID_REMAP_RULE;
+ }
+ }
+
+ // Maybe match length changed because there was a node name prefix
+ if (0 == len_match) {
+ RCL_SET_ERROR_MSG("match is zero length", allocator);
+ return RCL_RET_INVALID_REMAP_RULE;
+ }
+
+ // Make sure node name contains only valid characters
+ if (len_node_name) {
+ int validation_result;
+ size_t invalid_index;
+ if (
+ RMW_RET_OK != rmw_validate_node_name_with_size(arg, len_node_name, &validation_result,
+ &invalid_index))
+ {
+ RCL_SET_ERROR_MSG("failed to run check on node name", allocator);
+ return RCL_RET_ERROR;
+ }
+ if (RMW_NODE_NAME_VALID != validation_result) {
+ RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(
+ allocator,
+ "node name prefix invalid: %s", rmw_node_name_validation_result_string(validation_result));
+ return RCL_RET_INVALID_REMAP_RULE;
+ }
+ }
+
+ // Figure out what type of rule this is, default is to apply to topic and service names
+ rcl_remap_type_t type = RCL_TOPIC_REMAP | RCL_SERVICE_REMAP;
+ if (0 == strncmp("__ns", match_begin, len_match)) {
+ type = RCL_NAMESPACE_REMAP;
+ } else if (0 == strncmp("__node", match_begin, len_match)) {
+ type = RCL_NODENAME_REMAP;
+ }
+
+ if (type & (RCL_TOPIC_REMAP | RCL_SERVICE_REMAP)) {
+ // Replacement must be a valid topic name
+ int validation_result;
+ size_t invalid_index;
+ rcl_ret_t ret = rcl_validate_topic_name(replacement_begin, &validation_result, &invalid_index);
+ if (ret != RCL_RET_OK) {
+ return RCL_RET_ERROR;
+ } else if (validation_result != RCL_TOPIC_NAME_VALID) {
+ RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(
+ allocator,
+ "replacement is invalid: %s", rcl_topic_name_validation_result_string(validation_result));
+ return RCL_RET_INVALID_REMAP_RULE;
+ }
+ // Match must be a valid topic name
+ ret = rcl_validate_topic_name_with_size(
+ match_begin, len_match, &validation_result, &invalid_index);
+ if (ret != RCL_RET_OK) {
+ return RCL_RET_ERROR;
+ } else if (validation_result != RCL_TOPIC_NAME_VALID) {
+ RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(
+ allocator,
+ "match is invalid: %s", rcl_topic_name_validation_result_string(validation_result));
+ return RCL_RET_INVALID_REMAP_RULE;
+ }
+ } else if (RCL_NAMESPACE_REMAP == type) {
+ int validation_result;
+ size_t invalid_idx;
+ if (RMW_RET_OK != rmw_validate_namespace(replacement_begin, &validation_result, &invalid_idx)) {
+ RCL_SET_ERROR_MSG("failed to run check on namespace", allocator);
+ return RCL_RET_ERROR;
+ }
+ if (RMW_NAMESPACE_VALID != validation_result) {
+ RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(
+ allocator,
+ "namespace is invalid: %s", rmw_namespace_validation_result_string(validation_result));
+ return RCL_RET_INVALID_REMAP_RULE;
+ }
+ } else if (RCL_NODENAME_REMAP == type) {
+ int validation_result;
+ size_t invalid_idx;
+ if (RMW_RET_OK != rmw_validate_node_name(replacement_begin, &validation_result, &invalid_idx)) {
+ RCL_SET_ERROR_MSG("failed to run check on node name", allocator);
+ return RCL_RET_ERROR;
+ }
+ if (RMW_NODE_NAME_VALID != validation_result) {
+ RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(
+ allocator,
+ "node name is invalid: %s", rmw_node_name_validation_result_string(validation_result));
+ return RCL_RET_INVALID_REMAP_RULE;
+ }
+ }
+
+ // Rule is valid, construct a structure for it
+ output_rule->allocator = allocator;
+ output_rule->type = type;
+ if (len_node_name > 0) {
+ output_rule->node_name = rcutils_strndup(arg, len_node_name, allocator);
+ if (NULL == output_rule->node_name) {
+ goto cleanup_rule;
+ }
+ }
+ if (type & (RCL_TOPIC_REMAP | RCL_SERVICE_REMAP)) {
+ output_rule->match = rcutils_strndup(match_begin, len_match, allocator);
+ if (NULL == output_rule->match) {
+ goto cleanup_rule;
+ }
+ }
+ output_rule->replacement = rcutils_strndup(replacement_begin, len_replacement, allocator);
+ if (NULL == output_rule->replacement) {
+ goto cleanup_rule;
+ }
+ return RCL_RET_OK;
+
+cleanup_rule:
+ if (RCL_RET_OK != rcl_remap_fini(output_rule)) {
+ RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to fini remap rule after error occurred");
+ }
+ return RCL_RET_BAD_ALLOC;
+}
+
+rcl_ret_t
+rcl_parse_arguments(
+ int argc,
+ const char * const argv[],
+ rcl_allocator_t allocator,
+ rcl_arguments_t * args_output)
+{
+ RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+ if (argc < 0) {
+ return RCL_RET_INVALID_ARGUMENT;
+ } else if (argc > 0) {
+ RCL_CHECK_ARGUMENT_FOR_NULL(argv, RCL_RET_INVALID_ARGUMENT, allocator);
+ }
+ RCL_CHECK_ARGUMENT_FOR_NULL(args_output, RCL_RET_INVALID_ARGUMENT, allocator);
+
+ rcl_ret_t ret;
+ rcl_ret_t fail_ret;
+
+ args_output->impl = allocator.allocate(sizeof(rcl_arguments_impl_t), allocator.state);
+ if (NULL == args_output->impl) {
+ return RCL_RET_BAD_ALLOC;
+ }
+ rcl_arguments_impl_t * args_impl = args_output->impl;
+ args_impl->num_remap_rules = 0;
+ args_impl->remap_rules = NULL;
+ args_impl->unparsed_args = NULL;
+ args_impl->num_unparsed_args = 0;
+ args_impl->allocator = allocator;
+
+ if (argc == 0) {
+ // there are no arguments to parse
+ return RCL_RET_OK;
+ }
+
+ // over-allocate arrays to match the number of arguments
+ args_impl->remap_rules = allocator.allocate(sizeof(rcl_remap_t) * argc, allocator.state);
+ if (NULL == args_impl->remap_rules) {
+ ret = RCL_RET_BAD_ALLOC;
+ goto fail;
+ }
+ args_impl->unparsed_args = allocator.allocate(sizeof(int) * argc, allocator.state);
+ if (NULL == args_impl->unparsed_args) {
+ ret = RCL_RET_BAD_ALLOC;
+ goto fail;
+ }
+
+ // Attempt to parse arguments as remap rules
+ for (int i = 0; i < argc; ++i) {
+ rcl_remap_t * rule = &(args_impl->remap_rules[args_impl->num_remap_rules]);
+ *rule = rcl_remap_get_zero_initialized();
+ if (RCL_RET_OK == _rcl_parse_remap_rule(argv[i], allocator, rule)) {
+ ++(args_impl->num_remap_rules);
+ } else {
+ RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "arg %d error '%s'", i, rcl_get_error_string());
+ rcl_reset_error();
+ args_impl->unparsed_args[args_impl->num_unparsed_args] = i;
+ ++(args_impl->num_unparsed_args);
+ }
+ }
+
+ // Shrink remap_rules array to match number of successfully parsed rules
+ if (args_impl->num_remap_rules > 0) {
+ args_impl->remap_rules = rcutils_reallocf(
+ args_impl->remap_rules, sizeof(rcl_remap_t) * args_impl->num_remap_rules, &allocator);
+ if (NULL == args_impl->remap_rules) {
+ ret = RCL_RET_BAD_ALLOC;
+ goto fail;
+ }
+ } else {
+ // No remap rules
+ allocator.deallocate(args_impl->remap_rules, allocator.state);
+ args_impl->remap_rules = NULL;
+ }
+ // Shrink unparsed_args
+ if (0 == args_impl->num_unparsed_args) {
+ // No unparsed args
+ allocator.deallocate(args_impl->unparsed_args, allocator.state);
+ args_impl->unparsed_args = NULL;
+ } else if (args_impl->num_unparsed_args < argc) {
+ args_impl->unparsed_args = rcutils_reallocf(
+ args_impl->unparsed_args, sizeof(int) * args_impl->num_unparsed_args, &allocator);
+ if (NULL == args_impl->unparsed_args) {
+ ret = RCL_RET_BAD_ALLOC;
+ goto fail;
+ }
+ }
+
+ return RCL_RET_OK;
+fail:
+ fail_ret = ret;
+ if (NULL != args_impl) {
+ ret = rcl_arguments_fini(args_output);
+ if (RCL_RET_OK != ret) {
+ RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to fini arguments after earlier failure");
+ }
+ }
+ return fail_ret;
+}
+
+int
+rcl_arguments_get_count_unparsed(
+ rcl_arguments_t * args)
+{
+ if (NULL == args || NULL == args->impl) {
+ return -1;
+ }
+ return args->impl->num_unparsed_args;
+}
+
+rcl_ret_t
+rcl_arguments_get_unparsed(
+ rcl_arguments_t * args,
+ rcl_allocator_t allocator,
+ int ** output_unparsed_indices)
+{
+ RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+ RCL_CHECK_ARGUMENT_FOR_NULL(args, RCL_RET_INVALID_ARGUMENT, allocator);
+ RCL_CHECK_ARGUMENT_FOR_NULL(args->impl, RCL_RET_INVALID_ARGUMENT, allocator);
+ RCL_CHECK_ARGUMENT_FOR_NULL(output_unparsed_indices, RCL_RET_INVALID_ARGUMENT, allocator);
+
+ *output_unparsed_indices = NULL;
+ if (args->impl->num_unparsed_args) {
+ *output_unparsed_indices = allocator.allocate(
+ sizeof(int) * args->impl->num_unparsed_args, allocator.state);
+ if (NULL == *output_unparsed_indices) {
+ return RCL_RET_BAD_ALLOC;
+ }
+ for (int i = 0; i < args->impl->num_unparsed_args; ++i) {
+ (*output_unparsed_indices)[i] = args->impl->unparsed_args[i];
+ }
+ }
+ return RCL_RET_OK;
+}
+
+rcl_arguments_t
+rcl_get_zero_initialized_arguments(void)
+{
+ static rcl_arguments_t default_arguments = {
+ .impl = NULL
+ };
+ return default_arguments;
+}
+
+rcl_ret_t
+rcl_arguments_fini(
+ rcl_arguments_t * args)
+{
+ rcl_allocator_t alloc = rcl_get_default_allocator();
+ RCL_CHECK_ARGUMENT_FOR_NULL(args, RCL_RET_INVALID_ARGUMENT, alloc);
+ if (args->impl) {
+ rcl_ret_t ret = RCL_RET_OK;
+ alloc = args->impl->allocator;
+ if (args->impl->remap_rules) {
+ for (int i = 0; i < args->impl->num_remap_rules; ++i) {
+ rcl_ret_t remap_ret = rcl_remap_fini(&(args->impl->remap_rules[i]));
+ if (remap_ret != RCL_RET_OK) {
+ ret = remap_ret;
+ RCUTILS_LOG_ERROR_NAMED(
+ ROS_PACKAGE_NAME,
+ "Failed to finalize remap rule while finalizing arguments. Continuing...");
+ }
+ }
+ args->impl->allocator.deallocate(args->impl->remap_rules, args->impl->allocator.state);
+ args->impl->remap_rules = NULL;
+ args->impl->num_remap_rules = 0;
+ }
+
+ args->impl->allocator.deallocate(args->impl->unparsed_args, args->impl->allocator.state);
+ args->impl->num_unparsed_args = 0;
+ args->impl->unparsed_args = NULL;
+
+ args->impl->allocator.deallocate(args->impl, args->impl->allocator.state);
+ args->impl = NULL;
+ return ret;
+ }
+ RCL_SET_ERROR_MSG("rcl_arguments_t finalized twice", alloc);
+ return RCL_RET_ERROR;
+}
+
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_arguments_t *
+rcl_get_global_arguments()
+{
+ return &__rcl_global_arguments;
+}
+
+#if __cplusplus
+}
+#endif
diff --git a/rcl/src/rcl/arguments_impl.h b/rcl/src/rcl/arguments_impl.h
new file mode 100644
index 0000000..5e7e94b
--- /dev/null
+++ b/rcl/src/rcl/arguments_impl.h
@@ -0,0 +1,47 @@
+// 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.
+
+#ifndef RCL__ARGUMENTS_IMPL_H_
+#define RCL__ARGUMENTS_IMPL_H_
+
+#include "rcl/arguments.h"
+#include "./remap_impl.h"
+
+#if __cplusplus
+extern "C"
+{
+#endif
+
+/// \internal
+typedef struct rcl_arguments_impl_t
+{
+ /// Array of indices that were not valid ROS arguments.
+ int * unparsed_args;
+ /// Length of unparsed_args.
+ int num_unparsed_args;
+
+ /// Array of rules for name remapping.
+ rcl_remap_t * remap_rules;
+ /// Length of remap_rules.
+ int num_remap_rules;
+
+ /// Allocator used to allocate objects in this struct
+ rcl_allocator_t allocator;
+} rcl_arguments_impl_t;
+
+#if __cplusplus
+}
+#endif
+
+#endif // RCL__ARGUMENTS_IMPL_H_
diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c
index 1330b91..66a46c6 100644
--- a/rcl/src/rcl/client.c
+++ b/rcl/src/rcl/client.c
@@ -24,6 +24,7 @@ extern "C"
#include "rcl/error_handling.h"
#include "rcl/expand_topic_name.h"
+#include "rcl/remap.h"
#include "rcutils/logging_macros.h"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
@@ -100,6 +101,7 @@ rcl_client_init(
return RCL_RET_ERROR;
}
char * expanded_service_name = NULL;
+ char * remapped_service_name = NULL;
ret = rcl_expand_topic_name(
service_name,
rcl_node_get_name(node),
@@ -114,40 +116,60 @@ rcl_client_init(
return RCL_RET_ERROR;
}
if (ret != RCL_RET_OK) {
- if (ret == RCL_RET_BAD_ALLOC) {
- return ret;
- } else if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) {
- return RCL_RET_SERVICE_NAME_INVALID;
+ if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) {
+ ret = RCL_RET_SERVICE_NAME_INVALID;
} else {
- return RCL_RET_ERROR;
+ 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 = rcl_get_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;
+ }
+
// Validate the expanded service name.
int validation_result;
- rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_service_name, &validation_result, NULL);
+ 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_safe(), *allocator);
- return RCL_RET_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), *allocator)
- return RCL_RET_SERVICE_NAME_INVALID;
+ 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);
RCL_CHECK_FOR_NULL_WITH_MSG(
- client->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, *allocator);
+ client->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup, *allocator);
// Fill out implementation struct.
// rmw handle (create rmw client)
// TODO(wjwwood): pass along the allocator to rmw when it supports it
client->impl->rmw_handle = rmw_create_client(
rcl_node_get_rmw_handle(node),
type_support,
- expanded_service_name,
+ remapped_service_name,
&options->qos);
- allocator->deallocate(expanded_service_name, allocator->state);
if (!client->impl->rmw_handle) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator);
goto fail;
@@ -155,12 +177,22 @@ rcl_client_init(
// options
client->impl->options = *options;
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client initialized")
- return RCL_RET_OK;
+ ret = RCL_RET_OK;
+ goto cleanup;
fail:
if (client->impl) {
allocator->deallocate(client->impl, allocator->state);
}
- return fail_ret;
+ 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);
+ }
+ return ret;
}
rcl_ret_t
diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c
index 7834bfc..0f0e294 100644
--- a/rcl/src/rcl/node.c
+++ b/rcl/src/rcl/node.c
@@ -24,8 +24,10 @@ extern "C"
#include
#include
+#include "rcl/arguments.h"
#include "rcl/error_handling.h"
#include "rcl/rcl.h"
+#include "rcl/remap.h"
#include "rcutils/filesystem.h"
#include "rcutils/find.h"
#include "rcutils/format_string.h"
@@ -142,6 +144,7 @@ rcl_node_init(
rcl_guard_condition_get_default_options();
rcl_ret_t ret;
rcl_ret_t fail_ret = RCL_RET_ERROR;
+ char * remapped_node_name = NULL;
// Check options and allocator first, so allocator can be used for errors.
RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
@@ -191,7 +194,7 @@ rcl_node_init(
// length + 2, because new leading / and terminating \0
char * temp = (char *)allocator->allocate(namespace_length + 2, allocator->state);
RCL_CHECK_FOR_NULL_WITH_MSG(
- temp, "allocating memory failed", return RCL_RET_BAD_ALLOC, *allocator);
+ temp, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup, *allocator);
temp[0] = '/';
memcpy(temp + 1, namespace_, strlen(namespace_) + 1);
local_namespace_ = temp;
@@ -202,34 +205,54 @@ rcl_node_init(
ret = rmw_validate_namespace(local_namespace_, &validation_result, NULL);
if (ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator);
- if (should_free_local_namespace_) {
- allocator->deallocate((char *)local_namespace_, allocator->state);
- }
- return ret;
+ goto cleanup;
}
if (validation_result != RMW_NAMESPACE_VALID) {
const char * msg = rmw_namespace_validation_result_string(validation_result);
RCL_SET_ERROR_MSG_WITH_FORMAT_STRING((*allocator), "%s, result: %d", msg, validation_result);
- if (should_free_local_namespace_) {
- allocator->deallocate((char *)local_namespace_, allocator->state);
- }
- return RCL_RET_NODE_INVALID_NAMESPACE;
+ ret = RCL_RET_NODE_INVALID_NAMESPACE;
+ goto cleanup;
}
// Allocate space for the implementation struct.
node->impl = (rcl_node_impl_t *)allocator->allocate(sizeof(rcl_node_impl_t), allocator->state);
- if (!node->impl && should_free_local_namespace_) {
- allocator->deallocate((char *)local_namespace_, allocator->state);
- }
RCL_CHECK_FOR_NULL_WITH_MSG(
- node->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, *allocator);
+ node->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup, *allocator);
node->impl->rmw_node_handle = NULL;
node->impl->graph_guard_condition = NULL;
+ node->impl->logger_name = NULL;
// Initialize node impl.
// node options (assume it is trivially copyable)
node->impl->options = *options;
+ // Remap the node name and namespace if remap rules are given
+ rcl_arguments_t * global_args = NULL;
+ if (node->impl->options.use_global_arguments) {
+ global_args = rcl_get_global_arguments();
+ }
+ ret = rcl_remap_node_name(
+ &(node->impl->options.arguments), global_args, name, *allocator,
+ &remapped_node_name);
+ if (RCL_RET_OK != ret) {
+ goto fail;
+ } else if (NULL != remapped_node_name) {
+ name = remapped_node_name;
+ }
+ char * remapped_namespace = NULL;
+ ret = rcl_remap_node_namespace(
+ &(node->impl->options.arguments), global_args, local_namespace_,
+ *allocator, &remapped_namespace);
+ if (RCL_RET_OK != ret) {
+ goto fail;
+ } else if (NULL != remapped_namespace) {
+ if (should_free_local_namespace_) {
+ allocator->deallocate((char *)local_namespace_, allocator->state);
+ }
+ should_free_local_namespace_ = true;
+ local_namespace_ = remapped_namespace;
+ }
+
// node logger name
node->impl->logger_name = rcl_create_node_logger_name(name, local_namespace_, allocator);
RCL_CHECK_FOR_NULL_WITH_MSG(
@@ -264,10 +287,8 @@ rcl_node_init(
RCL_SET_ERROR_MSG(
"Environment variable " RCUTILS_STRINGIFY(ROS_SECURITY_ENABLE_VAR_NAME)
" could not be read", rcl_get_default_allocator());
- if (should_free_local_namespace_) {
- allocator->deallocate((char *)local_namespace_, allocator->state);
- }
- return RCL_RET_ERROR;
+ ret = RCL_RET_ERROR;
+ goto fail;
}
bool use_security = (0 == strcmp(ros_security_enable, "true"));
@@ -278,10 +299,8 @@ rcl_node_init(
RCL_SET_ERROR_MSG(
"Environment variable " RCUTILS_STRINGIFY(ROS_SECURITY_STRATEGY_VAR_NAME)
" could not be read", rcl_get_default_allocator());
- if (should_free_local_namespace_) {
- allocator->deallocate((char *)local_namespace_, allocator->state);
- }
- return RCL_RET_ERROR;
+ ret = RCL_RET_ERROR;
+ goto fail;
}
rmw_node_security_options_t node_security_options =
@@ -302,10 +321,8 @@ rcl_node_init(
"SECURITY ERROR: unable to find a folder matching the node name in the "
RCUTILS_STRINGIFY(ROS_SECURITY_ROOT_DIRECTORY_VAR_NAME)
" directory while the requested security strategy requires it", *allocator);
- if (should_free_local_namespace_) {
- allocator->deallocate((char *)local_namespace_, allocator->state);
- }
- return RCL_RET_ERROR;
+ ret = RCL_RET_ERROR;
+ goto cleanup;
}
}
}
@@ -314,11 +331,6 @@ rcl_node_init(
RCL_CHECK_FOR_NULL_WITH_MSG(
node->impl->rmw_node_handle, rmw_get_error_string_safe(), goto fail, *allocator);
- // free local_namespace_ if necessary
- if (should_free_local_namespace_) {
- allocator->deallocate((char *)local_namespace_, allocator->state);
- should_free_local_namespace_ = false;
- }
// instance id
node->impl->rcl_instance_id = rcl_get_instance_id();
// graph guard condition
@@ -345,7 +357,8 @@ rcl_node_init(
goto fail;
}
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node initialized")
- return RCL_RET_OK;
+ ret = RCL_RET_OK;
+ goto cleanup;
fail:
if (node->impl) {
if (node->impl->logger_name) {
@@ -374,11 +387,17 @@ fail:
}
*node = rcl_get_zero_initialized_node();
+ ret = fail_ret;
+ // fall through from fail -> cleanup
+cleanup:
if (should_free_local_namespace_) {
allocator->deallocate((char *)local_namespace_, allocator->state);
local_namespace_ = NULL;
}
- return fail_ret;
+ if (NULL != remapped_node_name) {
+ allocator->deallocate(remapped_node_name, allocator->state);
+ }
+ return ret;
}
rcl_ret_t
@@ -435,9 +454,11 @@ rcl_node_get_default_options()
// !!! MAKE SURE THAT CHANGES TO THESE DEFAULTS ARE REFLECTED IN THE HEADER DOC STRING
static rcl_node_options_t default_options = {
.domain_id = RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID,
+ .use_global_arguments = true,
};
// Must set the allocator after because it is not a compile time constant.
default_options.allocator = rcl_get_default_allocator();
+ default_options.arguments = rcl_get_zero_initialized_arguments();
return default_options;
}
diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c
index 44bc367..4b737f9 100644
--- a/rcl/src/rcl/publisher.c
+++ b/rcl/src/rcl/publisher.c
@@ -25,6 +25,7 @@ extern "C"
#include "rcl/allocator.h"
#include "rcl/error_handling.h"
#include "rcl/expand_topic_name.h"
+#include "rcl/remap.h"
#include "rcutils/logging_macros.h"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
@@ -99,6 +100,7 @@ rcl_publisher_init(
return RCL_RET_ERROR;
}
char * expanded_topic_name = NULL;
+ char * remapped_topic_name = NULL;
ret = rcl_expand_topic_name(
topic_name,
rcl_node_get_name(node),
@@ -108,56 +110,85 @@ rcl_publisher_init(
&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_safe(), *allocator)
- allocator->deallocate(expanded_topic_name, allocator->state);
- return RCL_RET_ERROR;
+ RCL_SET_ERROR_MSG(rcutils_get_error_string_safe(), *allocator);
+ ret = RCL_RET_ERROR;
+ goto cleanup;
}
if (ret != RCL_RET_OK) {
- if (ret == RCL_RET_BAD_ALLOC) {
- return ret;
- } else if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) {
- return RCL_RET_TOPIC_NAME_INVALID;
+ if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) {
+ ret = RCL_RET_TOPIC_NAME_INVALID;
} else {
- return RCL_RET_ERROR;
+ 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 = rcl_get_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;
+ }
+
// Validate the expanded topic name.
int validation_result;
- rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_topic_name, &validation_result, NULL);
+ 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_safe(), *allocator);
- return RCL_RET_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), *allocator)
- return RCL_RET_TOPIC_NAME_INVALID;
+ 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);
RCL_CHECK_FOR_NULL_WITH_MSG(
- publisher->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, *allocator);
+ publisher->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup, *allocator);
// Fill out implementation struct.
// rmw handle (create rmw publisher)
// TODO(wjwwood): pass along the allocator to rmw when it supports it
publisher->impl->rmw_handle = rmw_create_publisher(
rcl_node_get_rmw_handle(node),
type_support,
- expanded_topic_name,
+ remapped_topic_name,
&(options->qos));
- allocator->deallocate(expanded_topic_name, allocator->state);
RCL_CHECK_FOR_NULL_WITH_MSG(publisher->impl->rmw_handle,
rmw_get_error_string_safe(), goto fail, *allocator);
// options
publisher->impl->options = *options;
- RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized")
- return RCL_RET_OK;
+ RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized");
+ goto cleanup;
fail:
if (publisher->impl) {
allocator->deallocate(publisher->impl, allocator->state);
}
- return fail_ret;
+ 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);
+ }
+ return ret;
}
rcl_ret_t
diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c
index c0cb873..c998d48 100644
--- a/rcl/src/rcl/rcl.c
+++ b/rcl/src/rcl/rcl.c
@@ -21,7 +21,9 @@ extern "C"
#include
+#include "./arguments_impl.h"
#include "./stdatomic_helper.h"
+#include "rcl/arguments.h"
#include "rcl/error_handling.h"
#include "rcutils/logging_macros.h"
#include "rmw/error_handling.h"
@@ -49,6 +51,11 @@ __clean_up_init()
}
__rcl_argc = 0;
__rcl_argv = NULL;
+ // This is the only place where it is OK to finalize the global arguments.
+ rcl_arguments_t * global_args = rcl_get_global_arguments();
+ if (NULL != global_args->impl && RCL_RET_OK != rcl_arguments_fini(global_args)) {
+ rcl_reset_error();
+ }
rcl_atomic_store(&__rcl_instance_id, 0);
rcl_atomic_store(&__rcl_is_initialized, false);
}
@@ -68,6 +75,11 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator)
RCL_SET_ERROR_MSG("rcl_init called while already initialized", allocator);
return RCL_RET_ALREADY_INIT;
}
+
+ // Zero initialize global arguments before any chance of calling __clean_up_init()
+ rcl_arguments_t * global_args = rcl_get_global_arguments();
+ *global_args = rcl_get_zero_initialized_arguments();
+
// There is a race condition between the time __rcl_is_initialized is set true,
// and when the allocator is set, in which rcl_shutdown() could get rcl_ok() as
// true and try to use the allocator, but it isn't set yet...
@@ -101,6 +113,10 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator)
}
memcpy(__rcl_argv[i], argv[i], strlen(argv[i]));
}
+ if (RCL_RET_OK != rcl_parse_arguments(argc, (const char **)argv, allocator, global_args)) {
+ RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to parse global arguments");
+ goto fail;
+ }
rcl_atomic_store(&__rcl_instance_id, ++__rcl_next_unique_id);
if (rcl_atomic_load_uint64_t(&__rcl_instance_id) == 0) {
// Roll over occurred.
diff --git a/rcl/src/rcl/remap.c b/rcl/src/rcl/remap.c
new file mode 100644
index 0000000..690d161
--- /dev/null
+++ b/rcl/src/rcl/remap.c
@@ -0,0 +1,279 @@
+// 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 "rcl/remap.h"
+
+#include "./arguments_impl.h"
+#include "./remap_impl.h"
+#include "rcl/error_handling.h"
+#include "rcl/expand_topic_name.h"
+#include "rcutils/allocator.h"
+#include "rcutils/strdup.h"
+#include "rcutils/types/string_map.h"
+
+#if __cplusplus
+extern "C"
+{
+#endif
+
+rcl_remap_t
+rcl_remap_get_zero_initialized()
+{
+ rcl_remap_t rule;
+ rule.type = RCL_UNKNOWN_REMAP;
+ rule.node_name = NULL;
+ rule.match = NULL;
+ rule.replacement = NULL;
+ rule.allocator = rcutils_get_zero_initialized_allocator();
+ return rule;
+}
+
+rcl_ret_t
+rcl_remap_fini(
+ rcl_remap_t * rule)
+{
+ if (NULL != rule->node_name) {
+ rule->allocator.deallocate(rule->node_name, rule->allocator.state);
+ rule->node_name = NULL;
+ }
+ if (NULL != rule->match) {
+ rule->allocator.deallocate(rule->match, rule->allocator.state);
+ rule->match = NULL;
+ }
+ if (NULL != rule->replacement) {
+ rule->allocator.deallocate(rule->replacement, rule->allocator.state);
+ rule->replacement = NULL;
+ }
+ rule->allocator = rcutils_get_zero_initialized_allocator();
+ return RCL_RET_OK;
+}
+
+/// Get the first matching rule in a chain.
+/// \return RCL_RET_OK if no errors occurred while searching for a rule
+RCL_LOCAL
+rcl_ret_t
+_rcl_remap_first_match(
+ rcl_remap_t * remap_rules,
+ int num_rules,
+ rcl_remap_type_t type_bitmask,
+ const char * name,
+ const char * node_name,
+ const char * node_namespace,
+ const rcutils_string_map_t * substitutions,
+ rcutils_allocator_t allocator,
+ rcl_remap_t ** output_rule)
+{
+ *output_rule = NULL;
+ for (int i = 0; i < num_rules; ++i) {
+ rcl_remap_t * rule = &(remap_rules[i]);
+ if (!(rule->type & type_bitmask)) {
+ // Not the type of remap rule we're looking fore
+ continue;
+ }
+ if (rule->node_name != NULL && 0 != strcmp(rule->node_name, node_name)) {
+ // Rule has a node name prefix and the supplied node name didn't match
+ continue;
+ }
+ bool matched = false;
+ if (rule->type & (RCL_TOPIC_REMAP | RCL_SERVICE_REMAP)) {
+ // topic and service rules need the match side to be expanded to a FQN
+ char * expanded_match = NULL;
+ rcl_ret_t ret = rcl_expand_topic_name(
+ rule->match, node_name, node_namespace, substitutions, allocator, &expanded_match);
+ if (RCL_RET_OK != ret) {
+ rcl_reset_error();
+ if (
+ RCL_RET_NODE_INVALID_NAMESPACE == ret ||
+ RCL_RET_NODE_INVALID_NAME == ret ||
+ RCL_RET_BAD_ALLOC == ret)
+ {
+ // these are probably going to happen again. Stop processing rules
+ return ret;
+ }
+ continue;
+ }
+ matched = (0 == strcmp(expanded_match, name));
+ allocator.deallocate(expanded_match, allocator.state);
+ } else {
+ // nodename and namespace replacement apply if the type and node name prefix checks passed
+ matched = true;
+ }
+ if (matched) {
+ *output_rule = rule;
+ break;
+ }
+ }
+ return RCL_RET_OK;
+}
+
+/// Remap from one name to another using rules matching a given type bitmask.
+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)
+{
+ RCL_CHECK_ARGUMENT_FOR_NULL(node_name, RCL_RET_INVALID_ARGUMENT, allocator);
+ RCL_CHECK_ARGUMENT_FOR_NULL(output_name, RCL_RET_INVALID_ARGUMENT, allocator);
+ if (NULL != local_arguments && NULL == local_arguments->impl) {
+ local_arguments = NULL;
+ }
+ if (NULL != global_arguments && NULL == global_arguments->impl) {
+ global_arguments = NULL;
+ }
+ if (NULL == local_arguments && NULL == global_arguments) {
+ RCL_SET_ERROR_MSG("local_arguments invalid and not using global arguments", allocator);
+ return RCL_RET_INVALID_ARGUMENT;
+ }
+
+ *output_name = NULL;
+ rcl_remap_t * rule = NULL;
+
+ // Look at local rules first
+ if (NULL != local_arguments) {
+ 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) {
+ return ret;
+ }
+ }
+ // Check global rules if no local rule matched
+ if (NULL == rule && NULL != global_arguments) {
+ 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) {
+ return ret;
+ }
+ }
+ // Do the remapping
+ if (NULL != rule) {
+ if (rule->type & (RCL_TOPIC_REMAP | RCL_SERVICE_REMAP)) {
+ // topic and service rules need the replacement to be expanded to a FQN
+ rcl_ret_t ret = rcl_expand_topic_name(
+ rule->replacement, node_name, node_namespace, substitutions, allocator, output_name);
+ if (RCL_RET_OK != ret) {
+ return ret;
+ }
+ } else {
+ // nodename and namespace rules don't need replacment expanded
+ *output_name = rcutils_strdup(rule->replacement, allocator);
+ }
+ if (NULL == *output_name) {
+ RCL_SET_ERROR_MSG("Failed to set output", allocator);
+ return RCL_RET_ERROR;
+ }
+ }
+ return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_remap_topic_name(
+ const rcl_arguments_t * local_arguments,
+ const rcl_arguments_t * global_arguments,
+ const char * topic_name,
+ const char * node_name,
+ const char * node_namespace,
+ rcl_allocator_t allocator,
+ char ** output_name)
+{
+ RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_INVALID_ARGUMENT);
+ RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT, allocator);
+
+ rcutils_string_map_t substitutions = rcutils_get_zero_initialized_string_map();
+ rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions, 0, allocator);
+ rcl_ret_t ret = RCL_RET_ERROR;
+ if (RCUTILS_RET_OK == rcutils_ret) {
+ ret = rcl_get_default_topic_name_substitutions(&substitutions);
+ if (RCL_RET_OK == ret) {
+ ret = _rcl_remap_name(
+ local_arguments, global_arguments, RCL_TOPIC_REMAP, topic_name, node_name,
+ node_namespace, &substitutions, allocator, output_name);
+ }
+ }
+ if (RCUTILS_RET_OK != rcutils_string_map_fini(&substitutions)) {
+ return RCL_RET_ERROR;
+ }
+ return ret;
+}
+
+rcl_ret_t
+rcl_remap_service_name(
+ const rcl_arguments_t * local_arguments,
+ const rcl_arguments_t * global_arguments,
+ const char * service_name,
+ const char * node_name,
+ const char * node_namespace,
+ rcl_allocator_t allocator,
+ char ** output_name)
+{
+ RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_INVALID_ARGUMENT);
+ RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT, allocator);
+
+ rcutils_string_map_t substitutions = rcutils_get_zero_initialized_string_map();
+ rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions, 0, allocator);
+ rcl_ret_t ret = RCL_RET_ERROR;
+ if (rcutils_ret == RCUTILS_RET_OK) {
+ ret = rcl_get_default_topic_name_substitutions(&substitutions);
+ if (ret == RCL_RET_OK) {
+ ret = _rcl_remap_name(
+ local_arguments, global_arguments, RCL_SERVICE_REMAP, service_name, node_name,
+ node_namespace, &substitutions, allocator, output_name);
+ }
+ }
+ if (RCUTILS_RET_OK != rcutils_string_map_fini(&substitutions)) {
+ return RCL_RET_ERROR;
+ }
+ return ret;
+}
+
+rcl_ret_t
+rcl_remap_node_name(
+ const rcl_arguments_t * local_arguments,
+ const rcl_arguments_t * global_arguments,
+ const char * node_name,
+ rcl_allocator_t allocator,
+ char ** output_name)
+{
+ RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_INVALID_ARGUMENT);
+ return _rcl_remap_name(
+ local_arguments, global_arguments, RCL_NODENAME_REMAP, NULL, node_name, NULL, NULL,
+ allocator, output_name);
+}
+
+rcl_ret_t
+rcl_remap_node_namespace(
+ const rcl_arguments_t * local_arguments,
+ const rcl_arguments_t * global_arguments,
+ const char * node_name,
+ rcl_allocator_t allocator,
+ char ** output_namespace)
+{
+ RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_INVALID_ARGUMENT);
+ return _rcl_remap_name(
+ local_arguments, global_arguments, RCL_NAMESPACE_REMAP, NULL, node_name, NULL, NULL,
+ allocator, output_namespace);
+}
+
+#if __cplusplus
+}
+#endif
diff --git a/rcl/src/rcl/remap_impl.h b/rcl/src/rcl/remap_impl.h
new file mode 100644
index 0000000..cb073a9
--- /dev/null
+++ b/rcl/src/rcl/remap_impl.h
@@ -0,0 +1,80 @@
+// 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.
+
+#ifndef RCL__REMAP_IMPL_H_
+#define RCL__REMAP_IMPL_H_
+
+#include "rcl/types.h"
+
+#if __cplusplus
+extern "C"
+{
+#endif
+
+/// Enum doubles as a bitmask for rule sthat apply to both topics and services.
+typedef enum rcl_remap_type_t
+{
+ RCL_UNKNOWN_REMAP = 0,
+ RCL_TOPIC_REMAP = 1u << 0,
+ RCL_SERVICE_REMAP = 1u << 1,
+ RCL_NODENAME_REMAP = 1u << 2,
+ RCL_NAMESPACE_REMAP = 1u << 3
+} rcl_remap_type_t;
+
+typedef struct rcl_remap_t
+{
+ /// Bitmask indicating what type of rule this is.
+ rcl_remap_type_t type;
+ /// A node name that this rule is limited to, or NULL if it applies to any node.
+ char * node_name;
+ /// Match portion of a rule, or NULL if node name or namespace replacement.
+ char * match;
+ /// Replacement portion of a rule.
+ char * replacement;
+
+ /// Allocator used to allocate objects in this struct
+ rcl_allocator_t allocator;
+} rcl_remap_t;
+
+/// Get an rcl_remap_t structure initialized with NULL.
+rcl_remap_t
+rcl_remap_get_zero_initialized();
+
+/// Reclaim resources used in an rcl_remap_t structure.
+/**
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | No
+ * Thread-Safe | Yes
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ *
+ * \param[in] rule A rule to deallocate back to a zero initialized state.
+ * \return `RCL_RET_OK` if the structure was free'd, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or
+ * \return `RCL_RET_NODE_INVALID_NAME` if the name is invalid, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_remap_fini(
+ rcl_remap_t * rule);
+
+#if __cplusplus
+}
+#endif
+
+#endif // RCL__REMAP_IMPL_H_
diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c
index db46edd..c2195c6 100644
--- a/rcl/src/rcl/service.c
+++ b/rcl/src/rcl/service.c
@@ -24,6 +24,7 @@ extern "C"
#include "rcl/error_handling.h"
#include "rcl/expand_topic_name.h"
+#include "rcl/remap.h"
#include "rcutils/logging_macros.h"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
@@ -97,6 +98,7 @@ rcl_service_init(
return RCL_RET_ERROR;
}
char * expanded_service_name = NULL;
+ char * remapped_service_name = NULL;
ret = rcl_expand_topic_name(
service_name,
rcl_node_get_name(node),
@@ -107,35 +109,57 @@ rcl_service_init(
rcutils_ret = rcutils_string_map_fini(&substitutions_map);
if (rcutils_ret != RCUTILS_RET_OK) {
RCL_SET_ERROR_MSG(rcutils_get_error_string_safe(), *allocator)
- allocator->deallocate(expanded_service_name, allocator->state);
+ ret = RCL_RET_ERROR;
+ goto cleanup;
return RCL_RET_ERROR;
}
if (ret != RCL_RET_OK) {
- if (ret == RCL_RET_BAD_ALLOC) {
- return ret;
- } else if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) {
- return RCL_RET_SERVICE_NAME_INVALID;
+ if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) {
+ ret = RCL_RET_SERVICE_NAME_INVALID;
} else {
- return RCL_RET_ERROR;
+ 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 = rcl_get_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;
+ }
+
// Validate the expanded service name.
int validation_result;
- rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_service_name, &validation_result, NULL);
+ 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_safe(), *allocator);
- return RCL_RET_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), *allocator)
- return RCL_RET_SERVICE_NAME_INVALID;
+ 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);
RCL_CHECK_FOR_NULL_WITH_MSG(
- service->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, *allocator);
+ service->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup, *allocator);
if (RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL == options->qos.durability) {
RCUTILS_LOG_WARN_NAMED(
@@ -149,9 +173,8 @@ rcl_service_init(
service->impl->rmw_handle = rmw_create_service(
rcl_node_get_rmw_handle(node),
type_support,
- expanded_service_name,
+ remapped_service_name,
&options->qos);
- allocator->deallocate(expanded_service_name, allocator->state);
if (!service->impl->rmw_handle) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator);
goto fail;
@@ -159,12 +182,22 @@ rcl_service_init(
// options
service->impl->options = *options;
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service initialized")
- return RCL_RET_OK;
+ ret = RCL_RET_OK;
+ goto cleanup;
fail:
if (service->impl) {
allocator->deallocate(service->impl, allocator->state);
}
- return fail_ret;
+ 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);
+ }
+ return ret;
}
rcl_ret_t
diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c
index b3d4d37..de83cff 100644
--- a/rcl/src/rcl/subscription.c
+++ b/rcl/src/rcl/subscription.c
@@ -23,6 +23,7 @@ extern "C"
#include "rcl/error_handling.h"
#include "rcl/expand_topic_name.h"
+#include "rcl/remap.h"
#include "rcutils/logging_macros.h"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
@@ -96,6 +97,7 @@ rcl_subscription_init(
return RCL_RET_ERROR;
}
char * expanded_topic_name = NULL;
+ char * remapped_topic_name = NULL;
ret = rcl_expand_topic_name(
topic_name,
rcl_node_get_name(node),
@@ -106,45 +108,66 @@ rcl_subscription_init(
rcutils_ret = rcutils_string_map_fini(&substitutions_map);
if (rcutils_ret != RCUTILS_RET_OK) {
RCL_SET_ERROR_MSG(rcutils_get_error_string_safe(), *allocator)
- allocator->deallocate(expanded_topic_name, allocator->state);
- return RCL_RET_ERROR;
+ ret = RCL_RET_ERROR;
+ goto cleanup;
}
if (ret != RCL_RET_OK) {
- if (ret == RCL_RET_BAD_ALLOC) {
- return ret;
- } else if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) {
- return RCL_RET_TOPIC_NAME_INVALID;
+ if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) {
+ ret = RCL_RET_TOPIC_NAME_INVALID;
} else {
- return RCL_RET_ERROR;
+ 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 = rcl_get_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;
+ }
+
// Validate the expanded topic name.
int validation_result;
- rmw_ret_t rmw_ret = rmw_validate_full_topic_name(expanded_topic_name, &validation_result, NULL);
+ 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_safe(), *allocator);
- return RCL_RET_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), *allocator)
- return RCL_RET_TOPIC_NAME_INVALID;
+ 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);
RCL_CHECK_FOR_NULL_WITH_MSG(
- subscription->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, *allocator);
+ subscription->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup,
+ *allocator);
// Fill out the implemenation struct.
// rmw_handle
// TODO(wjwwood): pass allocator once supported in rmw api.
subscription->impl->rmw_handle = rmw_create_subscription(
rcl_node_get_rmw_handle(node),
type_support,
- expanded_topic_name,
+ remapped_topic_name,
&(options->qos),
options->ignore_local_publications);
- allocator->deallocate(expanded_topic_name, allocator->state);
if (!subscription->impl->rmw_handle) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator);
goto fail;
@@ -152,12 +175,22 @@ rcl_subscription_init(
// options
subscription->impl->options = *options;
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized")
- return RCL_RET_OK;
+ ret = RCL_RET_OK;
+ goto cleanup;
fail:
if (subscription->impl) {
allocator->deallocate(subscription->impl, allocator->state);
}
- return fail_ret;
+ 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);
+ }
+ return ret;
}
rcl_ret_t
diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt
index d12efcc..20a5a78 100644
--- a/rcl/test/CMakeLists.txt
+++ b/rcl/test/CMakeLists.txt
@@ -100,6 +100,29 @@ function(test_target_function)
AMENT_DEPENDENCIES ${rmw_implementation}
)
+ rcl_add_custom_gtest(test_arguments${target_suffix}
+ SRCS rcl/test_arguments.cpp
+ ENV ${extra_test_env}
+ APPEND_LIBRARY_DIRS ${extra_lib_dirs}
+ LIBRARIES ${PROJECT_NAME} ${extra_test_libraries}
+ )
+
+ rcl_add_custom_gtest(test_remap${target_suffix}
+ SRCS rcl/test_remap.cpp
+ ENV ${extra_test_env}
+ APPEND_LIBRARY_DIRS ${extra_lib_dirs}
+ LIBRARIES ${PROJECT_NAME} ${extra_test_libraries}
+ )
+
+ rcl_add_custom_gtest(test_remap_integration${target_suffix}
+ SRCS rcl/test_remap_integration.cpp
+ TIMEOUT 200
+ ENV ${extra_test_env}
+ APPEND_LIBRARY_DIRS ${extra_lib_dirs}
+ LIBRARIES ${PROJECT_NAME} ${extra_test_libraries}
+ AMENT_DEPENDENCIES "std_msgs" "example_interfaces"
+ )
+
rcl_add_custom_gtest(test_guard_condition${target_suffix}
SRCS rcl/test_guard_condition.cpp
ENV ${extra_test_env}
diff --git a/rcl/test/rcl/arg_macros.hpp b/rcl/test/rcl/arg_macros.hpp
new file mode 100644
index 0000000..0da4916
--- /dev/null
+++ b/rcl/test/rcl/arg_macros.hpp
@@ -0,0 +1,75 @@
+// 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.
+
+#ifndef RCL__ARG_MACROS_HPP_
+#define RCL__ARG_MACROS_HPP_
+
+#include "rcl/error_handling.h"
+#include "rcl/rcl.h"
+#include "rcutils/strdup.h"
+
+#include "../scope_exit.hpp"
+
+/// Helper to get around non-const args passed to rcl_init().
+char **
+copy_args(int argc, const char ** args)
+{
+ rcl_allocator_t allocator = rcl_get_default_allocator();
+ char ** copy = static_cast(allocator.allocate(sizeof(char *) * argc, allocator.state));
+ for (int i = 0; i < argc; ++i) {
+ copy[i] = rcutils_strdup(args[i], allocator);
+ }
+ return copy;
+}
+
+/// Destroy args allocated by copy_args.
+void
+destroy_args(int argc, char ** args)
+{
+ rcl_allocator_t allocator = rcl_get_default_allocator();
+ for (int i = 0; i < argc; ++i) {
+ allocator.deallocate(args[i], allocator.state);
+ }
+ allocator.deallocate(args, allocator.state);
+}
+
+#define SCOPE_GLOBAL_ARGS(argc, argv, ...) \
+ { \
+ const char * const_argv[] = {__VA_ARGS__}; \
+ argc = (sizeof(const_argv) / sizeof(const char *)); \
+ argv = copy_args(argc, const_argv); \
+ rcl_ret_t ret = rcl_init(argc, argv, rcl_get_default_allocator()); \
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); \
+ } \
+ auto __scope_global_args_exit = make_scope_exit( \
+ [argc, argv] { \
+ destroy_args(argc, argv); \
+ rcl_ret_t ret = rcl_shutdown(); \
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); \
+ })
+
+#define SCOPE_ARGS(local_arguments, ...) \
+ { \
+ const char * local_argv[] = {__VA_ARGS__}; \
+ unsigned int local_argc = (sizeof(local_argv) / sizeof(const char *)); \
+ rcl_ret_t ret = rcl_parse_arguments( \
+ local_argc, local_argv, rcl_get_default_allocator(), &local_arguments); \
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); \
+ } \
+ auto __scope_ ## local_arguments ## _exit = make_scope_exit( \
+ [&local_arguments] { \
+ ASSERT_EQ(RCL_RET_OK, rcl_arguments_fini(&local_arguments)); \
+ })
+
+#endif // RCL__ARG_MACROS_HPP_
diff --git a/rcl/test/rcl/test_arguments.cpp b/rcl/test/rcl/test_arguments.cpp
new file mode 100644
index 0000000..31aa00d
--- /dev/null
+++ b/rcl/test/rcl/test_arguments.cpp
@@ -0,0 +1,191 @@
+// 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
+
+#include "rcl/rcl.h"
+#include "rcl/arguments.h"
+
+#include "rcl/error_handling.h"
+
+#ifdef RMW_IMPLEMENTATION
+# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
+# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX)
+#else
+# define CLASSNAME(NAME, SUFFIX) NAME
+#endif
+
+class CLASSNAME (TestArgumentsFixture, RMW_IMPLEMENTATION) : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ }
+
+ void TearDown()
+ {
+ }
+};
+
+#define EXPECT_UNPARSED(parsed_args, ...) \
+ do { \
+ int expect_unparsed[] = {__VA_ARGS__}; \
+ int expect_num_unparsed = sizeof(expect_unparsed) / sizeof(int); \
+ rcl_allocator_t alloc = rcl_get_default_allocator(); \
+ int actual_num_unparsed = rcl_arguments_get_count_unparsed(&parsed_args); \
+ int * actual_unparsed = NULL; \
+ if (actual_num_unparsed > 0) { \
+ rcl_ret_t ret = rcl_arguments_get_unparsed(&parsed_args, alloc, &actual_unparsed); \
+ ASSERT_EQ(RCL_RET_OK, ret); \
+ } \
+ std::stringstream expected; \
+ expected << "["; \
+ for (int e = 0; e < expect_num_unparsed; ++e) { \
+ expected << expect_unparsed[e] << ", "; \
+ } \
+ expected << "]"; \
+ std::stringstream actual; \
+ actual << "["; \
+ for (int a = 0; a < actual_num_unparsed; ++a) { \
+ actual << actual_unparsed[a] << ", "; \
+ } \
+ actual << "]"; \
+ if (NULL != actual_unparsed) { \
+ alloc.deallocate(actual_unparsed, alloc.state); \
+ } \
+ EXPECT_STREQ(expected.str().c_str(), actual.str().c_str()); \
+ } while (0)
+
+bool
+is_valid_arg(const char * arg)
+{
+ const char * argv[] = {arg};
+ rcl_arguments_t parsed_args;
+ rcl_ret_t ret = rcl_parse_arguments(1, argv, rcl_get_default_allocator(), &parsed_args);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ bool is_valid = 0 == rcl_arguments_get_count_unparsed(&parsed_args);
+ EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
+ return is_valid;
+}
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), check_valid_vs_invalid_args) {
+ EXPECT_TRUE(is_valid_arg("__node:=node_name"));
+ EXPECT_TRUE(is_valid_arg("old_name:__node:=node_name"));
+ EXPECT_TRUE(is_valid_arg("old_name:__node:=nodename123"));
+ EXPECT_TRUE(is_valid_arg("__node:=nodename123"));
+ EXPECT_TRUE(is_valid_arg("__ns:=/foo/bar"));
+ EXPECT_TRUE(is_valid_arg("__ns:=/"));
+ EXPECT_TRUE(is_valid_arg("nodename:__ns:=/foobar"));
+ EXPECT_TRUE(is_valid_arg("foo:=bar"));
+ EXPECT_TRUE(is_valid_arg("~/foo:=~/bar"));
+ EXPECT_TRUE(is_valid_arg("/foo/bar:=bar"));
+ EXPECT_TRUE(is_valid_arg("foo:=/bar"));
+ EXPECT_TRUE(is_valid_arg("/foo123:=/bar123"));
+ EXPECT_TRUE(is_valid_arg("node:/foo123:=/bar123"));
+
+ EXPECT_FALSE(is_valid_arg(":="));
+ EXPECT_FALSE(is_valid_arg("foo:="));
+ EXPECT_FALSE(is_valid_arg(":=bar"));
+ EXPECT_FALSE(is_valid_arg("__ns:="));
+ EXPECT_FALSE(is_valid_arg("__node:="));
+ EXPECT_FALSE(is_valid_arg("__node:=/foo/bar"));
+ EXPECT_FALSE(is_valid_arg("__ns:=foo"));
+ EXPECT_FALSE(is_valid_arg(":__node:=nodename"));
+ EXPECT_FALSE(is_valid_arg("~:__node:=nodename"));
+ EXPECT_FALSE(is_valid_arg("}foo:=/bar"));
+ EXPECT_FALSE(is_valid_arg("f oo:=/bar"));
+ EXPECT_FALSE(is_valid_arg("foo:=/b ar"));
+ EXPECT_FALSE(is_valid_arg("f{oo:=/bar"));
+ EXPECT_FALSE(is_valid_arg("foo:=/b}ar"));
+}
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_no_args) {
+ rcl_arguments_t parsed_args;
+ rcl_ret_t ret = rcl_parse_arguments(0, NULL, rcl_get_default_allocator(), &parsed_args);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_EQ(0, rcl_arguments_get_count_unparsed(&parsed_args));
+ EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
+}
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_null_args) {
+ int argc = 1;
+ rcl_arguments_t parsed_args;
+ rcl_ret_t ret = rcl_parse_arguments(argc, NULL, rcl_get_default_allocator(), &parsed_args);
+ EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe();
+ rcl_reset_error();
+}
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_null_args_output) {
+ const char * argv[] = {"process_name"};
+ int argc = sizeof(argv) / sizeof(const char *);
+ rcl_ret_t ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), NULL);
+ EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe();
+ rcl_reset_error();
+}
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap) {
+ const char * argv[] = {"process_name", "/foo/bar:=/fiz/buz"};
+ int argc = sizeof(argv) / sizeof(const char *);
+ rcl_arguments_t parsed_args;
+ rcl_ret_t ret;
+ ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_UNPARSED(parsed_args, 0);
+ EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
+}
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_mix_valid_invalid_rules) {
+ const char * argv[] = {"process_name", "/foo/bar:=", "bar:=/fiz/buz", "}bar:=fiz"};
+ int argc = sizeof(argv) / sizeof(const char *);
+ rcl_arguments_t parsed_args;
+ rcl_ret_t ret;
+ ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_UNPARSED(parsed_args, 0, 1, 3);
+ EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
+}
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_two_namespace) {
+ const char * argv[] = {"process_name", "__ns:=/foo/bar", "__ns:=/fiz/buz"};
+ int argc = sizeof(argv) / sizeof(const char *);
+ rcl_arguments_t parsed_args;
+ rcl_ret_t ret;
+ ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_UNPARSED(parsed_args, 0);
+ EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
+}
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_fini_null) {
+ EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_arguments_fini(NULL));
+ rcl_reset_error();
+}
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_fini_impl_null) {
+ rcl_arguments_t parsed_args;
+ parsed_args.impl = NULL;
+ EXPECT_EQ(RCL_RET_ERROR, rcl_arguments_fini(&parsed_args));
+ rcl_reset_error();
+}
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_fini_twice) {
+ const char * argv[] = {"process_name"};
+ int argc = sizeof(argv) / sizeof(const char *);
+ rcl_arguments_t parsed_args;
+ ASSERT_EQ(RCL_RET_OK, rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args));
+ EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
+ EXPECT_EQ(RCL_RET_ERROR, rcl_arguments_fini(&parsed_args));
+ rcl_reset_error();
+}
diff --git a/rcl/test/rcl/test_remap.cpp b/rcl/test/rcl/test_remap.cpp
new file mode 100644
index 0000000..894c359
--- /dev/null
+++ b/rcl/test/rcl/test_remap.cpp
@@ -0,0 +1,467 @@
+// 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/rcl.h"
+#include "rcl/remap.h"
+#include "rcl/error_handling.h"
+
+#include "./arg_macros.hpp"
+
+#ifdef RMW_IMPLEMENTATION
+# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
+# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX)
+#else
+# define CLASSNAME(NAME, SUFFIX) NAME
+#endif
+
+class CLASSNAME (TestRemapFixture, RMW_IMPLEMENTATION) : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ }
+
+ void TearDown()
+ {
+ }
+};
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), global_namespace_replacement) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(global_arguments, "process_name", "__ns:=/foo/bar");
+
+ char * output = NULL;
+ ret = rcl_remap_node_namespace(
+ NULL, &global_arguments, "NodeName", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("/foo/bar", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), nodename_prefix_namespace_remap) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(
+ global_arguments,
+ "process_name", "Node1:__ns:=/foo/bar", "Node2:__ns:=/this_one", "Node3:__ns:=/bar/foo");
+
+ {
+ char * output = NULL;
+ ret = rcl_remap_node_namespace(
+ NULL, &global_arguments, "Node1", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("/foo/bar", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+ }
+ {
+ char * output = NULL;
+ ret = rcl_remap_node_namespace(
+ NULL, &global_arguments, "Node2", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("/this_one", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+ }
+ {
+ char * output = NULL;
+ ret = rcl_remap_node_namespace(
+ NULL, &global_arguments, "Node3", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("/bar/foo", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+ }
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), no_namespace_replacement) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(global_arguments, "process_name");
+
+ char * output = NULL;
+ ret = rcl_remap_node_namespace(
+ NULL, &global_arguments, "NodeName", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_EQ(NULL, output);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), local_namespace_replacement_before_global) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(global_arguments, "process_name", "__ns:=/global_args");
+ rcl_arguments_t local_arguments;
+ SCOPE_ARGS(local_arguments, "process_name", "__ns:=/local_args");
+
+ char * output = NULL;
+ ret = rcl_remap_node_namespace(
+ &local_arguments, &global_arguments, "NodeName", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("/local_args", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), no_use_global_namespace_replacement) {
+ rcl_ret_t ret;
+ rcl_arguments_t local_arguments;
+ SCOPE_ARGS(local_arguments, "process_name");
+
+ char * output = NULL;
+ ret = rcl_remap_node_namespace(
+ &local_arguments, NULL, "NodeName", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_EQ(NULL, output);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), other_rules_before_namespace_rule) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(
+ global_arguments, "process_name", "/foobar:=/foo/bar", "__ns:=/namespace", "__node:=new_name");
+
+ rcl_allocator_t allocator = rcl_get_default_allocator();
+ char * output = NULL;
+ ret = rcl_remap_node_namespace(NULL, &global_arguments, "NodeName", allocator, &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("/namespace", output);
+ allocator.deallocate(output, allocator.state);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), global_topic_name_replacement) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(global_arguments, "process_name", "/bar/foo:=/foo/bar");
+
+ {
+ char * output = NULL;
+ ret = rcl_remap_topic_name(
+ NULL, &global_arguments, "/bar/foo", "NodeName", "/", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ ASSERT_STREQ("/foo/bar", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+ }
+ {
+ char * output = NULL;
+ ret = rcl_remap_topic_name(
+ NULL, &global_arguments, "/foo/bar", "NodeName", "/", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_EQ(NULL, output);
+ }
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), relative_topic_name_remap) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(global_arguments, "process_name", "foo:=bar");
+
+ char * output = NULL;
+ ret = rcl_remap_topic_name(
+ NULL, &global_arguments, "/ns/foo", "NodeName", "/ns", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ ASSERT_STREQ("/ns/bar", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), nodename_prefix_topic_remap) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(
+ global_arguments,
+ "process_name", "Node1:/foo:=/foo/bar", "Node2:/foo:=/this_one", "Node3:/foo:=/bar/foo");
+
+ {
+ char * output = NULL;
+ ret = rcl_remap_topic_name(
+ NULL, &global_arguments, "/foo", "Node1", "/", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("/foo/bar", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+ }
+ {
+ char * output = NULL;
+ ret = rcl_remap_topic_name(
+ NULL, &global_arguments, "/foo", "Node2", "/", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("/this_one", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+ }
+ {
+ char * output = NULL;
+ ret = rcl_remap_topic_name(
+ NULL, &global_arguments, "/foo", "Node3", "/", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("/bar/foo", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+ }
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), no_use_global_topic_name_replacement) {
+ rcl_ret_t ret;
+ rcl_arguments_t local_arguments;
+ SCOPE_ARGS(local_arguments, "process_name");
+
+ char * output = NULL;
+ ret = rcl_remap_topic_name(
+ &local_arguments, NULL, "/bar/foo", "NodeName", "/", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_EQ(NULL, output);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), no_topic_name_replacement) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(global_arguments, "process_name");
+
+ char * output = NULL;
+ ret = rcl_remap_topic_name(
+ NULL, &global_arguments, "/bar/foo", "NodeName", "/", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_EQ(NULL, output);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), local_topic_replacement_before_global) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(global_arguments, "process_name", "/bar/foo:=/foo/global_args");
+ rcl_arguments_t local_arguments;
+ SCOPE_ARGS(local_arguments, "process_name", "/bar/foo:=/foo/local_args");
+
+ char * output = NULL;
+ ret = rcl_remap_topic_name(
+ &local_arguments, &global_arguments, "/bar/foo", "NodeName", "/", rcl_get_default_allocator(),
+ &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("/foo/local_args", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), other_rules_before_topic_rule) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(
+ global_arguments, "process_name", "__ns:=/namespace", "__node:=remap_name",
+ "/foobar:=/foo/bar");
+
+ rcl_allocator_t allocator = rcl_get_default_allocator();
+ char * output = NULL;
+ ret = rcl_remap_topic_name(
+ NULL, &global_arguments, "/foobar", "NodeName", "/", allocator, &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("/foo/bar", output);
+ allocator.deallocate(output, allocator.state);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), global_service_name_replacement) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(global_arguments, "process_name", "/bar/foo:=/foo/bar");
+
+ {
+ char * output = NULL;
+ ret = rcl_remap_service_name(
+ NULL, &global_arguments, "/bar/foo", "NodeName", "/", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ ASSERT_STREQ("/foo/bar", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+ }
+ {
+ char * output = NULL;
+ ret = rcl_remap_service_name(
+ NULL, &global_arguments, "/foobar", "NodeName", "/", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_EQ(NULL, output);
+ }
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), relative_service_name_remap) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(global_arguments, "process_name", "foo:=bar");
+
+ char * output = NULL;
+ ret = rcl_remap_service_name(
+ NULL, &global_arguments, "/ns/foo", "NodeName", "/ns", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ ASSERT_STREQ("/ns/bar", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), nodename_prefix_service_remap) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(
+ global_arguments,
+ "process_name", "Node1:/foo:=/foo/bar", "Node2:/foo:=/this_one", "Node3:/foo:=/bar/foo");
+
+ {
+ char * output = NULL;
+ ret = rcl_remap_service_name(
+ NULL, &global_arguments, "/foo", "Node1", "/", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("/foo/bar", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+ }
+ {
+ char * output = NULL;
+ ret = rcl_remap_service_name(
+ NULL, &global_arguments, "/foo", "Node2", "/", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("/this_one", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+ }
+ {
+ char * output = NULL;
+ ret = rcl_remap_service_name(
+ NULL, &global_arguments, "/foo", "Node3", "/", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("/bar/foo", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+ }
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), no_use_global_service_name_replacement) {
+ rcl_ret_t ret;
+ rcl_arguments_t local_arguments;
+ SCOPE_ARGS(local_arguments, "process_name");
+
+ char * output = NULL;
+ ret = rcl_remap_service_name(
+ &local_arguments, NULL, "/bar/foo", "NodeName", "/", rcl_get_default_allocator(),
+ &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_EQ(NULL, output);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), no_service_name_replacement) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(global_arguments, "process_name");
+
+ char * output = NULL;
+ ret = rcl_remap_service_name(
+ NULL, &global_arguments, "/bar/foo", "NodeName", "/", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_EQ(NULL, output);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), local_service_replacement_before_global) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(global_arguments, "process_name", "/bar/foo:=/foo/global_args");
+ rcl_arguments_t local_arguments;
+ SCOPE_ARGS(local_arguments, "process_name", "/bar/foo:=/foo/local_args");
+
+ char * output = NULL;
+ ret = rcl_remap_service_name(
+ &local_arguments, &global_arguments, "/bar/foo", "NodeName", "/", rcl_get_default_allocator(),
+ &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("/foo/local_args", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), other_rules_before_service_rule) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(
+ global_arguments, "process_name", "__ns:=/namespace", "__node:=remap_name",
+ "/foobar:=/foo/bar");
+
+ rcl_allocator_t allocator = rcl_get_default_allocator();
+ char * output = NULL;
+ ret = rcl_remap_service_name(
+ NULL, &global_arguments, "/foobar", "NodeName", "/", allocator, &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("/foo/bar", output);
+ allocator.deallocate(output, allocator.state);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), global_nodename_replacement) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(global_arguments, "process_name", "__node:=globalname");
+
+ rcl_allocator_t allocator = rcl_get_default_allocator();
+ char * output = NULL;
+ ret = rcl_remap_node_name(NULL, &global_arguments, "NodeName", allocator, &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("globalname", output);
+ allocator.deallocate(output, allocator.state);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), no_nodename_replacement) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(global_arguments, "process_name");
+
+ char * output = NULL;
+ ret = rcl_remap_node_name(NULL, &global_arguments, "NodeName", rcl_get_default_allocator(),
+ &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_EQ(NULL, output);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), local_nodename_replacement_before_global) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(global_arguments, "process_name", "__node:=global_name");
+ rcl_arguments_t local_arguments;
+ SCOPE_ARGS(local_arguments, "process_name", "__node:=local_name");
+
+ char * output = NULL;
+ ret = rcl_remap_node_name(
+ &local_arguments, &global_arguments, "NodeName", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("local_name", output);
+ rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), no_use_global_nodename_replacement) {
+ rcl_ret_t ret;
+ rcl_arguments_t local_arguments;
+ SCOPE_ARGS(local_arguments, "process_name");
+
+ char * output = NULL;
+ ret = rcl_remap_node_name(
+ &local_arguments, NULL, "NodeName", rcl_get_default_allocator(), &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_EQ(NULL, output);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), use_first_nodename_rule) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(global_arguments, "process_name", "__node:=firstname", "__node:=secondname");
+
+ rcl_allocator_t allocator = rcl_get_default_allocator();
+ char * output = NULL;
+ ret = rcl_remap_node_name(NULL, &global_arguments, "NodeName", allocator, &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("firstname", output);
+ allocator.deallocate(output, allocator.state);
+}
+
+TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), other_rules_before_nodename_rule) {
+ rcl_ret_t ret;
+ rcl_arguments_t global_arguments;
+ SCOPE_ARGS(
+ global_arguments, "process_name", "/foobar:=/foo", "__ns:=/namespace", "__node:=remap_name");
+
+ rcl_allocator_t allocator = rcl_get_default_allocator();
+ char * output = NULL;
+ ret = rcl_remap_node_name(NULL, &global_arguments, "NodeName", allocator, &output);
+ EXPECT_EQ(RCL_RET_OK, ret);
+ EXPECT_STREQ("remap_name", output);
+ allocator.deallocate(output, allocator.state);
+}
diff --git a/rcl/test/rcl/test_remap_integration.cpp b/rcl/test/rcl/test_remap_integration.cpp
new file mode 100644
index 0000000..fa9c3ce
--- /dev/null
+++ b/rcl/test/rcl/test_remap_integration.cpp
@@ -0,0 +1,289 @@
+// 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 "example_interfaces/srv/add_two_ints.h"
+#include "rcl/rcl.h"
+#include "rcl/remap.h"
+#include "rcl/error_handling.h"
+#include "std_msgs/msg/int64.h"
+
+#include "./arg_macros.hpp"
+
+#ifdef RMW_IMPLEMENTATION
+# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
+# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX)
+#else
+# define CLASSNAME(NAME, SUFFIX) NAME
+#endif
+
+class CLASSNAME (TestRemapIntegrationFixture, RMW_IMPLEMENTATION) : public ::testing::Test
+{
+public:
+ void SetUp()
+ {
+ }
+
+ void TearDown()
+ {
+ }
+};
+
+TEST_F(CLASSNAME(TestRemapIntegrationFixture, RMW_IMPLEMENTATION), remap_using_global_rule) {
+ int argc;
+ char ** argv;
+ SCOPE_GLOBAL_ARGS(
+ argc, argv, "process_name", "__node:=new_name", "__ns:=/new_ns", "/foo/bar:=/bar/foo");
+
+ rcl_node_t node = rcl_get_zero_initialized_node();
+ rcl_node_options_t default_options = rcl_node_get_default_options();
+ ASSERT_EQ(RCL_RET_OK, rcl_node_init(&node, "original_name", "/original_ns", &default_options));
+
+ { // Node name gets remapped
+ EXPECT_STREQ("new_name", rcl_node_get_name(&node));
+ }
+ { // Node namespace gets remapped
+ EXPECT_STREQ("/new_ns", rcl_node_get_namespace(&node));
+ }
+ { // Logger name gets remapped
+ EXPECT_STREQ("new_ns.new_name", rcl_node_get_logger_name(&node));
+ }
+ { // Publisher topic gets remapped
+ const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64);
+ rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options();
+ rcl_publisher_t publisher = rcl_get_zero_initialized_publisher();
+ rcl_ret_t ret = rcl_publisher_init(&publisher, &node, ts, "/foo/bar", &publisher_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_STREQ("/bar/foo", rcl_publisher_get_topic_name(&publisher));
+ EXPECT_EQ(RCL_RET_OK, rcl_publisher_fini(&publisher, &node));
+ }
+ { // Subscription topic gets remapped
+ const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64);
+ rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options();
+ rcl_subscription_t subscription = rcl_get_zero_initialized_subscription();
+ rcl_ret_t ret = rcl_subscription_init(
+ &subscription, &node, ts, "/foo/bar", &subscription_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_STREQ("/bar/foo", rcl_subscription_get_topic_name(&subscription));
+ EXPECT_EQ(RCL_RET_OK, rcl_subscription_fini(&subscription, &node));
+ }
+ { // Client service name gets remapped
+ const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT(
+ example_interfaces, AddTwoInts);
+ rcl_client_options_t client_options = rcl_client_get_default_options();
+ rcl_client_t client = rcl_get_zero_initialized_client();
+ rcl_ret_t ret = rcl_client_init(&client, &node, ts, "/foo/bar", &client_options);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_STREQ("/bar/foo", rcl_client_get_service_name(&client));
+ EXPECT_EQ(RCL_RET_OK, rcl_client_fini(&client, &node));
+ }
+ { // Server service name gets remapped
+ const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT(
+ example_interfaces, AddTwoInts);
+ rcl_service_options_t service_options = rcl_service_get_default_options();
+ rcl_service_t service = rcl_get_zero_initialized_service();
+ rcl_ret_t ret = rcl_service_init(&service, &node, ts, "/foo/bar", &service_options);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_STREQ("/bar/foo", rcl_service_get_service_name(&service));
+ EXPECT_EQ(RCL_RET_OK, rcl_service_fini(&service, &node));
+ }
+
+ EXPECT_EQ(RCL_RET_OK, rcl_node_fini(&node));
+}
+
+TEST_F(CLASSNAME(TestRemapIntegrationFixture, RMW_IMPLEMENTATION), ignore_global_rules) {
+ int argc;
+ char ** argv;
+ SCOPE_GLOBAL_ARGS(
+ argc, argv, "process_name", "__node:=new_name", "__ns:=/new_ns", "/foo/bar:=/bar/foo");
+ rcl_arguments_t local_arguments;
+ SCOPE_ARGS(local_arguments, "local_process_name");
+
+ rcl_node_t node = rcl_get_zero_initialized_node();
+ rcl_node_options_t options = rcl_node_get_default_options();
+ options.use_global_arguments = false;
+ options.arguments = local_arguments;
+ ASSERT_EQ(RCL_RET_OK, rcl_node_init(&node, "original_name", "/original_ns", &options));
+
+ { // Node name does not get remapped
+ EXPECT_STREQ("original_name", rcl_node_get_name(&node));
+ }
+ { // Node namespace does not get remapped
+ EXPECT_STREQ("/original_ns", rcl_node_get_namespace(&node));
+ }
+ { // Logger name gets remapped
+ EXPECT_STREQ("original_ns.original_name", rcl_node_get_logger_name(&node));
+ }
+ { // Publisher topic does not get remapped
+ const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64);
+ rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options();
+ rcl_publisher_t publisher = rcl_get_zero_initialized_publisher();
+ rcl_ret_t ret = rcl_publisher_init(&publisher, &node, ts, "/foo/bar", &publisher_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_STREQ("/foo/bar", rcl_publisher_get_topic_name(&publisher));
+ EXPECT_EQ(RCL_RET_OK, rcl_publisher_fini(&publisher, &node));
+ }
+ { // Subscription topic does not get remapped
+ const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64);
+ rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options();
+ rcl_subscription_t subscription = rcl_get_zero_initialized_subscription();
+ rcl_ret_t ret = rcl_subscription_init(
+ &subscription, &node, ts, "/foo/bar", &subscription_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_STREQ("/foo/bar", rcl_subscription_get_topic_name(&subscription));
+ EXPECT_EQ(RCL_RET_OK, rcl_subscription_fini(&subscription, &node));
+ }
+ { // Client service name does not get remapped
+ const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT(
+ example_interfaces, AddTwoInts);
+ rcl_client_options_t client_options = rcl_client_get_default_options();
+ rcl_client_t client = rcl_get_zero_initialized_client();
+ rcl_ret_t ret = rcl_client_init(&client, &node, ts, "/foo/bar", &client_options);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_STREQ("/foo/bar", rcl_client_get_service_name(&client));
+ EXPECT_EQ(RCL_RET_OK, rcl_client_fini(&client, &node));
+ }
+ { // Server service name does not get remapped
+ const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT(
+ example_interfaces, AddTwoInts);
+ rcl_service_options_t service_options = rcl_service_get_default_options();
+ rcl_service_t service = rcl_get_zero_initialized_service();
+ rcl_ret_t ret = rcl_service_init(&service, &node, ts, "/foo/bar", &service_options);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_STREQ("/foo/bar", rcl_service_get_service_name(&service));
+ EXPECT_EQ(RCL_RET_OK, rcl_service_fini(&service, &node));
+ }
+
+ EXPECT_EQ(RCL_RET_OK, rcl_node_fini(&node));
+}
+
+TEST_F(CLASSNAME(TestRemapIntegrationFixture, RMW_IMPLEMENTATION), local_rules_before_global) {
+ int argc;
+ char ** argv;
+ SCOPE_GLOBAL_ARGS(
+ argc, argv, "process_name", "__node:=global_name", "__ns:=/global_ns", "/foo/bar:=/bar/global");
+ rcl_arguments_t local_arguments;
+ SCOPE_ARGS(
+ local_arguments,
+ "process_name", "__node:=local_name", "__ns:=/local_ns", "/foo/bar:=/bar/local");
+
+ rcl_node_t node = rcl_get_zero_initialized_node();
+ rcl_node_options_t options = rcl_node_get_default_options();
+ options.arguments = local_arguments;
+ ASSERT_EQ(RCL_RET_OK, rcl_node_init(&node, "original_name", "/original_ns", &options));
+
+ { // Node name
+ EXPECT_STREQ("local_name", rcl_node_get_name(&node));
+ }
+ { // Node namespace
+ EXPECT_STREQ("/local_ns", rcl_node_get_namespace(&node));
+ }
+ { // Logger name
+ EXPECT_STREQ("local_ns.local_name", rcl_node_get_logger_name(&node));
+ }
+ { // Publisher topic
+ const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64);
+ rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options();
+ rcl_publisher_t publisher = rcl_get_zero_initialized_publisher();
+ rcl_ret_t ret = rcl_publisher_init(&publisher, &node, ts, "/foo/bar", &publisher_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_STREQ("/bar/local", rcl_publisher_get_topic_name(&publisher));
+ EXPECT_EQ(RCL_RET_OK, rcl_publisher_fini(&publisher, &node));
+ }
+ { // Subscription topic
+ const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64);
+ rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options();
+ rcl_subscription_t subscription = rcl_get_zero_initialized_subscription();
+ rcl_ret_t ret = rcl_subscription_init(
+ &subscription, &node, ts, "/foo/bar", &subscription_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_STREQ("/bar/local", rcl_subscription_get_topic_name(&subscription));
+ EXPECT_EQ(RCL_RET_OK, rcl_subscription_fini(&subscription, &node));
+ }
+ { // Client service name
+ const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT(
+ example_interfaces, AddTwoInts);
+ rcl_client_options_t client_options = rcl_client_get_default_options();
+ rcl_client_t client = rcl_get_zero_initialized_client();
+ rcl_ret_t ret = rcl_client_init(&client, &node, ts, "/foo/bar", &client_options);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_STREQ("/bar/local", rcl_client_get_service_name(&client));
+ EXPECT_EQ(RCL_RET_OK, rcl_client_fini(&client, &node));
+ }
+ { // Server service name
+ const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT(
+ example_interfaces, AddTwoInts);
+ rcl_service_options_t service_options = rcl_service_get_default_options();
+ rcl_service_t service = rcl_get_zero_initialized_service();
+ rcl_ret_t ret = rcl_service_init(&service, &node, ts, "/foo/bar", &service_options);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_STREQ("/bar/local", rcl_service_get_service_name(&service));
+ EXPECT_EQ(RCL_RET_OK, rcl_service_fini(&service, &node));
+ }
+
+ EXPECT_EQ(RCL_RET_OK, rcl_node_fini(&node));
+}
+
+TEST_F(CLASSNAME(TestRemapIntegrationFixture, RMW_IMPLEMENTATION), remap_relative_topic) {
+ int argc;
+ char ** argv;
+ SCOPE_GLOBAL_ARGS(argc, argv, "process_name", "/foo/bar:=remap/global");
+
+ rcl_node_t node = rcl_get_zero_initialized_node();
+ rcl_node_options_t default_options = rcl_node_get_default_options();
+ ASSERT_EQ(RCL_RET_OK, rcl_node_init(&node, "original_name", "/foo", &default_options));
+
+ { // Publisher topic
+ const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64);
+ rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options();
+ rcl_publisher_t publisher = rcl_get_zero_initialized_publisher();
+ rcl_ret_t ret = rcl_publisher_init(&publisher, &node, ts, "bar", &publisher_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_STREQ("/foo/remap/global", rcl_publisher_get_topic_name(&publisher));
+ EXPECT_EQ(RCL_RET_OK, rcl_publisher_fini(&publisher, &node));
+ }
+ { // Subscription topic
+ const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64);
+ rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options();
+ rcl_subscription_t subscription = rcl_get_zero_initialized_subscription();
+ rcl_ret_t ret = rcl_subscription_init(
+ &subscription, &node, ts, "bar", &subscription_options);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_STREQ("/foo/remap/global", rcl_subscription_get_topic_name(&subscription));
+ EXPECT_EQ(RCL_RET_OK, rcl_subscription_fini(&subscription, &node));
+ }
+ { // Client service name
+ const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT(
+ example_interfaces, AddTwoInts);
+ rcl_client_options_t client_options = rcl_client_get_default_options();
+ rcl_client_t client = rcl_get_zero_initialized_client();
+ rcl_ret_t ret = rcl_client_init(&client, &node, ts, "bar", &client_options);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_STREQ("/foo/remap/global", rcl_client_get_service_name(&client));
+ EXPECT_EQ(RCL_RET_OK, rcl_client_fini(&client, &node));
+ }
+ { // Server service name
+ const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT(
+ example_interfaces, AddTwoInts);
+ rcl_service_options_t service_options = rcl_service_get_default_options();
+ rcl_service_t service = rcl_get_zero_initialized_service();
+ rcl_ret_t ret = rcl_service_init(&service, &node, ts, "bar", &service_options);
+ EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+ EXPECT_STREQ("/foo/remap/global", rcl_service_get_service_name(&service));
+ EXPECT_EQ(RCL_RET_OK, rcl_service_fini(&service, &node));
+ }
+
+ EXPECT_EQ(RCL_RET_OK, rcl_node_fini(&node));
+}