Update BT nodes to use callback groups / executors to process

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -47,6 +47,10 @@ class BtActionNode : public BT::ActionNodeBase
   : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name)
   {
     node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
+    callback_group_ = node_->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive,
+      false);
+    callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
 
     // Get the required items from the blackboard
     bt_loop_duration_ =
@@ -82,7 +86,7 @@ class BtActionNode : public BT::ActionNodeBase
   void createActionClient(const std::string & action_name)
   {
     // Now that we have the ROS node to use, create the action client for this BT action
-    action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name);
+    action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name, callback_group_);
 
     // Make sure the server is actually there before continuing
     RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
@@ -223,7 +227,7 @@ class BtActionNode : public BT::ActionNodeBase
         }
       }
 
-      rclcpp::spin_some(node_);
+      callback_group_executor_.spin_some();
 
       // check if, after invoking spin_some(), we finally received the result
       if (!goal_result_available_) {
@@ -262,7 +266,7 @@ class BtActionNode : public BT::ActionNodeBase
   {
     if (should_cancel_goal()) {
       auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
-      if (rclcpp::spin_until_future_complete(node_, future_cancel, server_timeout_) !=
+      if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) !=
         rclcpp::FutureReturnCode::SUCCESS)
       {
         RCLCPP_ERROR(
@@ -291,7 +295,7 @@ class BtActionNode : public BT::ActionNodeBase
       return false;
     }
 
-    rclcpp::spin_some(node_);
+    callback_group_executor_.spin_some();
     auto status = goal_handle_->get_status();
 
     // Check if the goal is still executing
@@ -340,7 +344,8 @@ class BtActionNode : public BT::ActionNodeBase
     }
 
     auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
-    auto result = rclcpp::spin_until_future_complete(node_, *future_goal_handle_, timeout);
+    auto result =
+      callback_group_executor_.spin_until_future_complete(*future_goal_handle_, timeout);
     elapsed += timeout;
 
     if (result == rclcpp::FutureReturnCode::INTERRUPTED) {
@@ -383,6 +388,8 @@ class BtActionNode : public BT::ActionNodeBase
 
   // The node that will be used for any ROS operations
   rclcpp::Node::SharedPtr node_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   // The timeout value while waiting for response from a server when a
   // new action goal is sent or canceled

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -45,6 +45,10 @@ class BtServiceNode : public BT::ActionNodeBase
   : BT::ActionNodeBase(service_node_name, conf), service_node_name_(service_node_name)
   {
     node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node");
+    callback_group_ = node_->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive,
+      false);
+    callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
 
     // Get the required items from the blackboard
     bt_loop_duration_ =
@@ -55,7 +59,10 @@ class BtServiceNode : public BT::ActionNodeBase
 
     // Now that we have node_ to use, create the service client for this BT service
     getInput("service_name", service_name_);
-    service_client_ = node_->create_client<ServiceT>(service_name_);
+    service_client_ = node_->create_client<ServiceT>(
+      service_name_,
+      rmw_qos_profile_services_default,
+      callback_group_);
 
     // Make a request for the service without parameter
     request_ = std::make_shared<typename ServiceT::Request>();
@@ -147,7 +154,8 @@ class BtServiceNode : public BT::ActionNodeBase
     if (remaining > std::chrono::milliseconds(0)) {
       auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
 
-      auto rc = rclcpp::spin_until_future_complete(node_, future_result_, timeout);
+      rclcpp::FutureReturnCode rc;
+      rc = callback_group_executor_.spin_until_future_complete(future_result_, server_timeout_);
       if (rc == rclcpp::FutureReturnCode::SUCCESS) {
         request_sent_ = false;
         return BT::NodeStatus::SUCCESS;
@@ -195,6 +203,8 @@ class BtServiceNode : public BT::ActionNodeBase
 
   // The node that will be used for any ROS operations
   rclcpp::Node::SharedPtr node_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   // The timeout value while to use in the tick loop while waiting for
   // a result from the server

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp

@@ -88,6 +88,8 @@ class ControllerSelector : public BT::SyncActionNode
   std::string last_selected_controller_;
 
   rclcpp::Node::SharedPtr node_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   std::string topic_name_;
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp

@@ -89,6 +89,8 @@ class PlannerSelector : public BT::SyncActionNode
   std::string last_selected_planner_;
 
   rclcpp::Node::SharedPtr node_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   std::string topic_name_;
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp

@@ -74,12 +74,13 @@ class IsBatteryLowCondition : public BT::ConditionNode
   void batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg);
 
   rclcpp::Node::SharedPtr node_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
   rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_sub_;
   std::string battery_topic_;
   double min_battery_;
   bool is_voltage_;
   bool is_battery_low_;
-  std::mutex mutex_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp

@@ -86,6 +86,9 @@ class IsStuckCondition : public BT::ConditionNode
 private:
   // The node that will be used for any ROS operations
   rclcpp::Node::SharedPtr node_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
+  std::thread callback_group_executor_thread;
 
   std::atomic<bool> is_stuck_;
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp

@@ -74,6 +74,10 @@ class GoalUpdater : public BT::DecoratorNode
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
 
   geometry_msgs::msg::PoseStamped last_goal_received_;
+
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/plugins/action/controller_selector_node.cpp

@@ -33,19 +33,28 @@ ControllerSelector::ControllerSelector(
 : BT::SyncActionNode(name, conf)
 {
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  callback_group_ = node_->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
 
   getInput("topic_name", topic_name_);
 
   rclcpp::QoS qos(rclcpp::KeepLast(1));
   qos.transient_local().reliable();
 
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
   controller_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
-    topic_name_, qos, std::bind(&ControllerSelector::callbackControllerSelect, this, _1));
+    topic_name_,
+    qos,
+    std::bind(&ControllerSelector::callbackControllerSelect, this, _1),
+    sub_option);
 }
 
 BT::NodeStatus ControllerSelector::tick()
 {
-  rclcpp::spin_some(node_);
+  callback_group_executor_.spin_some();
 
   // This behavior always use the last selected controller received from the topic input.
   // When no input is specified it uses the default controller.

nav2_behavior_tree/plugins/action/planner_selector_node.cpp

@@ -33,19 +33,28 @@ PlannerSelector::PlannerSelector(
 : BT::SyncActionNode(name, conf)
 {
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  callback_group_ = node_->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
 
   getInput("topic_name", topic_name_);
 
   rclcpp::QoS qos(rclcpp::KeepLast(1));
   qos.transient_local().reliable();
 
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
   planner_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
-    topic_name_, qos, std::bind(&PlannerSelector::callbackPlannerSelect, this, _1));
+    topic_name_,
+    qos,
+    std::bind(&PlannerSelector::callbackPlannerSelect, this, _1),
+    sub_option);
 }
 
 BT::NodeStatus PlannerSelector::tick()
 {
-  rclcpp::spin_some(node_);
+  callback_group_executor_.spin_some();
 
   // This behavior always use the last selected planner received from the topic input.
   // When no input is specified it uses the default planner.

nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp

@@ -33,15 +33,23 @@ IsBatteryLowCondition::IsBatteryLowCondition(
   getInput("battery_topic", battery_topic_);
   getInput("is_voltage", is_voltage_);
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  callback_group_ = node_->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
   battery_sub_ = node_->create_subscription<sensor_msgs::msg::BatteryState>(
     battery_topic_,
     rclcpp::SystemDefaultsQoS(),
-    std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1));
+    std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1),
+    sub_option);
 }
 
 BT::NodeStatus IsBatteryLowCondition::tick()
 {
-  std::lock_guard<std::mutex> lock(mutex_);
+  callback_group_executor_.spin_some();
   if (is_battery_low_) {
     return BT::NodeStatus::SUCCESS;
   }
@@ -50,7 +58,6 @@ BT::NodeStatus IsBatteryLowCondition::tick()
 
 void IsBatteryLowCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(mutex_);
   if (is_voltage_) {
     is_battery_low_ = msg->voltage <= min_battery_;
   } else {

nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp

@@ -32,11 +32,19 @@ IsStuckCondition::IsStuckCondition(
   brake_accel_limit_(-10.0)
 {
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
-
+  callback_group_ = node_->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+  callback_group_executor_thread = std::thread([this]() {callback_group_executor_.spin();});
+
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
   odom_sub_ = node_->create_subscription<nav_msgs::msg::Odometry>(
     "odom",
     rclcpp::SystemDefaultsQoS(),
-    std::bind(&IsStuckCondition::onOdomReceived, this, std::placeholders::_1));
+    std::bind(&IsStuckCondition::onOdomReceived, this, std::placeholders::_1),
+    sub_option);
 
   RCLCPP_DEBUG(node_->get_logger(), "Initialized an IsStuckCondition BT node");
 
@@ -46,6 +54,8 @@ IsStuckCondition::IsStuckCondition(
 IsStuckCondition::~IsStuckCondition()
 {
   RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStuckCondition BT node");
+  callback_group_executor_.cancel();
+  callback_group_executor_thread.join();
 }
 
 void IsStuckCondition::onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg)

nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp

@@ -33,13 +33,22 @@ GoalUpdater::GoalUpdater(
   const BT::NodeConfiguration & conf)
 : BT::DecoratorNode(name, conf)
 {
-  auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  callback_group_ = node_->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
 
   std::string goal_updater_topic;
-  node->get_parameter_or<std::string>("goal_updater_topic", goal_updater_topic, "goal_update");
-
-  goal_sub_ = node->create_subscription<geometry_msgs::msg::PoseStamped>(
-    goal_updater_topic, 10, std::bind(&GoalUpdater::callback_updated_goal, this, _1));
+  node_->get_parameter_or<std::string>("goal_updater_topic", goal_updater_topic, "goal_update");
+
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
+  goal_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
+    goal_updater_topic,
+    10,
+    std::bind(&GoalUpdater::callback_updated_goal, this, _1),
+    sub_option);
 }
 
 inline BT::NodeStatus GoalUpdater::tick()
@@ -48,6 +57,8 @@ inline BT::NodeStatus GoalUpdater::tick()
 
   getInput("input_goal", goal);
 
+  callback_group_executor_.spin_some();
+
   if (rclcpp::Time(last_goal_received_.header.stamp) > rclcpp::Time(goal.header.stamp)) {
     goal = last_goal_received_;
   }