Remove service nodes in lifecycle_manager (#2267)

BriceRenaudeau · web-flow · commit 332304bc1614 · 2021-03-29T12:58:12.000-07:00
* remove service_node from nav2_lifecycle_manager

* wait_for_service() returns the value of service_is_ready()

* explain wait_for_service return

* use timer to trigger init()

* init() as lambda function

* wait_for_service as void

* fix gtest
diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
@@ -52,8 +52,7 @@ class LifecycleManager : public rclcpp::Node
   ~LifecycleManager();
 
 protected:
-  // The ROS node to use when calling lifecycle services
-  rclcpp::Node::SharedPtr service_client_node_;
+  // The ROS node to create bond
   rclcpp::Node::SharedPtr bond_client_node_;
   std::unique_ptr<nav2_util::NodeThread> bond_node_thread_;
 
@@ -169,6 +168,7 @@ class LifecycleManager : public rclcpp::Node
   void message(const std::string & msg);
 
   // Timer thread to look at bond connections
+  rclcpp::TimerBase::SharedPtr init_timer_;
   rclcpp::TimerBase::SharedPtr bond_timer_;
   std::chrono::milliseconds bond_timeout_;
 
diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp
@@ -57,11 +57,8 @@ LifecycleManager::LifecycleManager()
     get_name() + std::string("/is_active"),
     std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3));
 
-  auto service_options = rclcpp::NodeOptions().arguments(
-    {"--ros-args", "-r", std::string("__node:=") + get_name() + "_service_client", "--"});
   auto bond_options = rclcpp::NodeOptions().arguments(
     {"--ros-args", "-r", std::string("__node:=") + get_name() + "_bond_client", "--"});
-  service_client_node_ = std::make_shared<rclcpp::Node>("_", service_options);
   bond_client_node_ = std::make_shared<rclcpp::Node>("_", bond_options);
   bond_node_thread_ = std::make_unique<nav2_util::NodeThread>(bond_client_node_);
 
@@ -79,11 +76,15 @@ LifecycleManager::LifecycleManager()
   transition_label_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] =
     std::string("Shutting down ");
 
-  createLifecycleServiceClients();
-
-  if (autostart_) {
-    startup();
-  }
+  init_timer_ = this->create_wall_timer(
+    std::chrono::nanoseconds(10),
+    [this]() -> void {
+      init_timer_->cancel();
+      createLifecycleServiceClients();
+      if (autostart_) {
+        startup();
+      }
+    });
 }
 
 LifecycleManager::~LifecycleManager()
@@ -131,7 +132,7 @@ LifecycleManager::createLifecycleServiceClients()
   message("Creating and initializing lifecycle service clients");
   for (auto & node_name : node_names_) {
     node_map_[node_name] =
-      std::make_shared<LifecycleServiceClient>(node_name, service_client_node_);
+      std::make_shared<LifecycleServiceClient>(node_name, shared_from_this());
   }
 }
 
