Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 17 commits into from
Feb 29, 2024
Merged

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
17 commits
Select commit Hold shift + click to select a range
afe6fb4
behavior_tree: migration to BT.CPP 4.5.x
facontidavide Feb 20, 2024
8221772
fix formatting
facontidavide Feb 20, 2024
ee9351b
fix lint warning
facontidavide Feb 20, 2024
bb735ed
remove setStatus(BT::NodeStatus::IDLE)
facontidavide Feb 22, 2024
a11ed8c
handle SKIPPED state
facontidavide Feb 22, 2024
1cf3fb7
fix SequenceStar
facontidavide Feb 22, 2024
7f91caa
add tests
facontidavide Feb 27, 2024
8b07e54
fix
facontidavide Feb 27, 2024
4122287
fix unit tests
facontidavide Feb 27, 2024
76a2b67
add emitWakeUpSignal()
facontidavide Feb 28, 2024
517c90f
fit test
facontidavide Feb 28, 2024
7f2d21a
fix
facontidavide Feb 28, 2024
a53373c
remove SequenceStar
facontidavide Feb 29, 2024
1e988d7
remove SequenceStar
facontidavide Feb 29, 2024
6cfd09e
keep resetStatus() in action_node
facontidavide Feb 29, 2024
6719c72
add suggestions from peer reviewer
facontidavide Feb 29, 2024
dad85ca
fix compilation
facontidavide Feb 29, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions nav2_behavior_tree/CMakeLists.txt
View file
Open in desktop
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
View file
Open in desktop
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
6 changes: 3 additions & 3 deletions nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp
View file
Open in desktop
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
View file
Open in desktop
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);
facontidavide marked this conversation as resolved.
Show resolved Hide resolved
// 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
3 changes: 2 additions & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp
View file
Open in desktop
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
8 changes: 4 additions & 4 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
View file
Open in desktop
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);
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
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
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
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
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/planner_selector_node.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion ...ehavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion ...r_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion ...ehavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion ...ior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion ...ior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion ...avior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion ...avior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion ...avior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp
View file
Open in desktop
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
4 changes: 2 additions & 2 deletions nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp
View file
Open in desktop
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
4 changes: 2 additions & 2 deletions nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp
View file
Open in desktop
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
Loading