Handle unknown global ROS arguments. (#951)

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

rclcpp/CMakeLists.txt

@@ -36,6 +36,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/detail/rmw_implementation_specific_payload.cpp
   src/rclcpp/detail/rmw_implementation_specific_publisher_payload.cpp
   src/rclcpp/detail/rmw_implementation_specific_subscription_payload.cpp
+  src/rclcpp/detail/utilities.cpp
   src/rclcpp/duration.cpp
   src/rclcpp/event.cpp
   src/rclcpp/exceptions.cpp

rclcpp/include/rclcpp/detail/utilities.hpp

@@ -0,0 +1,40 @@
+// Copyright 2019 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.
+
+#ifndef RCLCPP__DETAIL__UTILITIES_HPP_
+#define RCLCPP__DETAIL__UTILITIES_HPP_
+
+#include "rclcpp/detail/utilities.hpp"
+
+#include <string>
+#include <vector>
+
+#include "rcl/allocator.h"
+#include "rcl/arguments.h"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+std::vector<std::string>
+get_unparsed_ros_arguments(
+  int argc, char const * const argv[],
+  rcl_arguments_t * arguments,
+  rcl_allocator_t allocator);
+
+}  // namespace detail
+}  // namespace rclcpp
+
+#endif  // RCLCPP__DETAIL__UTILITIES_HPP_

rclcpp/src/rclcpp/context.cpp

@@ -16,10 +16,13 @@
 
 #include <memory>
 #include <mutex>
+#include <sstream>
 #include <string>
 #include <vector>
+#include <utility>
 
 #include "rcl/init.h"
+#include "rclcpp/detail/utilities.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/logging.hpp"
 #include "rmw/impl/cpp/demangle.hpp"
@@ -91,10 +94,30 @@ Context::init(
     rcl_context_.reset();
     rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl");
   }
-  init_options_ = init_options;
+  try {
+    std::vector<std::string> unparsed_ros_arguments =
+      detail::get_unparsed_ros_arguments(argc, argv,
+        &(rcl_context_->global_arguments),
+        rcl_get_default_allocator());
+    if (!unparsed_ros_arguments.empty()) {
+      throw exceptions::UnknownROSArgsError(std::move(unparsed_ros_arguments));
+    }
 
-  std::lock_guard<std::mutex> lock(g_contexts_mutex);
-  g_contexts.push_back(this->shared_from_this());
+    init_options_ = init_options;
+
+    std::lock_guard<std::mutex> lock(g_contexts_mutex);
+    g_contexts.push_back(this->shared_from_this());
+  } catch (const std::exception & e) {
+    ret = rcl_shutdown(rcl_context_.get());
+    rcl_context_.reset();
+    if (RCL_RET_OK != ret) {
+      std::ostringstream oss;
+      oss << "While handling: " << e.what() << std::endl <<
+        "    another exception was thrown";
+      rclcpp::exceptions::throw_from_rcl_error(ret, oss.str());
+    }
+    throw;
+  }
 }
 
 bool

rclcpp/src/rclcpp/detail/utilities.cpp

@@ -0,0 +1,63 @@
+// Copyright 2019 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 "rclcpp/detail/utilities.hpp"
+
+#include <cassert>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rclcpp/exceptions.hpp"
+
+#include "rcl/allocator.h"
+#include "rcl/arguments.h"
+
+namespace rclcpp
+{
+namespace detail
+{
+
+std::vector<std::string>
+get_unparsed_ros_arguments(
+  int argc, char const * const argv[],
+  rcl_arguments_t * arguments,
+  rcl_allocator_t allocator)
+{
+  std::vector<std::string> unparsed_ros_arguments;
+  int unparsed_ros_args_count = rcl_arguments_get_count_unparsed_ros(arguments);
+  if (unparsed_ros_args_count > 0) {
+    int * unparsed_ros_args_indices = nullptr;
+    rcl_ret_t ret =
+      rcl_arguments_get_unparsed_ros(arguments, allocator, &unparsed_ros_args_indices);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get unparsed ROS arguments");
+    }
+    try {
+      for (int i = 0; i < unparsed_ros_args_count; ++i) {
+        assert(unparsed_ros_args_indices[i] >= 0);
+        assert(unparsed_ros_args_indices[i] < argc);
+        unparsed_ros_arguments.push_back(argv[unparsed_ros_args_indices[i]]);
+      }
+      allocator.deallocate(unparsed_ros_args_indices, allocator.state);
+    } catch (...) {
+      allocator.deallocate(unparsed_ros_args_indices, allocator.state);
+      throw;
+    }
+  }
+  return unparsed_ros_arguments;
+}
+
+}  // namespace detail
+}  // namespace rclcpp

