diff --git a/rclcpp/include/rclcpp/init_options.hpp b/rclcpp/include/rclcpp/init_options.hpp index 447c148e00..6f87904f25 100644 --- a/rclcpp/include/rclcpp/init_options.hpp +++ b/rclcpp/include/rclcpp/init_options.hpp @@ -16,6 +16,7 @@ #define RCLCPP__INIT_OPTIONS_HPP_ #include +#include #include "rcl/init_options.h" #include "rclcpp/visibility_control.hpp" @@ -80,11 +81,30 @@ class InitOptions const rcl_init_options_t * get_rcl_init_options() const; + /// Retrieve default domain id and set. + RCLCPP_PUBLIC + void + use_default_domain_id(); + + /// Set the domain id. + RCLCPP_PUBLIC + void + set_domain_id(size_t domain_id); + + /// Return domain id. + RCLCPP_PUBLIC + size_t + get_domain_id() const; + protected: void finalize_init_options(); private: + void + finalize_init_options_impl(); + + mutable std::mutex init_options_mutex_; std::unique_ptr init_options_; bool initialize_logging_{true}; }; diff --git a/rclcpp/src/rclcpp/init_options.cpp b/rclcpp/src/rclcpp/init_options.cpp index ad2a900b5f..cffa06bd11 100644 --- a/rclcpp/src/rclcpp/init_options.cpp +++ b/rclcpp/src/rclcpp/init_options.cpp @@ -63,7 +63,8 @@ InitOptions & InitOptions::operator=(const InitOptions & other) { if (this != &other) { - this->finalize_init_options(); + std::lock_guard init_options_lock(init_options_mutex_); + this->finalize_init_options_impl(); rcl_ret_t ret = rcl_init_options_copy(other.get_rcl_init_options(), init_options_.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options"); @@ -80,6 +81,13 @@ InitOptions::~InitOptions() void InitOptions::finalize_init_options() +{ + std::lock_guard init_options_lock(init_options_mutex_); + this->finalize_init_options_impl(); +} + +void +InitOptions::finalize_init_options_impl() { if (init_options_) { rcl_ret_t ret = rcl_init_options_fini(init_options_.get()); @@ -99,4 +107,38 @@ InitOptions::get_rcl_init_options() const return init_options_.get(); } +void +InitOptions::use_default_domain_id() +{ + size_t domain_id = RCL_DEFAULT_DOMAIN_ID; + rcl_ret_t ret = rcl_get_default_domain_id(&domain_id); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get default domain id"); + } + set_domain_id(domain_id); +} + +void +InitOptions::set_domain_id(size_t domain_id) +{ + std::lock_guard init_options_lock(init_options_mutex_); + rcl_ret_t ret = rcl_init_options_set_domain_id(init_options_.get(), domain_id); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to set domain id to rcl init options"); + } +} + +size_t +InitOptions::get_domain_id() const +{ + std::lock_guard init_options_lock(init_options_mutex_); + size_t domain_id; + rcl_ret_t ret = rcl_init_options_get_domain_id(init_options_.get(), &domain_id); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get domain id from rcl init options"); + } + + return domain_id; +} + } // namespace rclcpp diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 840c5de0e3..e9e70f8063 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -253,6 +253,11 @@ if(TARGET test_node_options) ament_target_dependencies(test_node_options "rcl") target_link_libraries(test_node_options ${PROJECT_NAME}) endif() +ament_add_gtest(test_init_options rclcpp/test_init_options.cpp) +if(TARGET test_init_options) + ament_target_dependencies(test_init_options "rcl") + target_link_libraries(test_init_options ${PROJECT_NAME}) +endif() ament_add_gtest(test_parameter_client rclcpp/test_parameter_client.cpp) if(TARGET test_parameter_client) ament_target_dependencies(test_parameter_client diff --git a/rclcpp/test/rclcpp/test_init_options.cpp b/rclcpp/test/rclcpp/test_init_options.cpp new file mode 100644 index 0000000000..53b2b9402e --- /dev/null +++ b/rclcpp/test/rclcpp/test_init_options.cpp @@ -0,0 +1,60 @@ +// Copyright 2020 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 + +#include +#include + +#include "rcl/allocator.h" +#include "rcl/domain_id.h" + +#include "rclcpp/init_options.hpp" + + +TEST(TestInitOptions, test_construction) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto options = rclcpp::InitOptions(allocator); + const rcl_init_options_t * rcl_options = options.get_rcl_init_options(); + ASSERT_TRUE(rcl_options != nullptr); + ASSERT_TRUE(rcl_options->impl != nullptr); + + { + auto options_copy = rclcpp::InitOptions(options); + const rcl_init_options_t * rcl_options_copy = options_copy.get_rcl_init_options(); + ASSERT_TRUE(rcl_options_copy != nullptr); + ASSERT_TRUE(rcl_options_copy->impl != nullptr); + } + + { + auto options_copy = options; + const rcl_init_options_t * rcl_options_copy = options_copy.get_rcl_init_options(); + ASSERT_TRUE(rcl_options_copy != nullptr); + ASSERT_TRUE(rcl_options_copy->impl != nullptr); + } +} + +TEST(TestInitOptions, test_domain_id) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto options = rclcpp::InitOptions(allocator); + size_t domain_id = RCL_DEFAULT_DOMAIN_ID; + EXPECT_EQ(RCL_RET_OK, rcl_get_default_domain_id(&domain_id)); + + options.use_default_domain_id(); + EXPECT_EQ(domain_id, options.get_domain_id()); + options.set_domain_id(42); + EXPECT_EQ((size_t)42, options.get_domain_id()); + options.use_default_domain_id(); + EXPECT_EQ(domain_id, options.get_domain_id()); +}