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

* behavior_tree: migration to BT.CPP 4.5.x Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix formatting Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix lint warning Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove setStatus(BT::NodeStatus::IDLE) Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * handle SKIPPED state Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix SequenceStar Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix unit tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add emitWakeUpSignal() Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fit test Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove SequenceStar Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove SequenceStar Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * keep resetStatus() in action_node Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add suggestions from peer reviewer Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix compilation Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com>
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"