From da11e6dc2bff06ebc556f87fdcd9811bc20b8c2f Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 18 Dec 2019 13:51:34 -0300 Subject: [PATCH 1/3] Handle unknown global ROS arguments. Signed-off-by: Michel Hidalgo --- rclcpp/CMakeLists.txt | 1 + rclcpp/include/rclcpp/detail/utilities.hpp | 39 +++++++++++++++ rclcpp/src/rclcpp/context.cpp | 29 +++++++++-- rclcpp/src/rclcpp/detail/utilities.cpp | 56 ++++++++++++++++++++++ rclcpp/src/rclcpp/node_options.cpp | 26 +++------- rclcpp/test/test_init.cpp | 23 +++++++++ 6 files changed, 152 insertions(+), 22 deletions(-) create mode 100644 rclcpp/include/rclcpp/detail/utilities.hpp create mode 100644 rclcpp/src/rclcpp/detail/utilities.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 81543dc73d..42f9eb3f81 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 0000000000..dcb6a7f106 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/utilities.hpp @@ -0,0 +1,39 @@ +// 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 a93086aa5f..0a06f65d9a 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 0000000000..118d83bd1d --- /dev/null +++ b/rclcpp/src/rclcpp/detail/utilities.cpp @@ -0,0 +1,56 @@ +// 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"); + } + 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); + } + 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 3d9e663ce2..9bdfec5ddf 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 78554993d8..bafc765c22 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()); +} From 4388fa678c0a6385b90d094f2ea85c988b5bb68c Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 18 Dec 2019 15:55:22 -0300 Subject: [PATCH 2/3] Deallocate ROS arguments' indices on throw too. Signed-off-by: Michel Hidalgo --- rclcpp/src/rclcpp/detail/utilities.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/rclcpp/src/rclcpp/detail/utilities.cpp b/rclcpp/src/rclcpp/detail/utilities.cpp index 118d83bd1d..0d4d0e6b2d 100644 --- a/rclcpp/src/rclcpp/detail/utilities.cpp +++ b/rclcpp/src/rclcpp/detail/utilities.cpp @@ -42,12 +42,17 @@ get_unparsed_ros_arguments(int argc, char const * const argv[], if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get unparsed ROS arguments"); } - 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]]); + 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; } - allocator.deallocate(unparsed_ros_args_indices, allocator.state); } return unparsed_ros_arguments; } From eff5eea8dfe8a9dc47c76bed19eda770e2250daa Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 18 Dec 2019 16:00:23 -0300 Subject: [PATCH 3/3] Please uncrustify Signed-off-by: Michel Hidalgo --- rclcpp/include/rclcpp/detail/utilities.hpp | 7 ++++--- rclcpp/src/rclcpp/context.cpp | 10 +++++----- rclcpp/src/rclcpp/detail/utilities.cpp | 8 +++++--- rclcpp/src/rclcpp/node_options.cpp | 4 ++-- rclcpp/test/test_init.cpp | 2 +- 5 files changed, 17 insertions(+), 14 deletions(-) diff --git a/rclcpp/include/rclcpp/detail/utilities.hpp b/rclcpp/include/rclcpp/detail/utilities.hpp index dcb6a7f106..62012ba0c8 100644 --- a/rclcpp/include/rclcpp/detail/utilities.hpp +++ b/rclcpp/include/rclcpp/detail/utilities.hpp @@ -29,9 +29,10 @@ namespace detail { std::vector -get_unparsed_ros_arguments(int argc, char const * const argv[], - rcl_arguments_t * arguments, - rcl_allocator_t allocator); +get_unparsed_ros_arguments( + int argc, char const * const argv[], + rcl_arguments_t * arguments, + rcl_allocator_t allocator); } // namespace detail } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 0a06f65d9a..fe92cfe154 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -97,8 +97,8 @@ Context::init( try { std::vector unparsed_ros_arguments = detail::get_unparsed_ros_arguments(argc, argv, - &(rcl_context_->global_arguments), - rcl_get_default_allocator()); + &(rcl_context_->global_arguments), + rcl_get_default_allocator()); if (!unparsed_ros_arguments.empty()) { throw exceptions::UnknownROSArgsError(std::move(unparsed_ros_arguments)); } @@ -107,13 +107,13 @@ Context::init( std::lock_guard lock(g_contexts_mutex); g_contexts.push_back(this->shared_from_this()); - } catch(const std::exception & e) { + } 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"; + oss << "While handling: " << e.what() << std::endl << + " another exception was thrown"; rclcpp::exceptions::throw_from_rcl_error(ret, oss.str()); } throw; diff --git a/rclcpp/src/rclcpp/detail/utilities.cpp b/rclcpp/src/rclcpp/detail/utilities.cpp index 0d4d0e6b2d..f1a1260347 100644 --- a/rclcpp/src/rclcpp/detail/utilities.cpp +++ b/rclcpp/src/rclcpp/detail/utilities.cpp @@ -30,9 +30,11 @@ namespace detail { std::vector -get_unparsed_ros_arguments(int argc, char const * const argv[], - rcl_arguments_t * arguments, - rcl_allocator_t allocator) { +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) { diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index 9bdfec5ddf..0852f3d946 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -118,8 +118,8 @@ NodeOptions::get_rcl_node_options() const std::vector unparsed_ros_arguments = detail::get_unparsed_ros_arguments(c_argc, c_argv.get(), - &(node_options_->arguments), - this->allocator_); + &(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 bafc765c22..6e51ccd6c3 100644 --- a/rclcpp/test/test_init.cpp +++ b/rclcpp/test/test_init.cpp @@ -46,7 +46,7 @@ TEST(TestInit, initialize_with_unknown_ros_args) { 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_THROW({rclcpp::init(argc, argv);}, rclcpp::exceptions::UnknownROSArgsError); EXPECT_FALSE(rclcpp::is_initialized()); }