From 27b7f38841699107ab9449c3777b919f4e33572f Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Wed, 9 Apr 2025 11:17:12 +0200 Subject: [PATCH] Add double spin_some in some BT nodes Signed-off-by: Alberto Tudela --- .../plugins/action/controller_selector_node.cpp | 3 +++ nav2_behavior_tree/plugins/action/planner_selector_node.cpp | 3 +++ nav2_behavior_tree/plugins/action/smoother_selector_node.cpp | 3 +++ .../plugins/condition/is_battery_charging_condition.cpp | 3 +++ .../plugins/condition/is_battery_low_condition.cpp | 3 +++ nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp | 5 +++-- 6 files changed, 18 insertions(+), 2 deletions(-) diff --git a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp index b58d78bb78f..ee18e746257 100644 --- a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp @@ -50,6 +50,9 @@ ControllerSelector::ControllerSelector( qos, std::bind(&ControllerSelector::callbackControllerSelect, this, _1), sub_option); + + // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin + callback_group_executor_.spin_some(std::chrono::nanoseconds(1)); } BT::NodeStatus ControllerSelector::tick() diff --git a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp index 5d0610c0fa5..8cb436ccd2e 100644 --- a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp @@ -50,6 +50,9 @@ PlannerSelector::PlannerSelector( qos, std::bind(&PlannerSelector::callbackPlannerSelect, this, _1), sub_option); + + // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin + callback_group_executor_.spin_some(std::chrono::nanoseconds(1)); } BT::NodeStatus PlannerSelector::tick() diff --git a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp index c717332c79b..4d11daf7213 100644 --- a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp +++ b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp @@ -51,6 +51,9 @@ SmootherSelector::SmootherSelector( qos, std::bind(&SmootherSelector::callbackSmootherSelect, this, _1), sub_option); + + // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin + callback_group_executor_.spin_some(std::chrono::nanoseconds(1)); } BT::NodeStatus SmootherSelector::tick() diff --git a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp index 27e1bd55fc7..1182669e021 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp @@ -40,6 +40,9 @@ IsBatteryChargingCondition::IsBatteryChargingCondition( rclcpp::SystemDefaultsQoS(), std::bind(&IsBatteryChargingCondition::batteryCallback, this, std::placeholders::_1), sub_option); + + // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin + callback_group_executor_.spin_some(std::chrono::nanoseconds(1)); } BT::NodeStatus IsBatteryChargingCondition::tick() 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 eb0a8be4301..8cac86aed0c 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp @@ -56,6 +56,9 @@ void IsBatteryLowCondition::initialize() std::bind(&IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1), sub_option); } + + // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin + callback_group_executor_.spin_some(std::chrono::nanoseconds(1)); } BT::NodeStatus IsBatteryLowCondition::tick() diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index 4127bc68439..3dfed36a08f 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -49,6 +49,9 @@ GoalUpdater::GoalUpdater( rclcpp::SystemDefaultsQoS(), std::bind(&GoalUpdater::callback_updated_goal, this, _1), sub_option); + + // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin + callback_group_executor_.spin_all(std::chrono::milliseconds(1)); } inline BT::NodeStatus GoalUpdater::tick() @@ -57,8 +60,6 @@ inline BT::NodeStatus GoalUpdater::tick() getInput("input_goal", goal); - // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin - callback_group_executor_.spin_all(std::chrono::milliseconds(1)); callback_group_executor_.spin_all(std::chrono::milliseconds(49)); if (last_goal_received_.header.stamp != rclcpp::Time(0)) {