Revert double spin

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp

@@ -18,6 +18,7 @@
 
 #include <memory>
 #include <string>
+#include <chrono>
 
 #include "std_msgs/msg/string.hpp"
 
@@ -102,6 +103,7 @@ class ControllerSelector : public BT::SyncActionNode
   rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   std::string topic_name_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp

@@ -18,6 +18,7 @@
 
 #include <memory>
 #include <string>
+#include <chrono>
 
 #include "std_msgs/msg/string.hpp"
 
@@ -102,6 +103,7 @@ class GoalCheckerSelector : public BT::SyncActionNode
   rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   std::string topic_name_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp

@@ -18,6 +18,7 @@
 
 #include <memory>
 #include <string>
+#include <chrono>
 
 #include "std_msgs/msg/string.hpp"
 
@@ -103,6 +104,7 @@ class PlannerSelector : public BT::SyncActionNode
   rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   std::string topic_name_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp

@@ -17,6 +17,7 @@
 
 #include <memory>
 #include <string>
+#include <chrono>
 
 #include "std_msgs/msg/string.hpp"
 
@@ -101,6 +102,7 @@ class ProgressCheckerSelector : public BT::SyncActionNode
   rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   std::string topic_name_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp

@@ -18,6 +18,7 @@
 
 #include <memory>
 #include <string>
+#include <chrono>
 
 #include "std_msgs/msg/string.hpp"
 
@@ -102,6 +103,7 @@ class SmootherSelector : public BT::SyncActionNode
   rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
 
   std::string topic_name_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp

@@ -18,6 +18,7 @@
 #include <string>
 #include <memory>
 #include <mutex>
+#include <chrono>
 
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "sensor_msgs/msg/battery_state.hpp"
@@ -85,6 +86,7 @@ class IsBatteryChargingCondition : public BT::ConditionNode
   nav2::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_sub_;
   std::string battery_topic_;
   bool is_battery_charging_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp

@@ -19,6 +19,7 @@
 #include <string>
 #include <memory>
 #include <mutex>
+#include <chrono>
 
 #include "nav2_ros_common/lifecycle_node.hpp"
 #include "sensor_msgs/msg/battery_state.hpp"
@@ -93,6 +94,7 @@ class IsBatteryLowCondition : public BT::ConditionNode
   double min_battery_;
   bool is_voltage_;
   bool is_battery_low_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp

@@ -19,6 +19,7 @@
 
 #include <memory>
 #include <string>
+#include <chrono>
 
 #include "behaviortree_cpp/decorator_node.h"
 #include "behaviortree_cpp/json_export.h"
@@ -110,6 +111,7 @@ class GoalUpdater : public BT::DecoratorNode
   rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
   std::string goal_updater_topic_;
   std::string goals_updater_topic_;
+  std::chrono::milliseconds bt_loop_duration_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/plugins/action/controller_selector_node.cpp

@@ -33,9 +33,8 @@ 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));
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
 }
 
 void ControllerSelector::initialize()
@@ -69,7 +68,7 @@ BT::NodeStatus ControllerSelector::tick()
     initialize();
   }
 
-  callback_group_executor_.spin_some();
+  callback_group_executor_.spin_all(bt_loop_duration_);
 
   // This behavior always use the last selected controller received from the topic input.
   // When no input is specified it uses the default controller.

nav2_behavior_tree/plugins/action/goal_checker_selector_node.cpp

@@ -33,6 +33,8 @@ GoalCheckerSelector::GoalCheckerSelector(
 : BT::SyncActionNode(name, conf)
 {
   initialize();
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
 }
 
 void GoalCheckerSelector::initialize()
@@ -66,7 +68,7 @@ BT::NodeStatus GoalCheckerSelector::tick()
     initialize();
   }
 
-  callback_group_executor_.spin_some();
+  callback_group_executor_.spin_all(bt_loop_duration_);
 
   // This behavior always use the last selected goal checker received from the topic input.
   // When no input is specified it uses the default goal checker.

nav2_behavior_tree/plugins/action/planner_selector_node.cpp

@@ -33,9 +33,8 @@ 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));
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
 }
 
 void PlannerSelector::initialize()
@@ -69,7 +68,7 @@ BT::NodeStatus PlannerSelector::tick()
     initialize();
   }
 
-  callback_group_executor_.spin_some();
+  callback_group_executor_.spin_all(bt_loop_duration_);
 
   // This behavior always use the last selected planner received from the topic input.
   // When no input is specified it uses the default planner.

nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp

@@ -32,6 +32,8 @@ ProgressCheckerSelector::ProgressCheckerSelector(
 : BT::SyncActionNode(name, conf)
 {
   initialize();
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
 }
 
 void ProgressCheckerSelector::initialize()
@@ -65,7 +67,7 @@ BT::NodeStatus ProgressCheckerSelector::tick()
     initialize();
   }
 
-  callback_group_executor_.spin_some();
+  callback_group_executor_.spin_all(bt_loop_duration_);
 
   // This behavior always use the last selected progress checker received from the topic input.
   // When no input is specified it uses the default goaprogressl checker.

nav2_behavior_tree/plugins/action/smoother_selector_node.cpp

@@ -34,9 +34,8 @@ 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));
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
 }
 
 void SmootherSelector::initialize()
@@ -70,7 +69,7 @@ BT::NodeStatus SmootherSelector::tick()
     initialize();
   }
 
