Revert double spin (#5477)

* Revert "Add double spin_some in some BT nodes (#5055)"

This reverts commit 4e8469e.

Signed-off-by: mini-1235 <[email protected]>

* Update spin some to use spin all

Signed-off-by: mini-1235 <[email protected]>

* Replace 50 ms with bt loop duration

Signed-off-by: mini-1235 <[email protected]>

---------

Signed-off-by: mini-1235 <[email protected]>

Author: mini-1235

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.