Ability to configure domain_id via InitOptions. (#1165)

Signed-off-by: Tomoya.Fujita <[email protected]>
Signed-off-by: Pedro Pena <[email protected]>

Author: fujitatomoya

rclcpp/include/rclcpp/init_options.hpp

@@ -16,6 +16,7 @@
 #define RCLCPP__INIT_OPTIONS_HPP_
 
 #include <memory>
+#include <mutex>
 
 #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<rcl_init_options_t> init_options_;
   bool initialize_logging_{true};
 };

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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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

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

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 <gtest/gtest.h>
+
+#include <string>
+#include <vector>
+
+#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());
+}