Reduce lifecycle manager nodes (#2456)

* remove bond_client_node_ in class LifecycleManager Signed-off-by: zhenpeng ge <[email protected]> * clear unused variables Signed-off-by: zhenpeng ge <[email protected]> * fix lint Signed-off-by: zhenpeng ge <[email protected]> * use dedicated executor thread Signed-off-by: William Woodall <[email protected]> * fix building error Signed-off-by: zhenpeng ge <[email protected]> * support to process executor for NodeThread Signed-off-by: zhenpeng ge <[email protected]> * use NodeThread for LifecycleManager Signed-off-by: zhenpeng ge <[email protected]> Co-authored-by: William Woodall <[email protected]>
ros-navigation · Jul 22, 2021 · 6062617 · 6062617
1 parent c417e2f
commit 6062617
Show file tree

Hide file tree

Showing 5 changed files with 39 additions and 20 deletions.
diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
@@ -18,6 +18,7 @@
 #include <map>
 #include <memory>
 #include <string>
+#include <thread>
 #include <unordered_map>
 #include <vector>
 
@@ -52,9 +53,9 @@ class LifecycleManager : public rclcpp::Node
   ~LifecycleManager();
 
 protected:
-  // The ROS node to create bond
-  rclcpp::Node::SharedPtr bond_client_node_;
-  std::unique_ptr<nav2_util::NodeThread> bond_node_thread_;
+  // Callback group used by services and timers
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  std::unique_ptr<nav2_util::NodeThread> service_thread_;
 
   // The services provided by this node
   rclcpp::Service<ManageLifecycleNodes>::SharedPtr manager_srv_;

diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp
@@ -49,18 +49,18 @@ LifecycleManager::LifecycleManager()
   bond_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(
     std::chrono::duration<double>(bond_timeout_s));
 
+  callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
   manager_srv_ = create_service<ManageLifecycleNodes>(
     get_name() + std::string("/manage_nodes"),
-    std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3));
+    std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3),
+    rmw_qos_profile_services_default,
+    callback_group_);
 
   is_active_srv_ = create_service<std_srvs::srv::Trigger>(
     get_name() + std::string("/is_active"),
-    std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3));
-
-  auto bond_options = rclcpp::NodeOptions().arguments(
-    {"--ros-args", "-r", std::string("__node:=") + get_name() + "_bond_client", "--"});
-  bond_client_node_ = std::make_shared<rclcpp::Node>("_", bond_options);
-  bond_node_thread_ = std::make_unique<nav2_util::NodeThread>(bond_client_node_);
+    std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3),
+    rmw_qos_profile_services_default,
+    callback_group_);
 
   transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE;
   transition_state_map_[Transition::TRANSITION_CLEANUP] = State::PRIMARY_STATE_UNCONFIGURED;
@@ -84,12 +84,17 @@ LifecycleManager::LifecycleManager()
       if (autostart_) {
         startup();
       }
-    });
+    },
+    callback_group_);
+  auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  executor->add_callback_group(callback_group_, get_node_base_interface());
+  service_thread_ = std::make_unique<nav2_util::NodeThread>(executor);
 }
 
 LifecycleManager::~LifecycleManager()
 {
   RCLCPP_INFO(get_logger(), "Destroying %s", get_name());
+  service_thread_.reset();
 }
 
 void
@@ -154,7 +159,7 @@ LifecycleManager::createBondConnection(const std::string & node_name)
 
   if (bond_map_.find(node_name) == bond_map_.end() && bond_timeout_.count() > 0.0) {
     bond_map_[node_name] =
-      std::make_shared<bond::Bond>("bond", node_name, bond_client_node_);
+      std::make_shared<bond::Bond>("bond", node_name, shared_from_this());
     bond_map_[node_name]->setHeartbeatTimeout(timeout_s);
     bond_map_[node_name]->setHeartbeatPeriod(0.10);
     bond_map_[node_name]->start();
@@ -317,7 +322,8 @@ LifecycleManager::createBondTimer()
 
   bond_timer_ = this->create_wall_timer(
     200ms,
-    std::bind(&LifecycleManager::checkBondConnections, this));
+    std::bind(&LifecycleManager::checkBondConnections, this),
+    callback_group_);
 }
 
 void

diff --git a/nav2_lifecycle_manager/src/main.cpp b/nav2_lifecycle_manager/src/main.cpp
@@ -21,7 +21,7 @@ int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
   auto node = std::make_shared<nav2_lifecycle_manager::LifecycleManager>();
-  rclcpp::spin(node->get_node_base_interface());
+  rclcpp::spin(node);
   rclcpp::shutdown();
 
   return 0;

diff --git a/nav2_util/include/nav2_util/node_thread.hpp b/nav2_util/include/nav2_util/node_thread.hpp
@@ -23,7 +23,7 @@ namespace nav2_util
 {
 /**
  * @class nav2_util::NodeThread
- * @brief A background thread to process node callbacks
+ * @brief A background thread to process node/executor callbacks
  */
 class NodeThread
 {
@@ -34,6 +34,12 @@ class NodeThread
    */
   explicit NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base);
 
+  /**
+   * @brief A background thread to process executor's callbacks constructor
+   * @param executor Interface to executor to spin in thread
+   */
+  explicit NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor);
+
   /**
    * @brief A background thread to process node callbacks constructor
    * @param node Node pointer to spin in thread
@@ -51,7 +57,7 @@ class NodeThread
 protected:
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_;
   std::unique_ptr<std::thread> thread_;
-  rclcpp::executors::SingleThreadedExecutor executor_;
+  rclcpp::Executor::SharedPtr executor_;
 };
 
 }  // namespace nav2_util

diff --git a/nav2_util/src/node_thread.cpp b/nav2_util/src/node_thread.cpp
@@ -22,19 +22,25 @@ namespace nav2_util
 NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base)
 : node_(node_base)
 {
+  executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
   thread_ = std::make_unique<std::thread>(
     [&]()
     {
-      executor_.add_node(node_);
-      executor_.spin();
-      executor_.remove_node(node_);
+      executor_->add_node(node_);
+      executor_->spin();
+      executor_->remove_node(node_);
     });
 }
 
+NodeThread::NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor)
+: executor_(executor)
+{
+  thread_ = std::make_unique<std::thread>([&]() {executor_->spin();});
+}
 
 NodeThread::~NodeThread()
 {
-  executor_.cancel();
+  executor_->cancel();
   thread_->join();
 }