-  callback_group_executor_.spin_some();
+  callback_group_executor_.spin_all(bt_loop_duration_);
 
   // This behavior always use the last selected smoother received from the topic input.
   // When no input is specified it uses the default smoother.

nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp

@@ -27,9 +27,8 @@ 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));
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
 }
 
 void IsBatteryChargingCondition::initialize()
@@ -65,7 +64,7 @@ BT::NodeStatus IsBatteryChargingCondition::tick()
     initialize();
   }
 
-  callback_group_executor_.spin_some();
+  callback_group_executor_.spin_all(bt_loop_duration_);
   if (is_battery_charging_) {
     return BT::NodeStatus::SUCCESS;
   }

nav2_behavior_tree/plugins/condition/is_battery_low_condition.cpp

@@ -30,9 +30,8 @@ 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));
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
 }
 
 void IsBatteryLowCondition::initialize()
@@ -71,7 +70,7 @@ BT::NodeStatus IsBatteryLowCondition::tick()
     initialize();
   }
 
-  callback_group_executor_.spin_some();
+  callback_group_executor_.spin_all(bt_loop_duration_);
   if (is_battery_low_) {
     return BT::NodeStatus::SUCCESS;
   }

nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp

@@ -36,9 +36,8 @@ 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));
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
 }
 
 void GoalUpdater::initialize()
@@ -91,7 +90,7 @@ inline BT::NodeStatus GoalUpdater::tick()
   getInput("input_goal", goal);
   getInput("input_goals", goals);
 
-  callback_group_executor_.spin_all(std::chrono::milliseconds(49));
+  callback_group_executor_.spin_all(bt_loop_duration_);
 
   if (last_goal_received_set_) {
     if (last_goal_received_.header.stamp == rclcpp::Time(0)) {

nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp

@@ -44,6 +44,9 @@ class ControllerSelectorTestFixture : public ::testing::Test
     config_->blackboard = BT::Blackboard::create();
     // Put items on the blackboard
     config_->blackboard->set("node", node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
 
     BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
         return std::make_unique<nav2_behavior_tree::ControllerSelector>(name, config);

nav2_behavior_tree/test/plugins/action/test_goal_checker_selector_node.cpp

@@ -44,6 +44,9 @@ class GoalCheckerSelectorTestFixture : public ::testing::Test
     config_->blackboard = BT::Blackboard::create();
     // Put items on the blackboard
     config_->blackboard->set("node", node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
 
     BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
         return std::make_unique<nav2_behavior_tree::GoalCheckerSelector>(name, config);

nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp

@@ -44,6 +44,9 @@ class PlannerSelectorTestFixture : public ::testing::Test
     config_->blackboard = BT::Blackboard::create();
     // Put items on the blackboard
     config_->blackboard->set("node", node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
 
     BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
         return std::make_unique<nav2_behavior_tree::PlannerSelector>(name, config);

nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp

@@ -43,6 +43,9 @@ class ProgressCheckerSelectorTestFixture : public ::testing::Test
     config_->blackboard = BT::Blackboard::create();
     // Put items on the blackboard
     config_->blackboard->set("node", node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
 
     BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
         return std::make_unique<nav2_behavior_tree::ProgressCheckerSelector>(name, config);

nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp

@@ -44,6 +44,9 @@ class SmootherSelectorTestFixture : public ::testing::Test
     config_->blackboard = BT::Blackboard::create();
     // Put items on the blackboard
     config_->blackboard->set("node", node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
 
     BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
         return std::make_unique<nav2_behavior_tree::SmootherSelector>(name, config);

nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp

@@ -39,6 +39,9 @@ class IsBatteryChargingConditionTestFixture : public ::testing::Test
     config_->blackboard->set(
       "node",
       node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
 
     factory_->registerNodeType<nav2_behavior_tree::IsBatteryChargingCondition>("IsBatteryCharging");
 

nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp

@@ -40,6 +40,9 @@ class IsBatteryLowConditionTestFixture : public ::testing::Test
     config_->blackboard->set(
       "node",
       node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
 
     factory_->registerNodeType<nav2_behavior_tree::IsBatteryLowCondition>("IsBatteryLow");
 

nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp

@@ -43,6 +43,9 @@ class GoalUpdaterTestFixture : public ::testing::Test
     config_->blackboard->set(
       "node",
       node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "bt_loop_duration",
+      std::chrono::milliseconds(10));
 
     BT::NodeBuilder builder =
       [](const std::string & name, const BT::NodeConfiguration & config)

nav2_route/test/test_graph_loader.cpp

@@ -107,7 +107,6 @@ TEST(GraphLoader, test_transformation_api)
   tf_broadcaster->sendTransform(transform);
   rclcpp::Rate(1).sleep();
   tf_broadcaster->sendTransform(transform);
-  rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(1));
   rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50));
 
   graph[0].coords.frame_id = "map_test";

nav2_route/test/test_graph_saver.cpp

@@ -143,7 +143,6 @@ TEST(GraphSaver, test_transformation_api)
   tf_broadcaster->sendTransform(transform);
   rclcpp::Rate(1).sleep();
   tf_broadcaster->sendTransform(transform);
-  rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(1));
   rclcpp::spin_all(node->get_node_base_interface(), std::chrono::milliseconds(50));
 
   GraphSaver graph_saver(node, tf, frame);