rclcpp/src/rclcpp/node_options.cpp

@@ -20,6 +20,7 @@
 #include <vector>
 #include <utility>
 
+#include "rclcpp/detail/utilities.hpp"
 #include "rclcpp/exceptions.hpp"
 #include "rclcpp/logging.hpp"
 #include "rclcpp/publisher_options.hpp"
@@ -115,25 +116,12 @@ NodeOptions::get_rcl_node_options() const
       throw_from_rcl_error(ret, "failed to parse arguments");
     }
 
-    int unparsed_ros_args_count =
-      rcl_arguments_get_count_unparsed_ros(&(node_options_->arguments));
-    if (unparsed_ros_args_count > 0) {
-      int * unparsed_ros_args_indices = nullptr;
-      ret = rcl_arguments_get_unparsed_ros(
-        &(node_options_->arguments), this->allocator_, &unparsed_ros_args_indices);
-      if (RCL_RET_OK != ret) {
-        throw_from_rcl_error(ret, "failed to get unparsed ROS arguments");
-      }
-      try {
-        std::vector<std::string> unparsed_ros_args;
-        for (int i = 0; i < unparsed_ros_args_count; ++i) {
-          unparsed_ros_args.push_back(c_argv[unparsed_ros_args_indices[i]]);
-        }
-        throw exceptions::UnknownROSArgsError(std::move(unparsed_ros_args));
-      } catch (...) {
-        this->allocator_.deallocate(unparsed_ros_args_indices, this->allocator_.state);
-        throw;
-      }
+    std::vector<std::string> unparsed_ros_arguments =
+      detail::get_unparsed_ros_arguments(c_argc, c_argv.get(),
+        &(node_options_->arguments),
+        this->allocator_);
+    if (!unparsed_ros_arguments.empty()) {
+      throw exceptions::UnknownROSArgsError(std::move(unparsed_ros_arguments));
     }
   }
 

rclcpp/test/test_init.cpp

@@ -14,6 +14,7 @@
 
 #include <gtest/gtest.h>
 
+#include "rclcpp/exceptions.hpp"
 #include "rclcpp/utilities.hpp"
 
 TEST(TestInit, is_initialized) {
@@ -27,3 +28,25 @@ TEST(TestInit, is_initialized) {
 
   EXPECT_FALSE(rclcpp::is_initialized());
 }
+
+TEST(TestInit, initialize_with_ros_args) {
+  EXPECT_FALSE(rclcpp::is_initialized());
+
+  rclcpp::init(0, nullptr);
+
+  EXPECT_TRUE(rclcpp::is_initialized());
+
+  rclcpp::shutdown();
+
+  EXPECT_FALSE(rclcpp::is_initialized());
+}
+
+TEST(TestInit, initialize_with_unknown_ros_args) {
+  EXPECT_FALSE(rclcpp::is_initialized());
+
+  const char * const argv[] = {"--ros-args", "unknown"};
+  const int argc = static_cast<int>(sizeof(argv) / sizeof(const char *));
+  EXPECT_THROW({rclcpp::init(argc, argv);}, rclcpp::exceptions::UnknownROSArgsError);
+
+  EXPECT_FALSE(rclcpp::is_initialized());
+}