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 c40ddfa5d0..daf6abd40d 100644 --- 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 #include #include +#include #include #include @@ -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 bond_node_thread_; + // Callback group used by services and timers + rclcpp::CallbackGroup::SharedPtr callback_group_; + std::unique_ptr service_thread_; // The services provided by this node rclcpp::Service::SharedPtr manager_srv_; diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index 9ca4dc1ebb..2d26f7c144 100644 --- 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::duration(bond_timeout_s)); + callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); manager_srv_ = create_service( 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( 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("_", bond_options); - bond_node_thread_ = std::make_unique(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(); + executor->add_callback_group(callback_group_, get_node_base_interface()); + service_thread_ = std::make_unique(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", node_name, bond_client_node_); + std::make_shared("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 index a07cfa4235..94268ae2f3 100644 --- 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(); - 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 index 634b4acd5b..78d84fb8db 100644 --- 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 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 index e498e1371d..c19fbd9847 100644 --- 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(); thread_ = std::make_unique( [&]() { - 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([&]() {executor_->spin();}); +} NodeThread::~NodeThread() { - executor_.cancel(); + executor_->cancel(); thread_->join(); }