Remove ros arguments (#223)

* Make argument vector const array of const.

* Add remove_ros_arguments method.

* Mark rcl_arguments const where appropriate.

rcl/include/rcl/arguments.h

@@ -42,8 +42,8 @@ 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`.
+ * 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()
  *
@@ -99,7 +99,7 @@ RCL_PUBLIC
 RCL_WARN_UNUSED
 int
 rcl_arguments_get_count_unparsed(
-  rcl_arguments_t * args);
+  const rcl_arguments_t * args);
 
 /// Return a list of indexes that weren't successfully parsed.
 /**
@@ -130,10 +130,47 @@ RCL_PUBLIC
 RCL_WARN_UNUSED
 rcl_ret_t
 rcl_arguments_get_unparsed(
-  rcl_arguments_t * args,
+  const rcl_arguments_t * args,
   rcl_allocator_t allocator,
   int ** output_unparsed_indices);
 
+/// Return a list of arguments with ROS-specific arguments removed.
+/**
+ * Some arguments may not have been intended as ROS arguments.
+ * This function populates an array of the aruments in a new argv array.
+ * Since the first argument is always assumed to be a process name, the list
+ * will always contain the first value from the argument vector.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | Yes
+ * Thread-Safe        | Yes
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] argv The argument vector
+ * \param[in] args An arguments structure that has been parsed.
+ * \param[in] allocator A valid allocator.
+ * \param[out] nonros_argc The count of arguments that aren't ROS-specific
+ * \param[out] nonros_argv An allocated array of arguments that aren't ROS-specific
+ *   This array must be deallocated by the caller using the given allocator.
+ *   If there are no non-ROS 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_remove_ros_arguments(
+  char const * const argv[],
+  const rcl_arguments_t * args,
+  rcl_allocator_t allocator,
+  int * nonros_argc,
+  const char ** nonros_argv[]);
+
 /// Reclaim resources held inside rcl_arguments_t structure.
 /**
  * <hr>

rcl/include/rcl/rcl.h

@@ -121,7 +121,7 @@ extern "C"
 RCL_PUBLIC
 RCL_WARN_UNUSED
 rcl_ret_t
-rcl_init(int argc, char ** argv, rcl_allocator_t allocator);
+rcl_init(int argc, char const * const * argv, rcl_allocator_t allocator);
 
 /// Signal global shutdown of rcl.
 /**

rcl/src/rcl/arguments.c

@@ -313,7 +313,7 @@ fail:
 
 int
 rcl_arguments_get_count_unparsed(
-  rcl_arguments_t * args)
+  const rcl_arguments_t * args)
 {
   if (NULL == args || NULL == args->impl) {
     return -1;
@@ -323,7 +323,7 @@ rcl_arguments_get_count_unparsed(
 
 rcl_ret_t
 rcl_arguments_get_unparsed(
-  rcl_arguments_t * args,
+  const rcl_arguments_t * args,
   rcl_allocator_t allocator,
   int ** output_unparsed_indices)
 {
@@ -355,6 +355,48 @@ rcl_get_zero_initialized_arguments(void)
   return default_arguments;
 }
 
+rcl_ret_t
+rcl_remove_ros_arguments(
+  char const * const argv[],
+  const rcl_arguments_t * args,
+  rcl_allocator_t allocator,
+  int * nonros_argc,
+  const char ** nonros_argv[])
+{
+  RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(argv, RCL_RET_INVALID_ARGUMENT, allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(nonros_argc, RCL_RET_INVALID_ARGUMENT, allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(args, RCL_RET_INVALID_ARGUMENT, allocator);
+
+  *nonros_argc = rcl_arguments_get_count_unparsed(args);
+  *nonros_argv = NULL;
+
+  if (*nonros_argc <= 0) {
+    return RCL_RET_INVALID_ARGUMENT;
+  }
+
+  int * unparsed_indices = NULL;
+  rcl_ret_t ret;
+  ret = rcl_arguments_get_unparsed(args, allocator, &unparsed_indices);
+
+  if (RCL_RET_OK != ret) {
+    return ret;
+  }
+
+  size_t alloc_size = sizeof(char *) * *nonros_argc;
+  *nonros_argv = allocator.allocate(alloc_size, allocator.state);
+  if (NULL == *nonros_argv) {
+    allocator.deallocate(unparsed_indices, allocator.state);
+    return RCL_RET_BAD_ALLOC;
+  }
+  for (int i = 0; i < *nonros_argc; ++i) {
+    (*nonros_argv)[i] = argv[unparsed_indices[i]];
+  }
+
+  allocator.deallocate(unparsed_indices, allocator.state);
+  return RCL_RET_OK;
+}
+
 rcl_ret_t
 rcl_arguments_fini(
   rcl_arguments_t * args)

rcl/src/rcl/rcl.c

@@ -61,7 +61,7 @@ __clean_up_init()
 }
 
 rcl_ret_t
-rcl_init(int argc, char ** argv, rcl_allocator_t allocator)
+rcl_init(int argc, char const * const * argv, rcl_allocator_t allocator)
 {
   rcl_ret_t fail_ret = RCL_RET_ERROR;
 

rcl/test/rcl/test_arguments.cpp

@@ -190,3 +190,61 @@ TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_fini_twice) {
   EXPECT_EQ(RCL_RET_ERROR, rcl_arguments_fini(&parsed_args));
   rcl_reset_error();
 }
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remove_ros_args) {
+  const char * argv[] =
+  {"process_name", "-d", "__ns:=/foo/bar", "__ns:=/fiz/buz", "--foo=bar", "--baz"};
+  int argc = sizeof(argv) / sizeof(const char *);
+
+  rcl_allocator_t alloc = rcl_get_default_allocator();
+  rcl_arguments_t parsed_args;
+  rcl_ret_t ret;
+  ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args);
+  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+
+  int nonros_argc = 0;
+  const char ** nonros_argv = NULL;
+
+  ret = rcl_remove_ros_arguments(
+    argv,
+    &parsed_args,
+    alloc,
+    &nonros_argc,
+    &nonros_argv);
+
+  EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  ASSERT_EQ(nonros_argc, 4);
+  EXPECT_STREQ(nonros_argv[0], "process_name");
+  EXPECT_STREQ(nonros_argv[1], "-d");
+  EXPECT_STREQ(nonros_argv[2], "--foo=bar");
+  EXPECT_STREQ(nonros_argv[3], "--baz");
+  EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args));
+
+  if (NULL != nonros_argv) {
+    alloc.deallocate(nonros_argv, alloc.state);
+  }
+}
+
+TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remove_ros_args_zero) {
+  const char * argv[] = {""};
+  rcl_ret_t ret;
+
+  rcl_allocator_t alloc = rcl_get_default_allocator();
+  rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
+
+  int nonros_argc = 0;
+  const char ** nonros_argv = NULL;
+
+  ret = rcl_remove_ros_arguments(
+    argv,
+    &parsed_args,
+    alloc,
+    &nonros_argc,
+    &nonros_argv);
+
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe();
+
+  if (NULL != nonros_argv) {
+    alloc.deallocate(nonros_argv, alloc.state);
+  }
+}