diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp index 96e3f5296b6..c40ddfa5d02 100644 --- 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 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 index 43ef63b4099..9ca4dc1ebba 100644 --- 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("_", service_options); bond_client_node_ = std::make_shared("_", bond_options); bond_node_thread_ = std::make_unique(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(node_name, service_client_node_); + std::make_shared(node_name, shared_from_this()); } }