create component_container_isolated (ros2#1781)

Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com>

rclcpp_components/CMakeLists.txt

@@ -61,9 +61,20 @@ ament_target_dependencies(component_container_mt
   "rclcpp"
 )
 
+add_executable(
+  component_container_isolated
+  src/component_container_isolated.cpp
+)
+target_link_libraries(component_container_isolated component_manager)
+ament_target_dependencies(component_container_isolated
+  "rclcpp"
+)
+
+
 if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
   target_link_libraries(component_container "stdc++fs")
   target_link_libraries(component_container_mt "stdc++fs")
+  target_link_libraries(component_container_isolated "stdc++fs")
 endif()
 
 if(BUILD_TESTING)
@@ -135,7 +146,7 @@ install(
 
 # Install executables
 install(
-  TARGETS component_container component_container_mt
+  TARGETS component_container component_container_mt component_container_isolated
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 

rclcpp_components/include/rclcpp_components/component_manager.hpp

@@ -140,6 +140,22 @@ class ComponentManager : public rclcpp::Node
   virtual rclcpp::NodeOptions
   create_node_options(const std::shared_ptr<LoadNode::Request> request);
 
+  /// Add component node to executor model, it's invoked in on_load_node()
+  /**
+   * \param node_id  node_id of loaded component node in node_wrappers_
+   */
+  RCLCPP_COMPONENTS_PUBLIC
+  virtual void
+  add_node_to_executor(uint64_t node_id);
+
+  /// Remove component node from executor model, it's invoked in on_unload_node()
+  /**
+   * \param node_id  node_id of loaded component node in node_wrappers_
+   */
+  RCLCPP_COMPONENTS_PUBLIC
+  virtual void
+  remove_node_from_executor(uint64_t node_id);
+
   /// Service callback to load a new node in the component
   /**
    * This function allows to add parameters, remap rules, a specific node, name a namespace
@@ -231,7 +247,7 @@ class ComponentManager : public rclcpp::Node
     on_list_nodes(request_header, request, response);
   }
 
-private:
+protected:
   std::weak_ptr<rclcpp::Executor> executor_;
 
   uint64_t unique_id_ {1};

rclcpp_components/include/rclcpp_components/component_manager_isolated.hpp

@@ -0,0 +1,99 @@
+// Copyright 2021 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_COMPONENTS__COMPONENT_MANAGER_ISOLATED_HPP__
+#define RCLCPP_COMPONENTS__COMPONENT_MANAGER_ISOLATED_HPP__
+
+#include <map>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+#include <unordered_map>
+
+#include "rclcpp_components/component_manager.hpp"
+
+
+namespace rclcpp_components
+{
+/// ComponentManagerIsolated uses dedicated single-threaded executors for each components.
+template<typename ExecutorT = rclcpp::executors::SingleThreadedExecutor>
+class ComponentManagerIsolated : public rclcpp_components::ComponentManager
+{
+  using rclcpp_components::ComponentManager::ComponentManager;
+
+  struct DedicatedExecutorWrapper
+  {
+    std::shared_ptr<rclcpp::Executor> executor;
+    std::thread thread;
+    std::promise<void> promise;
+  };
+
+public:
+  ~ComponentManagerIsolated()
+  {
+    if (node_wrappers_.size()) {
+      for (auto & executor_wrapper : dedicated_executor_wrappers_) {
+        executor_wrapper.second.promise.set_value();
+        executor_wrapper.second.executor->cancel();
+        executor_wrapper.second.thread.join();
+      }
+      node_wrappers_.clear();
+    }
+  }
+
+protected:
+  /// Add component node to executor model, it's invoked in on_load_node()
+  /**
+   * \param node_id  node_id of loaded component node in node_wrappers_
+   */
+  RCLCPP_COMPONENTS_PUBLIC
+  void
+  add_node_to_executor(uint64_t node_id) override
+  {
+    DedicatedExecutorWrapper executor_wrapper;
+    auto exec = std::make_shared<ExecutorT>();
+    exec->add_node(node_wrappers_[node_id].get_node_base_interface());
+    executor_wrapper.executor = exec;
+    executor_wrapper.thread = std::thread(
+      [exec, cancel_token = executor_wrapper.promise.get_future()]() {
+        exec->spin_until_future_complete(cancel_token);
+      });
+    dedicated_executor_wrappers_[node_id] = std::move(executor_wrapper);
+  }
+  /// Remove component node from executor model, it's invoked in on_unload_node()
+  /**
+   * \param node_id  node_id of loaded component node in node_wrappers_
+   */
+  RCLCPP_COMPONENTS_PUBLIC
+  void
+  remove_node_from_executor(uint64_t node_id) override
+  {
+    auto executor_wrapper = dedicated_executor_wrappers_.find(node_id);
+    if (executor_wrapper != dedicated_executor_wrappers_.end()) {
+      executor_wrapper->second.promise.set_value();
+      executor_wrapper->second.executor->cancel();
+      executor_wrapper->second.thread.join();
+      dedicated_executor_wrappers_.erase(executor_wrapper);
+    }
+  }
+
+private:
+  std::unordered_map<uint64_t, DedicatedExecutorWrapper> dedicated_executor_wrappers_;
+};
+
+}  // namespace rclcpp_components
+
+#endif  // RCLCPP_COMPONENTS__COMPONENT_MANAGER_ISOLATED_HPP__

rclcpp_components/src/component_container_isolated.cpp

@@ -0,0 +1,49 @@
+// Copyright 2021 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 <memory>
+#include <vector>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp_components/component_manager_isolated.hpp"
+
+int main(int argc, char * argv[])
+{
+  /// Component container with dedicated single-threaded executors for each components.
+  rclcpp::init(argc, argv);
+  // parse arguments
+  bool use_multi_threaded_executor{false};
+  std::vector<std::string> args = rclcpp::remove_ros_arguments(argc, argv);
+  for (auto & arg : args) {
+    if (arg == std::string("--use_multi_threaded_executor")) {
+      use_multi_threaded_executor = true;
+    }
+  }
+  // create executor and component manager
+  auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  rclcpp::Node::SharedPtr node;
+  if (use_multi_threaded_executor) {
+    using ComponentManagerIsolated =
+      rclcpp_components::ComponentManagerIsolated<rclcpp::executors::MultiThreadedExecutor>;
+    node = std::make_shared<ComponentManagerIsolated>(exec);
+  } else {
+    using ComponentManagerIsolated =
+      rclcpp_components::ComponentManagerIsolated<rclcpp::executors::SingleThreadedExecutor>;
+    node = std::make_shared<ComponentManagerIsolated>(exec);
+  }
+  exec->add_node(node);
+  exec->spin();
+}

rclcpp_components/src/component_manager.cpp

@@ -166,6 +166,22 @@ ComponentManager::create_node_options(const std::shared_ptr<LoadNode::Request> r
   return options;
 }
 
+void
+ComponentManager::add_node_to_executor(uint64_t node_id)
+{
+  if (auto exec = executor_.lock()) {
+    exec->add_node(node_wrappers_[node_id].get_node_base_interface(), true);
+  }
+}
+
+void
+ComponentManager::remove_node_from_executor(uint64_t node_id)
+{
+  if (auto exec = executor_.lock()) {
+    exec->remove_node(node_wrappers_[node_id].get_node_base_interface());
+  }
+}
+
 void
 ComponentManager::on_load_node(
   const std::shared_ptr<rmw_request_id_t> request_header,
@@ -214,10 +230,9 @@ ComponentManager::on_load_node(
         throw ComponentManagerException("Component constructor threw an exception");
       }
 
+      add_node_to_executor(node_id);
+
       auto node = node_wrappers_[node_id].get_node_base_interface();
-      if (auto exec = executor_.lock()) {
-        exec->add_node(node, true);
-      }
       response->full_node_name = node->get_fully_qualified_name();
       response->unique_id = node_id;
       response->success = true;
@@ -253,9 +268,7 @@ ComponentManager::on_unload_node(
     response->error_message = ss.str();
     RCLCPP_WARN(get_logger(), "%s", ss.str().c_str());
   } else {
-    if (auto exec = executor_.lock()) {
-      exec->remove_node(wrapper->second.get_node_base_interface());
-    }
+    remove_node_from_executor(request->unique_id);
     node_wrappers_.erase(wrapper);
     response->success = true;
   }

rclcpp_components/test/test_component_manager_api.cpp

@@ -22,6 +22,7 @@
 #include "composition_interfaces/srv/list_nodes.hpp"
 
 #include "rclcpp_components/component_manager.hpp"
+#include "rclcpp_components/component_manager_isolated.hpp"
 
 using namespace std::chrono_literals;
 
@@ -36,11 +37,18 @@ class TestComponentManager : public ::testing::Test
 
 // TODO(hidmic): split up tests once Node bring up/tear down races
 //               are solved https://github.com/ros2/rclcpp/issues/863
-TEST_F(TestComponentManager, components_api)
+void test_components_api(bool use_dedicated_executor)
 {
   auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
   auto node = rclcpp::Node::make_shared("test_component_manager");
-  auto manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
+  std::shared_ptr<rclcpp_components::ComponentManager> manager;
+  if (use_dedicated_executor) {
+    using ComponentManagerIsolated =
+      rclcpp_components::ComponentManagerIsolated<rclcpp::executors::SingleThreadedExecutor>;
+    manager = std::make_shared<ComponentManagerIsolated>(exec);
+  } else {
+    manager = std::make_shared<rclcpp_components::ComponentManager>(exec);
+  }
 
   exec->add_node(manager);
   exec->add_node(node);
@@ -321,3 +329,15 @@ TEST_F(TestComponentManager, components_api)
     }
   }
 }
+
+TEST_F(TestComponentManager, components_api)
+{
+  {
+    SCOPED_TRACE("ComponentManager");
+    test_components_api(false);
+  }
+  {
+    SCOPED_TRACE("ComponentManagerIsolated");
+    test_components_api(true);
+  }
+}