Add double spin_some in some BT nodes (#5073)

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

Author: ajtudela

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()

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()

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()

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()

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()

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)) {