behavior_tree: migration to BT.CPP 4.5.x (#4128)

* behavior_tree: migration to BT.CPP 4.5.x Signed-off-by: Davide Faconti <[email protected]> * fix formatting Signed-off-by: Davide Faconti <[email protected]> * fix lint warning Signed-off-by: Davide Faconti <[email protected]> * remove setStatus(BT::NodeStatus::IDLE) Signed-off-by: Davide Faconti <[email protected]> * handle SKIPPED state Signed-off-by: Davide Faconti <[email protected]> * fix SequenceStar Signed-off-by: Davide Faconti <[email protected]> * add tests Signed-off-by: Davide Faconti <[email protected]> * fix Signed-off-by: Davide Faconti <[email protected]> * fix unit tests Signed-off-by: Davide Faconti <[email protected]> * add emitWakeUpSignal() Signed-off-by: Davide Faconti <[email protected]> * fit test Signed-off-by: Davide Faconti <[email protected]> * fix Signed-off-by: Davide Faconti <[email protected]> * remove SequenceStar Signed-off-by: Davide Faconti <[email protected]> * remove SequenceStar Signed-off-by: Davide Faconti <[email protected]> * keep resetStatus() in action_node Signed-off-by: Davide Faconti <[email protected]> * add suggestions from peer reviewer Signed-off-by: Davide Faconti <[email protected]> * fix compilation Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]>
ros-navigation · Feb 29, 2024 · efd777e · efd777e
1 parent d509fbf
commit efd777e
Show file tree

Hide file tree

