diff --git a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp b/nav2_behavior_tree/plugins/action/controller_selector_node.cpp index 8518a1a818f..f1730870b4b 100644 --- a/nav2_behavior_tree/plugins/action/controller_selector_node.cpp +++ b/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() diff --git a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp b/nav2_behavior_tree/plugins/action/planner_selector_node.cpp index e25e2694fdf..4e9c1d1587d 100644 --- a/nav2_behavior_tree/plugins/action/planner_selector_node.cpp +++ b/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() diff --git a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp index 254d45f8258..58dad4866db 100644 --- a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp +++ b/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() 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 4d364744412..2135e86bf6a 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp +++ b/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() 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 db1710027a9..16130224f59 100644 --- a/nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp +++ b/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() diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index de969937704..2e445c6a3e7 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/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_) {