diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 99c0e249a7d..334bdfe158d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/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("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(node_, action_name); + action_client_ = rclcpp_action::create_client(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 diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 366ce0f87b9..5317b9136ea 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/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("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(service_name_); + service_client_ = node_->create_client( + service_name_, + rmw_qos_profile_services_default, + callback_group_); // Make a request for the service without parameter request_ = std::make_shared(); @@ -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 diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp index d39fa26df10..968750a05e9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp +++ b/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_; }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp index d0757e486c9..3aa9e6573e1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp +++ b/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_; }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp index 6d785f13c93..5c4753cf9a1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp +++ b/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::SharedPtr battery_sub_; std::string battery_topic_; double min_battery_; bool is_voltage_; bool is_battery_low_; - std::mutex mutex_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp index 48dbe5027bd..565f261100f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp +++ b/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 is_stuck_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index 8c9af220126..49dfbc1a0c5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -74,6 +74,10 @@ class GoalUpdater : public BT::DecoratorNode rclcpp::Subscription::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 diff --git a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp index fd165449d59..7d77adbee39 100644 --- a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp @@ -33,19 +33,28 @@ ControllerSelector::ControllerSelector( : BT::SyncActionNode(name, conf) { node_ = config().blackboard->get("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( - 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. diff --git a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp index 225055f80cf..ccb85184127 100644 --- a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp @@ -33,19 +33,28 @@ PlannerSelector::PlannerSelector( : BT::SyncActionNode(name, conf) { node_ = config().blackboard->get("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( - 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. diff --git a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp index 4aad6d16769..b8630261e6e 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/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("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( 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 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 lock(mutex_); if (is_voltage_) { is_battery_low_ = msg->voltage <= min_battery_; } else { diff --git a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp index bdd96e830c4..211254dafe9 100644 --- a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp @@ -32,11 +32,19 @@ IsStuckCondition::IsStuckCondition( brake_accel_limit_(-10.0) { node_ = config().blackboard->get("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( "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) diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index 5279613498b..febaa7de648 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/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("node"); + node_ = config().blackboard->get("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("goal_updater_topic", goal_updater_topic, "goal_update"); - - goal_sub_ = node->create_subscription( - goal_updater_topic, 10, std::bind(&GoalUpdater::callback_updated_goal, this, _1)); + node_->get_parameter_or("goal_updater_topic", goal_updater_topic, "goal_update"); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + goal_sub_ = node_->create_subscription( + 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_; }