Avoid parsing arguments twice

Signed-off-by: Chen Lihui <Lihui.Chen@sony.com>

rclcpp/src/rclcpp/utilities.cpp

@@ -21,6 +21,7 @@
 
 #include "./signal_handler.hpp"
 #include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/detail/utilities.hpp"
 #include "rclcpp/exceptions.hpp"
 
 #include "rcl/error_handling.h"
@@ -56,53 +57,32 @@ uninstall_signal_handlers()
   return SignalHandler::get_global_signal_handler().uninstall();
 }
 
+static
 std::vector<std::string>
-init_and_remove_ros_arguments(
-  int argc,
+_remove_ros_arguments(
   char const * const argv[],
-  const InitOptions & init_options)
+  const rcl_arguments_t * args,
+  rcl_allocator_t alloc)
 {
-  init(argc, argv, init_options);
-  return remove_ros_arguments(argc, argv);
-}
-
-std::vector<std::string>
-remove_ros_arguments(int argc, char const * const argv[])
-{
-  rcl_allocator_t alloc = rcl_get_default_allocator();
-  rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
-
   rcl_ret_t ret;
-
-  ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args);
-  if (RCL_RET_OK != ret) {
-    exceptions::throw_from_rcl_error(ret, "failed to parse arguments");
-  }
-
   int nonros_argc = 0;
   const char ** nonros_argv = NULL;
 
   ret = rcl_remove_ros_arguments(
     argv,
-    &parsed_args,
+    args,
     alloc,
     &nonros_argc,
     &nonros_argv);
 
   if (RCL_RET_OK != ret || nonros_argc < 0) {
     // Not using throw_from_rcl_error, because we may need to append deallocation failures.
-    exceptions::RCLErrorBase base_exc(ret, rcl_get_error_state());
+    exceptions::RCLError exc(ret, rcl_get_error_state(), "");
     rcl_reset_error();
     if (NULL != nonros_argv) {
       alloc.deallocate(nonros_argv, alloc.state);
     }
-    if (RCL_RET_OK != rcl_arguments_fini(&parsed_args)) {
-      base_exc.formatted_message += std::string(
-        ", failed also to cleanup parsed arguments, leaking memory: ") +
-        rcl_get_error_string().str;
-      rcl_reset_error();
-    }
-    throw exceptions::RCLError(base_exc, "");
+    throw exc;
   }
 
   std::vector<std::string> return_arguments(static_cast<size_t>(nonros_argc));
@@ -115,6 +95,48 @@ remove_ros_arguments(int argc, char const * const argv[])
     alloc.deallocate(nonros_argv, alloc.state);
   }
 
+  return return_arguments;
+}
+
+std::vector<std::string>
+init_and_remove_ros_arguments(
+  int argc,
+  char const * const argv[],
+  const InitOptions & init_options)
+{
+  init(argc, argv, init_options);
+
+  using rclcpp::contexts::get_global_default_context;
+  auto rcl_context = get_global_default_context()->get_rcl_context();
+  return _remove_ros_arguments(argv, &(rcl_context->global_arguments), rcl_get_default_allocator());
+}
+
+std::vector<std::string>
+remove_ros_arguments(int argc, char const * const argv[])
+{
+  rcl_allocator_t alloc = rcl_get_default_allocator();
+  rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments();
+
+  rcl_ret_t ret;
+
+  ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args);
+  if (RCL_RET_OK != ret) {
+    exceptions::throw_from_rcl_error(ret, "failed to parse arguments");
+  }
+
+  std::vector<std::string> return_arguments;
+  try {
+    return_arguments = _remove_ros_arguments(argv, &parsed_args, alloc);
+  } catch (exceptions::RCLError & exc) {
+    if (RCL_RET_OK != rcl_arguments_fini(&parsed_args)) {
+      exc.formatted_message += std::string(
+        ", failed also to cleanup parsed arguments, leaking memory: ") +
+        rcl_get_error_string().str;
+      rcl_reset_error();
+    }
+    throw exc;
+  }
+
   ret = rcl_arguments_fini(&parsed_args);
   if (RCL_RET_OK != ret) {
     exceptions::throw_from_rcl_error(

rclcpp/test/rclcpp/test_utilities.cpp

@@ -61,6 +61,29 @@ TEST(TestUtilities, init_with_args) {
   rclcpp::shutdown();
 }
 
+TEST(TestUtilities, init_with_args_contains_ros) {
+  EXPECT_FALSE(rclcpp::signal_handlers_installed());
+  const char * const argv[] = {
+    "process_name",
+    "-d", "--ros-args",
+    "-r", "__ns:=/foo/bar",
+    "-r", "__ns:=/fiz/buz",
+    "--", "--foo=bar", "--baz"
+  };
+  int argc = sizeof(argv) / sizeof(const char *);
+  auto args = rclcpp::init_and_remove_ros_arguments(argc, argv);
+
+  ASSERT_EQ(4u, args.size());
+  ASSERT_EQ(std::string{"process_name"}, args[0]);
+  ASSERT_EQ(std::string{"-d"}, args[1]);
+  ASSERT_EQ(std::string{"--foo=bar"}, args[2]);
+  ASSERT_EQ(std::string{"--baz"}, args[3]);
+  EXPECT_TRUE(rclcpp::signal_handlers_installed());
+
+  EXPECT_TRUE(rclcpp::ok());
+  rclcpp::shutdown();
+}
+
 TEST(TestUtilities, multi_init) {
   auto context1 = std::make_shared<rclcpp::contexts::DefaultContext>();
   auto context2 = std::make_shared<rclcpp::contexts::DefaultContext>();