Remove ros arguments (#454)

* Mark arguments vector as const.

* Add C++ version of rcl_remove_ros_arguments

rclcpp/CMakeLists.txt

@@ -322,6 +322,14 @@ if(BUILD_TESTING)
       "rcl")
     target_link_libraries(test_time_source ${PROJECT_NAME})
   endif()
+
+  ament_add_gtest(test_utilities test/test_utilities.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  if(TARGET test_utilities)
+    ament_target_dependencies(test_utilities
+      "rcl")
+    target_link_libraries(test_utilities ${PROJECT_NAME})
+  endif()
 endif()
 
 ament_package(

rclcpp/include/rclcpp/utilities.hpp

@@ -18,6 +18,7 @@
 #include <chrono>
 #include <functional>
 #include <limits>
+#include <vector>
 
 #include "rclcpp/visibility_control.hpp"
 
@@ -45,7 +46,6 @@ std::string to_string(T value)
 
 namespace rclcpp
 {
-
 /// Initialize communications via the rmw implementation and set up a global signal handler.
 /**
  * \param[in] argc Number of arguments.
@@ -53,7 +53,33 @@ namespace rclcpp
  */
 RCLCPP_PUBLIC
 void
-init(int argc, char * argv[]);
+init(int argc, char const * const argv[]);
+
+/// Initialize communications via the rmw implementation and set up a global signal handler.
+/**
+ * Additionally removes ROS-specific arguments from the argument vector.
+ * \param[in] argc Number of arguments.
+ * \param[in] argv Argument vector.
+ * \returns Members of the argument vector that are not ROS arguments.
+ */
+RCLCPP_PUBLIC
+std::vector<std::string>
+init_and_remove_ros_arguments(int argc, char const * const argv[]);
+
+/// Remove ROS-specific arguments from argument vector.
+/**
+ * Some arguments may not have been intended as ROS arguments.
+ * This function populates a the aruments in a vector.
+ * Since the first argument is always assumed to be a process name, the vector
+ * will always contain the process name.
+ *
+ * \param[in] argc Number of arguments.
+ * \param[in] argv Argument vector.
+ * \returns Members of the argument vector that are not ROS arguments.
+ */
+RCLCPP_PUBLIC
+std::vector<std::string>
+remove_ros_arguments(int argc, char const * const argv[]);
 
 /// Check rclcpp's status.
 /** \return True if SIGINT hasn't fired yet, false otherwise. */

rclcpp/src/rclcpp/utilities.cpp

@@ -24,6 +24,8 @@
 #include <string>
 #include <vector>
 
+#include "rclcpp/exceptions.hpp"
+
 #include "rcl/error_handling.h"
 #include "rcl/rcl.h"
 
@@ -160,7 +162,7 @@ signal_handler(int signal_value)
 }
 
 void
-rclcpp::init(int argc, char * argv[])
+rclcpp::init(int argc, char const * const argv[])
 {
   g_is_interrupted.store(false);
   if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
@@ -191,6 +193,57 @@ rclcpp::init(int argc, char * argv[])
 #endif
 }
 
+std::vector<std::string>
+rclcpp::init_and_remove_ros_arguments(int argc, char const * const argv[])
+{
+  rclcpp::init(argc, argv);
+  return rclcpp::remove_ros_arguments(argc, argv);
+}
+
+std::vector<std::string>
+rclcpp::remove_ros_arguments(int argc, char const * const argv[])
+{
+  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);
+  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,
+    alloc,
+    &nonros_argc,
+    &nonros_argv);
+
+  if (RCL_RET_OK != ret) {
+    if (NULL != nonros_argv) {
+      alloc.deallocate(nonros_argv, alloc.state);
+    }
+    exceptions::throw_from_rcl_error(ret, "Failed to remove ROS arguments: ");
+  }
+
+  std::vector<std::string> return_arguments;
+  return_arguments.resize(nonros_argc);
+
+  for (int ii = 0; ii < nonros_argc; ++ii) {
+    return_arguments[ii] = std::string(nonros_argv[ii]);
+  }
+
+  if (NULL != nonros_argv) {
+    alloc.deallocate(nonros_argv, alloc.state);
+  }
+
+  return return_arguments;
+}
+
 bool
 rclcpp::ok()
 {

rclcpp/test/test_utilities.cpp

@@ -0,0 +1,51 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "rclcpp/exceptions.hpp"
+#include "rclcpp/utilities.hpp"
+
+TEST(TestUtilities, remove_ros_arguments) {
+  const char * const argv[] = {"process_name", "-d", "__ns:=/foo/bar",
+    "__ns:=/fiz/buz", "--foo=bar", "--baz"};
+  int argc = sizeof(argv) / sizeof(const char *);
+  auto args = rclcpp::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]);
+}
+
+TEST(TestUtilities, remove_ros_arguments_null) {
+  // In the case of a C executable, we would expect to get
+  // argc=1 and argv = ["process_name"], so this is an invalid input.
+  ASSERT_THROW({
+    rclcpp::remove_ros_arguments(0, nullptr);
+  }, rclcpp::exceptions::RCLErrorBase);
+}
+
+TEST(TestUtilities, init_with_args) {
+  const char * const argv[] = {"process_name"};
+  int argc = sizeof(argv) / sizeof(const char *);
+  auto other_args = rclcpp::init_and_remove_ros_arguments(argc, argv);
+
+  ASSERT_EQ(1u, other_args.size());
+  ASSERT_EQ(std::string{"process_name"}, other_args[0]);
+}