Support passing yaml parameter files via commandline (#253)

* wip

* rcl_arguments_get_param_files() with char **

Restore version of function that outputs a char **

* _rcl_parse_param_rule() allocates string

Before this commit `rcl_parse_params` was allocating space the size of
the argument, but the space only needs to be as big as the argument
minus the prefix length.

* Debug print before incrementing num files

* Alloc and make sure to dealloc on failure

* fix print of parsed parameter rule

* rcutils types not needed as we dont use rcutils_string_array anymore

* minimal test

* cleanup debug prints

* linter fixup

* add tests for param arguments

* lint tests

* Doc for rcl_arguments_get_param_files_count()

* Shrink parameter files

* const param_prefix_len

* Copy parameter file names

* deallocate parameter files in fini

* free memory after tests pass

rcl/include/rcl/arguments.h

@@ -137,6 +137,55 @@ rcl_arguments_get_unparsed(
   rcl_allocator_t allocator,
   int ** output_unparsed_indices);
 
+/// Return the number of parameter yaml files given in the arguments.
+/**
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] args An arguments structure that has been parsed.
+ * \return number of yaml files, or
+ * \return -1 if args is `NULL` or zero initialized.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+int
+rcl_arguments_get_param_files_count(
+  const rcl_arguments_t * args);
+
+
+/// Return a list of yaml parameter file paths specified on the command line.
+/**
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | Yes
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] arguments An arguments structure that has been parsed.
+ * \param[in] allocator A valid allocator.
+ * \param[out] parameter_files An allocated array of paramter file names.
+ *   This array must be deallocated by the caller using the given allocator.
+ *   The output is NULL if there were no paramter files.
+ * \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_param_files(
+  const rcl_arguments_t * arguments,
+  rcl_allocator_t allocator,
+  char *** parameter_files);
+
 /// Return a list of arguments with ROS-specific arguments removed.
 /**
  * Some arguments may not have been intended as ROS arguments.

rcl/include/rcl/types.h

@@ -92,5 +92,7 @@ typedef rmw_ret_t rcl_ret_t;
 #define RCL_RET_INVALID_REMAP_RULE 1001
 /// Expected one type of lexeme but got another
 #define RCL_RET_WRONG_LEXEME 1002
+/// Argument is not a valid parameter rule
+#define RCL_RET_INVALID_PARAM_RULE 1010
 
 #endif  // RCL__TYPES_H_

rcl/src/rcl/arguments.c

@@ -51,6 +51,53 @@ _rcl_parse_remap_rule(
   rcl_allocator_t allocator,
   rcl_remap_t * output_rule);
 
+RCL_LOCAL
+rcl_ret_t
+_rcl_parse_param_rule(
+  const char * arg,
+  rcl_allocator_t allocator,
+  char ** output_rule);
+
+rcl_ret_t
+rcl_arguments_get_param_files(
+  const rcl_arguments_t * arguments,
+  rcl_allocator_t allocator,
+  char *** parameter_files)
+{
+  RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(arguments, RCL_RET_INVALID_ARGUMENT, allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(arguments->impl, RCL_RET_INVALID_ARGUMENT, allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(parameter_files, RCL_RET_INVALID_ARGUMENT, allocator);
+  *(parameter_files) = allocator.allocate(
+    sizeof(char *) * arguments->impl->num_param_files_args, allocator.state);
+  if (NULL == *parameter_files) {
+    return RCL_RET_BAD_ALLOC;
+  }
+  for (int i = 0; i < arguments->impl->num_param_files_args; ++i) {
+    (*parameter_files)[i] = rcutils_strdup(arguments->impl->parameter_files[i], allocator);
+    if (NULL == *parameter_files) {
+      // deallocate allocated memory
+      for (int r = i; r >= 0; --r) {
+        allocator.deallocate((*parameter_files[i]), allocator.state);
+      }
+      allocator.deallocate((*parameter_files), allocator.state);
+      (*parameter_files) = NULL;
+      return RCL_RET_BAD_ALLOC;
+    }
+  }
+  return RCL_RET_OK;
+}
+
+int
+rcl_arguments_get_param_files_count(
+  const rcl_arguments_t * args)
+{
+  if (NULL == args || NULL == args->impl) {
+    return -1;
+  }
+  return args->impl->num_param_files_args;
+}
+
 rcl_ret_t
 rcl_parse_arguments(
   int argc,
@@ -84,6 +131,8 @@ rcl_parse_arguments(
   args_impl->remap_rules = NULL;
   args_impl->unparsed_args = NULL;
   args_impl->num_unparsed_args = 0;
+  args_impl->parameter_files = NULL;
+  args_impl->num_param_files_args = 0;
   args_impl->allocator = allocator;
 
   if (argc == 0) {
@@ -102,12 +151,27 @@ rcl_parse_arguments(
     ret = RCL_RET_BAD_ALLOC;
     goto fail;
   }
+  args_impl->parameter_files = allocator.allocate(sizeof(char *) * argc, allocator.state);
+  if (NULL == args_impl->parameter_files) {
+    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->parameter_files[args_impl->num_param_files_args] = NULL;
+    if (
+      RCL_RET_OK == _rcl_parse_param_rule(
+        argv[i], allocator, &(args_impl->parameter_files[args_impl->num_param_files_args])))
+    {
+      ++(args_impl->num_param_files_args);
+      RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME,
+        "params rule : %s\n total num param rules %d",
+        args_impl->parameter_files[args_impl->num_param_files_args - 1],
+        args_impl->num_param_files_args)
+    } else 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 (%s) error '%s'", i, argv[i],
@@ -131,6 +195,18 @@ rcl_parse_arguments(
     allocator.deallocate(args_impl->remap_rules, allocator.state);
     args_impl->remap_rules = NULL;
   }
+  // Shrink Parameter files
+  if (0 == args_impl->num_param_files_args) {
+    allocator.deallocate(args_impl->parameter_files, allocator.state);
+    args_impl->parameter_files = NULL;
+  } else if (args_impl->num_param_files_args < argc) {
+    args_impl->parameter_files = rcutils_reallocf(
+      args_impl->parameter_files, sizeof(char *) * args_impl->num_param_files_args, &allocator);
+    if (NULL == args_impl->parameter_files) {
+      ret = RCL_RET_BAD_ALLOC;
+      goto fail;
+    }
+  }
   // Shrink unparsed_args
   if (0 == args_impl->num_unparsed_args) {
     // No unparsed args
@@ -270,6 +346,7 @@ rcl_arguments_copy(
   // 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;
+  args_out->impl->num_param_files_args = 0;
 
   // Copy unparsed args
   args_out->impl->unparsed_args = allocator.allocate(
@@ -306,6 +383,30 @@ rcl_arguments_copy(
       return ret;
     }
   }
+  // Copy parameter files
+  if (args->impl->num_param_files_args) {
+    args_out->impl->parameter_files = allocator.allocate(
+      sizeof(char *) * args->impl->num_param_files_args, allocator.state);
+    if (NULL == args_out->impl->parameter_files) {
+      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_param_files_args = args->impl->num_param_files_args;
+    for (int i = 0; i < args->impl->num_param_files_args; ++i) {
+      args_out->impl->parameter_files[i] =
+        rcutils_strdup(args->impl->parameter_files[i], allocator);
+      if (NULL == args_out->impl->parameter_files[i]) {
+        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;
+      }
+    }
+  } else {
+    args_out->impl->parameter_files = NULL;
+  }
   return RCL_RET_OK;
 }
 
@@ -337,6 +438,16 @@ rcl_arguments_fini(
     args->impl->num_unparsed_args = 0;
     args->impl->unparsed_args = NULL;
 
+    if (args->impl->parameter_files) {
+      for (int p = 0; p < args->impl->num_param_files_args; ++p) {
+        args->impl->allocator.deallocate(
+          args->impl->parameter_files[p], args->impl->allocator.state);
+      }
+      args->impl->allocator.deallocate(args->impl->parameter_files, args->impl->allocator.state);
+      args->impl->num_param_files_args = 0;
+      args->impl->parameter_files = NULL;
+    }
+
     args->impl->allocator.deallocate(args->impl, args->impl->allocator.state);
     args->impl = NULL;
     return ret;
@@ -857,6 +968,28 @@ _rcl_parse_remap_rule(
   return ret;
 }
 
+rcl_ret_t
+_rcl_parse_param_rule(
+  const char * arg,
+  rcl_allocator_t allocator,
+  char ** output_rule)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(arg, RCL_RET_INVALID_ARGUMENT, allocator);
+
+  const char * param_prefix = "__params:=";
+  const size_t param_prefix_len = strlen(param_prefix);
+  if (strncmp(param_prefix, arg, param_prefix_len) == 0) {
+    size_t outlen = strlen(arg) - param_prefix_len;
+    *output_rule = allocator.allocate(sizeof(char) * (outlen + 1), allocator.state);
+    if (NULL == output_rule) {
+      return RCL_RET_BAD_ALLOC;
+    }
+    snprintf(*output_rule, outlen + 1, "%s", arg + param_prefix_len);
+    return RCL_RET_OK;
+  }
+  return RCL_RET_INVALID_PARAM_RULE;
+}
+
 #ifdef __cplusplus
 }
 #endif

rcl/src/rcl/arguments_impl.h

@@ -31,6 +31,12 @@ typedef struct rcl_arguments_impl_t
   /// Length of unparsed_args.
   int num_unparsed_args;
 
+  // TODO(mikaelarguedas) consider storing CLI parameter rules here
+  /// Array of yaml parameter file paths
+  char ** parameter_files;
+  /// Length of parameter_files.
+  int num_param_files_args;
+
   /// Array of rules for name remapping.
   rcl_remap_t * remap_rules;
   /// Length of remap_rules.

rcl/test/rcl/test_arguments.cpp

@@ -100,6 +100,7 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), check_valid_vs_inval
   EXPECT_TRUE(is_valid_arg("rostopic://rostopic:=rosservice"));
   EXPECT_TRUE(is_valid_arg("rostopic:///rosservice:=rostopic"));
   EXPECT_TRUE(is_valid_arg("rostopic:///foo/bar:=baz"));
+  EXPECT_TRUE(is_valid_arg("__params:=node_name"));
 
   EXPECT_FALSE(is_valid_arg(":="));
   EXPECT_FALSE(is_valid_arg("foo:="));
@@ -117,6 +118,7 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), check_valid_vs_inval
   EXPECT_FALSE(is_valid_arg("foo:=/b}ar"));
   EXPECT_FALSE(is_valid_arg("rostopic://:=rosservice"));
   EXPECT_FALSE(is_valid_arg("rostopic::=rosservice"));
+  EXPECT_FALSE(is_valid_arg("__param:=node_name"));
 }
 
 TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_no_args) {
@@ -299,3 +301,74 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remove_ros_args
     alloc.deallocate(nonros_argv, alloc.state);
   }
 }
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_zero) {
+  const char * argv[] = {"process_name", "__ns:=/namespace", "random:=arg"};
+  int argc = sizeof(argv) / sizeof(const char *);
+  rcl_ret_t ret;
+
+  rcl_allocator_t alloc = rcl_get_default_allocator();
+  rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
+
+  ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+
+  int parameter_filecount = rcl_arguments_get_param_files_count(&parsed_args);
+  EXPECT_EQ(0, parameter_filecount);
+  EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
+}
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_single) {
+  const char * argv[] = {
+    "process_name", "__ns:=/namespace", "random:=arg", "__params:=parameter_filepath"
+  };
+  int argc = sizeof(argv) / sizeof(const char *);
+  rcl_ret_t ret;
+
+  rcl_allocator_t alloc = rcl_get_default_allocator();
+  rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
+
+  ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+
+  int parameter_filecount = rcl_arguments_get_param_files_count(&parsed_args);
+  EXPECT_EQ(1, parameter_filecount);
+  char ** parameter_files = NULL;
+  ret = rcl_arguments_get_param_files(&parsed_args, alloc, &parameter_files);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  EXPECT_STREQ("parameter_filepath", parameter_files[0]);
+
+  for (int i = 0; i < parameter_filecount; ++i) {
+    alloc.deallocate(parameter_files[i], alloc.state);
+  }
+  alloc.deallocate(parameter_files, alloc.state);
+  EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
+}
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_multiple) {
+  const char * argv[] = {
+    "process_name", "__params:=parameter_filepath1", "__ns:=/namespace",
+    "random:=arg", "__params:=parameter_filepath2"
+  };
+  int argc = sizeof(argv) / sizeof(const char *);
+  rcl_ret_t ret;
+
+  rcl_allocator_t alloc = rcl_get_default_allocator();
+  rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
+
+  ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+
+  int parameter_filecount = rcl_arguments_get_param_files_count(&parsed_args);
+  EXPECT_EQ(2, parameter_filecount);
+  char ** parameter_files = NULL;
+  ret = rcl_arguments_get_param_files(&parsed_args, alloc, &parameter_files);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  EXPECT_STREQ("parameter_filepath1", parameter_files[0]);
+  EXPECT_STREQ("parameter_filepath2", parameter_files[1]);
+  for (int i = 0; i < parameter_filecount; ++i) {
+    alloc.deallocate(parameter_files[i], alloc.state);
+  }
+  alloc.deallocate(parameter_files, alloc.state);
+  EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
+}