Showing 160 changed files with 480 additions and 394 deletions.
diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
@@ -11,7 +11,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nav2_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
-find_package(behaviortree_cpp_v3 REQUIRED)
+find_package(behaviortree_cpp REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
@@ -34,7 +34,7 @@ set(dependencies
   sensor_msgs
   nav2_msgs
   nav_msgs
-  behaviortree_cpp_v3
+  behaviortree_cpp
   tf2
   tf2_ros
   tf2_geometry_msgs

diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md
@@ -41,7 +41,7 @@ BehaviorTreeEngine::BehaviorTreeEngine()
 Once a new node is registered with the factory, it is now available to the BehaviorTreeEngine and can be used in Behavior Trees. For example, the following simple XML description of a BT shows the FollowPath node in use:
 
 ```XML
-<root main_tree_to_execute="MainTree">
+<root BTCPP_format="4">
   <BehaviorTree ID="MainTree">
     <Sequence name="root">
       <ComputePathToPose goal="${goal}"/>

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp
@@ -20,9 +20,9 @@
 #include <string>
 #include <vector>
 
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
-#include "behaviortree_cpp_v3/xml_parsing.h"
+#include "behaviortree_cpp/behavior_tree.h"
+#include "behaviortree_cpp/bt_factory.h"
+#include "behaviortree_cpp/xml_parsing.h"
 
 #include "rclcpp/rclcpp.hpp"
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
@@ -19,7 +19,7 @@
 #include <string>
 #include <chrono>
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 #include "nav2_util/node_utils.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
@@ -188,7 +188,7 @@ class BtActionNode : public BT::ActionNodeBase
   BT::NodeStatus tick() override
   {
     // first step to be done only at the beginning of the Action
-    if (status() == BT::NodeStatus::IDLE) {
+    if (!BT::isStatusActive(status())) {
       // setting the status to RUNNING to notify the BT Loggers (if any)
       setStatus(BT::NodeStatus::RUNNING);
 
@@ -325,7 +325,9 @@ class BtActionNode : public BT::ActionNodeBase
       on_cancelled();
     }
 
-    setStatus(BT::NodeStatus::IDLE);
+    // this is probably redundant, since the parent node
+    // is supposed to call it, but we keep it, just in case
+    resetStatus();
   }
 
 protected:
@@ -376,12 +378,14 @@ class BtActionNode : public BT::ActionNodeBase
         if (this->goal_handle_->get_goal_id() == result.goal_id) {
           goal_result_available_ = true;
           result_ = result;
+          emitWakeUpSignal();
         }
       };
     send_goal_options.feedback_callback =
       [this](typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr,
         const std::shared_ptr<const typename ActionT::Feedback> feedback) {
         feedback_ = feedback;
+        emitWakeUpSignal();
       };
 
     future_goal_handle_ = std::make_shared<
@@ -434,9 +438,9 @@ class BtActionNode : public BT::ActionNodeBase
   void increment_recovery_count()
   {
     int recovery_count = 0;
-    config().blackboard->template get<int>("number_recoveries", recovery_count);  // NOLINT
+    [[maybe_unused]] auto res = config().blackboard->get("number_recoveries", recovery_count);  // NOLINT
     recovery_count += 1;
-    config().blackboard->template set<int>("number_recoveries", recovery_count);  // NOLINT
+    config().blackboard->set("number_recoveries", recovery_count);  // NOLINT
   }
 
   std::string action_name_;

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -237,7 +237,8 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
   // Create the Behavior Tree from the XML input
   try {
     tree_ = bt_->createTreeFromFile(filename, blackboard_);
-    for (auto & blackboard : tree_.blackboard_stack) {
+    for (auto & subtree : tree_.subtrees) {
+      auto & blackboard = subtree->blackboard;
       blackboard->set("node", client_node_);
       blackboard->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);
       blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp
@@ -19,7 +19,7 @@
 #include <string>
 #include <chrono>
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 #include "nav2_util/node_utils.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
@@ -19,7 +19,7 @@
 #include <memory>
 #include <chrono>
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 #include "nav2_util/node_utils.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
@@ -157,7 +157,7 @@ class BtServiceNode : public BT::ActionNodeBase
   void halt() override
   {
     request_sent_ = false;
-    setStatus(BT::NodeStatus::IDLE);
+    resetStatus();
   }
 
   /**
@@ -230,9 +230,9 @@ class BtServiceNode : public BT::ActionNodeBase
   void increment_recovery_count()
   {
     int recovery_count = 0;
-    config().blackboard->template get<int>("number_recoveries", recovery_count);  // NOLINT
+    [[maybe_unused]] auto res = config().blackboard->get("number_recoveries", recovery_count);  // NOLINT
     recovery_count += 1;
-    config().blackboard->template set<int>("number_recoveries", recovery_count);  // NOLINT
+    config().blackboard->set("number_recoveries", recovery_count);  // NOLINT
   }
 
   std::string service_name_, service_node_name_;

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp
@@ -20,7 +20,7 @@
 
 #include "rclcpp/time.hpp"
 #include "rclcpp/node.hpp"
-#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp/behavior_tree.h"
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp
@@ -21,7 +21,7 @@
 
 #include "std_msgs/msg/string.hpp"
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 
 #include "rclcpp/rclcpp.hpp"
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp
@@ -21,7 +21,7 @@
 
 #include "std_msgs/msg/string.hpp"
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 
 #include "rclcpp/rclcpp.hpp"
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp
@@ -21,7 +21,7 @@
 
 #include "std_msgs/msg/string.hpp"
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 
 #include "rclcpp/rclcpp.hpp"
 

diff --git a/...ehavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp b/...ehavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp
@@ -20,7 +20,7 @@
 
 #include "std_msgs/msg/string.hpp"
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 
 #include "rclcpp/rclcpp.hpp"
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp
@@ -22,7 +22,7 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp
@@ -21,7 +21,7 @@
 
 #include "std_msgs/msg/string.hpp"
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 
 #include "rclcpp/rclcpp.hpp"
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp
@@ -21,7 +21,7 @@
 
 #include "nav_msgs/msg/path.hpp"
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp
@@ -22,7 +22,7 @@
 
 #include "nav_msgs/msg/path.hpp"
 
-#include "behaviortree_cpp_v3/action_node.h"
+#include "behaviortree_cpp/action_node.h"
 #include "tf2_ros/buffer.h"
 
 namespace nav2_behavior_tree

diff --git a/...r_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp b/...r_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp
@@ -21,7 +21,7 @@
 #include <set>
 
 #include "rclcpp/rclcpp.hpp"
-#include "behaviortree_cpp_v3/condition_node.h"
+#include "behaviortree_cpp/condition_node.h"
 
 namespace nav2_behavior_tree
 {

diff --git a/...ehavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/...ehavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp
@@ -19,7 +19,7 @@
 #include <string>
 #include <memory>
 
-#include "behaviortree_cpp_v3/condition_node.h"
+#include "behaviortree_cpp/condition_node.h"
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"

diff --git a/...ior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp b/...ior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp
@@ -20,7 +20,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "behaviortree_cpp_v3/condition_node.h"
+#include "behaviortree_cpp/condition_node.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp
@@ -19,7 +19,7 @@
 #include <memory>
 
 #include "rclcpp/rclcpp.hpp"
-#include "behaviortree_cpp_v3/condition_node.h"
+#include "behaviortree_cpp/condition_node.h"
 #include "tf2_ros/buffer.h"
 
 namespace nav2_behavior_tree

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp
@@ -18,7 +18,7 @@
 #include <string>
 #include <vector>
 
-#include "behaviortree_cpp_v3/condition_node.h"
+#include "behaviortree_cpp/condition_node.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
 namespace nav2_behavior_tree

diff --git a/...ior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp b/...ior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp
@@ -16,7 +16,7 @@
 #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_
 
 #include <string>
-#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp/behavior_tree.h"
 
 namespace nav2_behavior_tree
 {

diff --git a/...avior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp b/...avior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp
@@ -21,7 +21,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "sensor_msgs/msg/battery_state.hpp"
-#include "behaviortree_cpp_v3/condition_node.h"
+#include "behaviortree_cpp/condition_node.h"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp
@@ -22,7 +22,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "sensor_msgs/msg/battery_state.hpp"
-#include "behaviortree_cpp_v3/condition_node.h"
+#include "behaviortree_cpp/condition_node.h"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp
@@ -19,7 +19,7 @@
 #include <memory>
 
 #include "rclcpp/rclcpp.hpp"
-#include "behaviortree_cpp_v3/condition_node.h"
+#include "behaviortree_cpp/condition_node.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_msgs/srv/is_path_valid.hpp"
 

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp
@@ -20,7 +20,7 @@
 #include <deque>
 
 #include "rclcpp/rclcpp.hpp"
-#include "behaviortree_cpp_v3/condition_node.h"
+#include "behaviortree_cpp/condition_node.h"
 #include "nav_msgs/msg/odometry.hpp"
 
 namespace nav2_behavior_tree

diff --git a/...avior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp b/...avior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp
@@ -18,7 +18,7 @@
 #include <string>
 
 #include "rclcpp/rclcpp.hpp"
-#include "behaviortree_cpp_v3/condition_node.h"
+#include "behaviortree_cpp/condition_node.h"
 #include "nav_msgs/msg/path.hpp"
 
 namespace nav2_behavior_tree

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp
@@ -19,7 +19,7 @@
 #include <string>
 
 #include "rclcpp/rclcpp.hpp"
-#include "behaviortree_cpp_v3/condition_node.h"
+#include "behaviortree_cpp/condition_node.h"
 
 namespace nav2_behavior_tree
 {

diff --git a/...avior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp b/...avior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp
@@ -20,7 +20,7 @@
 #include <memory>
 
 #include "rclcpp/rclcpp.hpp"
-#include "behaviortree_cpp_v3/condition_node.h"
+#include "behaviortree_cpp/condition_node.h"
 #include "tf2_ros/buffer.h"
 
 namespace nav2_behavior_tree

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp
@@ -16,8 +16,8 @@
 #define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_
 
 #include <string>
-#include "behaviortree_cpp_v3/control_node.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/control_node.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp
@@ -16,7 +16,7 @@
 #define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__RECOVERY_NODE_HPP_
 
 #include <string>
-#include "behaviortree_cpp_v3/control_node.h"
+#include "behaviortree_cpp/control_node.h"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp
@@ -17,8 +17,8 @@
 
 #include <string>
 
-#include "behaviortree_cpp_v3/control_node.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/control_node.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp
@@ -22,7 +22,7 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "tf2_ros/buffer.h"
 
-#include "behaviortree_cpp_v3/decorator_node.h"
+#include "behaviortree_cpp/decorator_node.h"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp
@@ -19,7 +19,7 @@
 #include <string>
 #include <vector>
 
-#include "behaviortree_cpp_v3/decorator_node.h"
+#include "behaviortree_cpp/decorator_node.h"
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"