Add double spin_some in some BT nodes (ros-navigation#5055)

* Add a double spin

Signed-off-by: Alberto Tudela <[email protected]>

* Add comment

Signed-off-by: Alberto Tudela <[email protected]>

* Move spin to constructor

Signed-off-by: Alberto Tudela <[email protected]>

---------

Signed-off-by: Alberto Tudela <[email protected]>

Author: ajtudela

nav2_behavior_tree/plugins/action/controller_selector_node.cpp

@@ -33,6 +33,9 @@ ControllerSelector::ControllerSelector(
 : BT::SyncActionNode(name, conf)
 {
   initialize();
+
+  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
+  callback_group_executor_.spin_some(std::chrono::nanoseconds(1));
 }
 
 void ControllerSelector::initialize()

nav2_behavior_tree/plugins/action/planner_selector_node.cpp

@@ -33,6 +33,9 @@ PlannerSelector::PlannerSelector(
 : BT::SyncActionNode(name, conf)
 {
   initialize();
+
+  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
+  callback_group_executor_.spin_some(std::chrono::nanoseconds(1));
 }
 
 void PlannerSelector::initialize()

nav2_behavior_tree/plugins/action/smoother_selector_node.cpp

@@ -34,6 +34,9 @@ SmootherSelector::SmootherSelector(
 : BT::SyncActionNode(name, conf)
 {
   initialize();
+
+  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
+  callback_group_executor_.spin_some(std::chrono::nanoseconds(1));
 }
 
 void SmootherSelector::initialize()

nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp

@@ -27,6 +27,9 @@ IsBatteryChargingCondition::IsBatteryChargingCondition(
   is_battery_charging_(false)
 {
   initialize();
+
+  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
+  callback_group_executor_.spin_some(std::chrono::nanoseconds(1));
 }
 
 void IsBatteryChargingCondition::initialize()

nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp

@@ -30,6 +30,9 @@ IsBatteryLowCondition::IsBatteryLowCondition(
   is_battery_low_(false)
 {
   initialize();
+
+  // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
+  callback_group_executor_.spin_some(std::chrono::nanoseconds(1));
 }
 
 void IsBatteryLowCondition::initialize()

nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp

@@ -36,6 +36,9 @@ GoalUpdater::GoalUpdater(
   goals_updater_topic_("goals_update")
 {
   initialize();
+
+    // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
+  callback_group_executor_.spin_all(std::chrono::milliseconds(1));
 }
 
 void GoalUpdater::initialize()
@@ -92,8 +95,6 @@ inline BT::NodeStatus GoalUpdater::tick()
   getInput("input_goal", goal);
   getInput("input_goals", goals);
 
-  // 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_set_) {