Support passing yaml parameter files via commandline

rcl/include/rcl/arguments.h

@@ -137,6 +137,42 @@ rcl_arguments_get_unparsed(
   rcl_allocator_t allocator,
   int ** output_unparsed_indices);
 
+
+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],
@@ -857,6 +921,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:=";
+  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,62 @@ 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);
+}
+
+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]);
+}
+
+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]);
+}