Skip to content

Commit

Permalink
behavior_tree: migration to BT.CPP 4.5.x (ros-navigation#4128)
Browse files Browse the repository at this point in the history
* 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]>
  • Loading branch information
facontidavide authored and ajtudela committed Apr 19, 2024
1 parent 97c7c06 commit 1ed2776
Show file tree
Hide file tree
Showing 160 changed files with 480 additions and 394 deletions.
4 changes: 2 additions & 2 deletions nav2_behavior_tree/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -34,7 +34,7 @@ set(dependencies
sensor_msgs
nav2_msgs
nav_msgs
behaviortree_cpp_v3
behaviortree_cpp
tf2
tf2_ros
tf2_geometry_msgs
Expand Down
2 changes: 1 addition & 1 deletion nav2_behavior_tree/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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}"/>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand Down
14 changes: 9 additions & 5 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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<
Expand Down Expand Up @@ -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_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -157,7 +157,7 @@ class BtServiceNode : public BT::ActionNodeBase
void halt() override
{
request_sent_ = false;
setStatus(BT::NodeStatus::IDLE);
resetStatus();
}

/**
Expand Down Expand Up @@ -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_;
Expand Down
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"


Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
Loading

0 comments on commit 1ed2776

Please sign in to comment.