rcl_node_init() copies node options passed into it (#231)

* Add rcl_remap_copy()
* Add rcl_arguments_coy()
* Add rcl_node_options_copy()
* Node copies options passed to it

rcl/include/rcl/arguments.h

@@ -174,6 +174,34 @@ rcl_remove_ros_arguments(
   int * nonros_argc,
   const char ** nonros_argv[]);
 
+/// Copy one arguments structure into another.
+/**
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | Yes
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] error_alloc an alocator to use if an error occurs.
+ *  This allocator is not used to allocate args_out.
+ * \param[in] args The structure to be copied.
+ *  Its allocator is used to copy memory into the new structure.
+ * \param[out] args_out A zero-initialized arguments structure to be copied into.
+ * \return `RCL_RET_OK` if the structure was copied 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_arguments_copy(
+  rcl_allocator_t error_alloc,
+  const rcl_arguments_t * args,
+  rcl_arguments_t * args_out);
+
 /// Reclaim resources held inside rcl_arguments_t structure.
 /**
  * <hr>

rcl/include/rcl/node.h

@@ -152,7 +152,9 @@ rcl_get_zero_initialized_node(void);
  * \param[inout] node a preallocated rcl_node_t
  * \param[in] name the name of the node, must be a valid c-string
  * \param[in] namespace_ the namespace of the node, must be a valid c-string
- * \param[in] options the node options
+ * \param[in] options the node options.
+ *  The options are deep copied into the node.
+ *  The caller is always responsible for freeing memory used options they pass in.
  * \return `RCL_RET_OK` if the node was initialized successfully, or
  * \return `RCL_RET_ALREADY_INIT` if the node has already be initialized, or
  * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
@@ -208,6 +210,34 @@ RCL_PUBLIC
 rcl_node_options_t
 rcl_node_get_default_options(void);
 
+/// Copy one options structure into another.
+/**
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | Yes
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] error_alloc an alocator to use if an error occurs.
+ *  This allocator is not used to allocate the output.
+ * \param[in] options The structure to be copied.
+ *  Its allocator is used to copy memory into the new structure.
+ * \param[out] options_out An options structure containing default values.
+ * \return `RCL_RET_OK` if the structure was copied 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_node_options_copy(
+  rcl_allocator_t error_alloc,
+  const rcl_node_options_t * options,
+  rcl_node_options_t * options_out);
+
 /// Return `true` if the node is valid, else `false`.
 /**
  * Also return `false` if the node pointer is `NULL` or the allocator is invalid.

rcl/src/rcl/arguments.c

@@ -242,6 +242,72 @@ rcl_remove_ros_arguments(
   return RCL_RET_OK;
 }
 
+rcl_ret_t
+rcl_arguments_copy(
+  rcl_allocator_t error_alloc,
+  const rcl_arguments_t * args,
+  rcl_arguments_t * args_out)
+{
+  RCL_CHECK_ALLOCATOR_WITH_MSG(&error_alloc, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(args, RCL_RET_INVALID_ARGUMENT, error_alloc);
+  RCL_CHECK_ARGUMENT_FOR_NULL(args->impl, RCL_RET_INVALID_ARGUMENT, error_alloc);
+  RCL_CHECK_ARGUMENT_FOR_NULL(args_out, RCL_RET_INVALID_ARGUMENT, error_alloc);
+  if (NULL != args_out->impl) {
+    RCL_SET_ERROR_MSG("args_out must be zero initialized", error_alloc);
+    return RCL_RET_INVALID_ARGUMENT;
+  }
+
+  rcl_allocator_t allocator = args->impl->allocator;
+
+  args_out->impl = allocator.allocate(sizeof(rcl_arguments_impl_t), allocator.state);
+  if (NULL == args_out->impl) {
+    return RCL_RET_BAD_ALLOC;
+  }
+
+  args_out->impl->allocator = allocator;
+
+  // Zero so it's safe to call rcl_arguments_fini() if an error occurrs while copying.
+  args_out->impl->num_remap_rules = 0;
+  args_out->impl->num_unparsed_args = 0;
+
+  // Copy unparsed args
+  args_out->impl->unparsed_args = allocator.allocate(
+    sizeof(int) * args->impl->num_unparsed_args, allocator.state);
+  if (NULL == args_out->impl->unparsed_args) {
+    if (RCL_RET_OK != rcl_arguments_fini(args_out)) {
+      RCL_SET_ERROR_MSG("Error while finalizing arguments due to another error", error_alloc);
+    }
+    return RCL_RET_BAD_ALLOC;
+  }
+  for (int i = 0; i < args->impl->num_unparsed_args; ++i) {
+    args_out->impl->unparsed_args[i] = args->impl->unparsed_args[i];
+  }
+  args_out->impl->num_unparsed_args = args->impl->num_unparsed_args;
+
+  // Copy remap rules
+  args_out->impl->remap_rules = allocator.allocate(
+    sizeof(rcl_remap_t) * args->impl->num_remap_rules, allocator.state);
+  if (NULL == args_out->impl->remap_rules) {
+    if (RCL_RET_OK != rcl_arguments_fini(args_out)) {
+      RCL_SET_ERROR_MSG("Error while finalizing arguments due to another error", error_alloc);
+    }
+    return RCL_RET_BAD_ALLOC;
+  }
+  args_out->impl->num_remap_rules = args->impl->num_remap_rules;
+  for (int i = 0; i < args->impl->num_remap_rules; ++i) {
+    args_out->impl->remap_rules[i] = rcl_remap_get_zero_initialized();
+    rcl_ret_t ret = rcl_remap_copy(
+      error_alloc, &(args->impl->remap_rules[i]), &(args_out->impl->remap_rules[i]));
+    if (RCL_RET_OK != ret) {
+      if (RCL_RET_OK != rcl_arguments_fini(args_out)) {
+        RCL_SET_ERROR_MSG("Error while finalizing arguments due to another error", error_alloc);
+      }
+      return ret;
+    }
+  }
+  return RCL_RET_OK;
+}
+
 rcl_ret_t
 rcl_arguments_fini(
   rcl_arguments_t * args)

rcl/src/rcl/node.c

@@ -222,9 +222,12 @@ rcl_node_init(
   node->impl->rmw_node_handle = NULL;
   node->impl->graph_guard_condition = NULL;
   node->impl->logger_name = NULL;
+  node->impl->options = rcl_node_get_default_options();
   // Initialize node impl.
-  // node options (assume it is trivially copyable)
+  ret = rcl_node_options_copy(*allocator, options, &(node->impl->options));
-  node->impl->options = *options;
+  if (RCL_RET_OK != ret) {
+    goto fail;
+  }
 
   // Remap the node name and namespace if remap rules are given
   rcl_arguments_t * global_args = NULL;
@@ -383,6 +386,15 @@ fail:
       }
       allocator->deallocate(node->impl->graph_guard_condition, allocator->state);
     }
+    if (NULL != node->impl->options.arguments.impl) {
+      ret = rcl_arguments_fini(&(node->impl->options.arguments));
+      if (ret != RCL_RET_OK) {
+        RCUTILS_LOG_ERROR_NAMED(
+          ROS_PACKAGE_NAME,
+          "failed to fini arguments in error recovery: %s", rcl_get_error_string_safe()
+        )
+      }
+    }
     allocator->deallocate(node->impl, allocator->state);
   }
   *node = rcl_get_zero_initialized_node();
@@ -424,6 +436,12 @@ rcl_node_fini(rcl_node_t * node)
   allocator.deallocate(node->impl->graph_guard_condition, allocator.state);
   // assuming that allocate and deallocate are ok since they are checked in init
   allocator.deallocate((char *)node->impl->logger_name, allocator.state);
+  if (NULL != node->impl->options.arguments.impl) {
+    rcl_ret_t ret = rcl_arguments_fini(&(node->impl->options.arguments));
+    if (ret != RCL_RET_OK) {
+      return ret;
+    }
+  }
   allocator.deallocate(node->impl, allocator.state);
   node->impl = NULL;
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node finalized")
@@ -462,6 +480,31 @@ rcl_node_get_default_options()
   return default_options;
 }
 
+rcl_ret_t
+rcl_node_options_copy(
+  rcl_allocator_t error_alloc,
+  const rcl_node_options_t * options,
+  rcl_node_options_t * options_out)
+{
+  RCL_CHECK_ALLOCATOR_WITH_MSG(&error_alloc, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT, error_alloc);
+  RCL_CHECK_ARGUMENT_FOR_NULL(options_out, RCL_RET_INVALID_ARGUMENT, error_alloc);
+  if (options_out == options) {
+    RCL_SET_ERROR_MSG(
+      "Attempted to copy options into itself", error_alloc);
+    return RCL_RET_INVALID_ARGUMENT;
+  }
+  options_out->domain_id = options->domain_id;
+  options_out->allocator = options->allocator;
+  options_out->use_global_arguments = options->use_global_arguments;
+  if (NULL != options->arguments.impl) {
+    rcl_ret_t ret = rcl_arguments_copy(
+      error_alloc, &(options->arguments), &(options_out->arguments));
+    return ret;
+  }
+  return RCL_RET_OK;
+}
+
 const char *
 rcl_node_get_name(const rcl_node_t * node)
 {

rcl/src/rcl/remap.c

@@ -39,6 +39,45 @@ rcl_remap_get_zero_initialized()
   return rule;
 }
 
+rcl_ret_t
+rcl_remap_copy(
+  rcl_allocator_t error_alloc,
+  const rcl_remap_t * rule,
+  rcl_remap_t * rule_out)
+{
+  RCL_CHECK_ALLOCATOR_WITH_MSG(&error_alloc, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(rule, RCL_RET_INVALID_ARGUMENT, error_alloc);
+  RCL_CHECK_ARGUMENT_FOR_NULL(rule_out, RCL_RET_INVALID_ARGUMENT, error_alloc);
+
+  rcl_allocator_t allocator = rule->allocator;
+  rule_out->allocator = allocator;
+  rule_out->type = rule->type;
+  if (NULL != rule->node_name) {
+    rule_out->node_name = rcutils_strdup(rule->node_name, allocator);
+    if (NULL == rule_out->node_name) {
+      goto fail;
+    }
+  }
+  if (NULL != rule->match) {
+    rule_out->match = rcutils_strdup(rule->match, allocator);
+    if (NULL == rule_out->match) {
+      goto fail;
+    }
+  }
+  if (NULL != rule->replacement) {
+    rule_out->replacement = rcutils_strdup(rule->replacement, allocator);
+    if (NULL == rule_out->replacement) {
+      goto fail;
+    }
+  }
+  return RCL_RET_OK;
+fail:
+  if (RCL_RET_OK != rcl_remap_fini(rule_out)) {
+    RCL_SET_ERROR_MSG("Error while finalizing remap rule due to another error", error_alloc);
+  }
+  return RCL_RET_BAD_ALLOC;
+}
+
 rcl_ret_t
 rcl_remap_fini(
   rcl_remap_t * rule)

rcl/src/rcl/remap_impl.h

@@ -51,6 +51,34 @@ typedef struct rcl_remap_t
 rcl_remap_t
 rcl_remap_get_zero_initialized();
 
+/// Copy one remap structure into another.
+/**
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | Yes
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] error_alloc an alocator to use if an error occurs.
+ *  This allocator is not used to allocate rule_out.
+ * \param[in] rule The structure to be copied.
+ *  Its allocator is used to copy memory into the new structure.
+ * \param[out] rule_out A zero-initialized rcl_remap_t structure to be copied into.
+ * \return `RCL_RET_OK` if the structure was copied 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_remap_copy(
+  rcl_allocator_t error_alloc,
+  const rcl_remap_t * rule,
+  rcl_remap_t * rule_out);
+
 /// Reclaim resources used in an rcl_remap_t structure.
 /**
  * <hr>

rcl/test/rcl/test_arguments.cpp

@@ -165,6 +165,26 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_mix_valid_inval
   EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
 }
 
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy) {
+  const char * argv[] = {"process_name", "/foo/bar:=", "bar:=/fiz/buz", "__ns:=/foo"};
+  int argc = sizeof(argv) / sizeof(const char *);
+  rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
+  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();
+
+  rcl_arguments_t copied_args = rcl_get_zero_initialized_arguments();
+  ret = rcl_arguments_copy(rcl_get_default_allocator(), &parsed_args, &copied_args);
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+
+  EXPECT_UNPARSED(parsed_args, 0, 1);
+  EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
+
+  EXPECT_UNPARSED(copied_args, 0, 1);
+  EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&copied_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 *);