diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 81543dc..42f9eb3 100644 --- a/rclcpp/CMakeLists.txt +++ b/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 diff --git a/rclcpp/include/rclcpp/detail/utilities.hpp b/rclcpp/include/rclcpp/detail/utilities.hpp new file mode 100644 index 0000000..62012ba --- /dev/null +++ b/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 +#include + +#include "rcl/allocator.h" +#include "rcl/arguments.h" + +namespace rclcpp +{ +namespace detail +{ + +std::vector +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_ diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index a93086a..fe92cfe 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -16,10 +16,13 @@ #include #include +#include #include #include +#include #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 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 lock(g_contexts_mutex); - g_contexts.push_back(this->shared_from_this()); + init_options_ = init_options; + + std::lock_guard 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 diff --git a/rclcpp/src/rclcpp/detail/utilities.cpp b/rclcpp/src/rclcpp/detail/utilities.cpp new file mode 100644 index 0000000..f1a1260 --- /dev/null +++ b/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 +#include +#include +#include + +#include "rclcpp/exceptions.hpp" + +#include "rcl/allocator.h" +#include "rcl/arguments.h" + +namespace rclcpp +{ +namespace detail +{ + +std::vector +get_unparsed_ros_arguments( + int argc, char const * const argv[], + rcl_arguments_t * arguments, + rcl_allocator_t allocator) +{ + std::vector 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 diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index 3d9e663..0852f3d 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -20,6 +20,7 @@ #include #include +#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 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 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)); } } diff --git a/rclcpp/test/test_init.cpp b/rclcpp/test/test_init.cpp index 7855499..6e51ccd 100644 --- a/rclcpp/test/test_init.cpp +++ b/rclcpp/test/test_init.cpp @@ -14,6 +14,7 @@ #include +#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(sizeof(argv) / sizeof(const char *)); + EXPECT_THROW({rclcpp::init(argc, argv);}, rclcpp::exceptions::UnknownROSArgsError); + + EXPECT_FALSE(rclcpp::is_initialized()); +}