diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
index c6092591e83ac1408fb1625750ec7443b08f47f5..86da24c7b40020c6eb99d624b4a433c212c4401e 100644
--- a/nav2_behavior_tree/CMakeLists.txt
+++ b/nav2_behavior_tree/CMakeLists.txt
@@ -11,7 +11,7 @@ find_package(builtin_interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
-find_package(behaviortree_cpp REQUIRED)
+find_package(behaviortree_cpp_v3 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
@@ -38,7 +38,7 @@ set(dependencies
geometry_msgs
nav2_msgs
nav_msgs
- behaviortree_cpp
+ behaviortree_cpp_v3
tf2
tf2_ros
tf2_geometry_msgs
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp
index 5a56395bacaed5777032d808be3fc87f2bcd30d0..abc421cbc197cc838d380f2e5e5c1df71f57e592 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/back_up_action.hpp
@@ -28,19 +28,15 @@ namespace nav2_behavior_tree
class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
{
public:
- explicit BackUpAction(
+ BackUpAction(
const std::string & action_name,
- const BT::NodeParameters & params)
- : BtActionNode<nav2_msgs::action::BackUp>(action_name, params)
- {
- }
-
- void on_init() override
+ const BT::NodeConfiguration & conf)
+ : BtActionNode<nav2_msgs::action::BackUp>(action_name, conf)
{
double dist;
- getParam<double>("backup_dist", dist);
+ getInput("backup_dist", dist);
double speed;
- getParam<double>("backup_speed", speed);
+ getInput("backup_speed", speed);
// silently fix, vector direction determined by distance sign
if (speed < 0.0) {
@@ -54,11 +50,12 @@ public:
goal_.speed = speed;
}
- static const BT::NodeParameters & requiredNodeParameters()
+ static BT::PortsList providedPorts()
{
- static BT::NodeParameters params = {
- {"backup_dist", "-0.15"}, {"backup_speed", "0.025"}};
- return params;
+ return {
+ BT::InputPort<double>("backup_dist", -0.15, "Distance to backup"),
+ BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup")
+ };
}
};
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
index 429c8fa3fbcb68a22350529917dda9f3bf9c48ba..31e0b530ae9a10ae1c473a5f5ffb68f5aab84936 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp
@@ -19,7 +19,6 @@
#include <string>
#include "behaviortree_cpp/behavior_tree.h"
-#include "behaviortree_cpp/blackboard/blackboard_local.h"
#include "behaviortree_cpp/bt_factory.h"
#include "behaviortree_cpp/xml_parsing.h"
@@ -47,7 +46,9 @@ public:
std::function<bool()> cancelRequested,
std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10));
- BT::Tree buildTreeFromText(std::string & xml_string, BT::Blackboard::Ptr blackboard);
+ BT::Tree buildTreeFromText(
+ const std::string & xml_string,
+ BT::Blackboard::Ptr blackboard);
void haltAllActions(BT::TreeNode * root_node)
{
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
index 9d7553221459186711bcb5164d57dd5f2d873531..9208cd2284fa7092fc4b5a06d68fdb1dc3ffc3a3 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
@@ -29,29 +29,12 @@ template<class ActionT>
class BtActionNode : public BT::CoroActionNode
{
public:
- explicit BtActionNode(const std::string & action_name)
- : BT::CoroActionNode(action_name), action_name_(action_name)
+ BtActionNode(
+ const std::string & action_name,
+ const BT::NodeConfiguration & conf)
+ : BT::CoroActionNode(action_name, conf), action_name_(action_name)
{
- }
-
- BtActionNode(const std::string & action_name, const BT::NodeParameters & params)
- : BT::CoroActionNode(action_name, params), action_name_(action_name)
- {
- }
-
- BtActionNode() = delete;
-
- virtual ~BtActionNode()
- {
- }
-
- // This is a callback from the BT library invoked after the node is created and after the
- // blackboard has been set for the node by the library. It is the first opportunity for
- // the node to access the blackboard. Derived classes do not override this method,
- // but override on_init instead.
- void onInit() final
- {
- node_ = blackboard()->template get<rclcpp::Node::SharedPtr>("node");
+ node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
// Initialize the input and output messages
goal_ = typename ActionT::Goal();
@@ -59,7 +42,7 @@ public:
// Get the required items from the blackboard
node_loop_timeout_ =
- blackboard()->template get<std::chrono::milliseconds>("node_loop_timeout");
+ config().blackboard->get<std::chrono::milliseconds>("node_loop_timeout");
// Now that we have the ROS node to use, create the action client for this BT action
action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name_);
@@ -69,18 +52,18 @@ public:
action_client_->wait_for_action_server();
// Give the derive class a chance to do any initialization
- on_init();
RCLCPP_INFO(node_->get_logger(), "\"%s\" BtActionNode initialized", action_name_.c_str());
}
- // Derived classes can override any of the following methods to hook into the
- // processing for the action: on_init, on_tick, on_loop_timeout, and on_success
+ BtActionNode() = delete;
- // Perform any local initialization such as getting values from the blackboard
- virtual void on_init()
+ virtual ~BtActionNode()
{
}
+ // Derived classes can override any of the following methods to hook into the
+ // processing for the action: on_tick, on_loop_timeout, and on_success
+
// Could do dynamic checks, such as getting updates to values on the blackboard
virtual void on_tick()
{
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp
index 223d420321e871c784bd74535175a0853adddb68..ead7ed686a3d74306265f60b076e079b131efd5d 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp
@@ -27,7 +27,7 @@ namespace BT
// data type.
template<>
-inline geometry_msgs::msg::Point convertFromString(const StringView & key)
+inline geometry_msgs::msg::Point convertFromString(const StringView key)
{
// three real numbers separated by semicolons
auto parts = BT::splitString(key, ';');
@@ -43,7 +43,7 @@ inline geometry_msgs::msg::Point convertFromString(const StringView & key)
}
template<>
-inline geometry_msgs::msg::Quaternion convertFromString(const StringView & key)
+inline geometry_msgs::msg::Quaternion convertFromString(const StringView key)
{
// three real numbers separated by semicolons
auto parts = BT::splitString(key, ';');
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
index d6cc33ef8b4dfc8ee31bf41640202a635683fb8b..8f40866bfcd40eef08652a71087140531efb807d 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
@@ -31,39 +31,17 @@ class BtServiceNode : public BT::CoroActionNode
public:
BtServiceNode(
const std::string & service_node_name,
- const BT::NodeParameters & params)
- : BT::CoroActionNode(service_node_name, params), service_node_name_(service_node_name)
+ const BT::NodeConfiguration & conf)
+ : BT::CoroActionNode(service_node_name, conf), service_node_name_(service_node_name)
{
- }
-
- BtServiceNode() = delete;
-
- virtual ~BtServiceNode()
- {
- }
-
- // Any BT node that accepts parameters must provide a requiredNodeParameters method
- static const BT::NodeParameters & requiredNodeParameters()
- {
- static BT::NodeParameters params = {{"service_name",
- "please_set_service_name_in_BT_Node"}};
- return params;
- }
-
- // This is a callback from the BT library invoked after the node
- // is created and after the blackboard has been set for the node
- // by the library. It is the first opportunity for the node to
- // access the blackboard. Derived classes do not override this method,
- // but override on_init instead.
- void onInit() final
- {
- node_ = blackboard()->template get<rclcpp::Node::SharedPtr>("node");
+ node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
// Get the required items from the blackboard
node_loop_timeout_ =
- blackboard()->template get<std::chrono::milliseconds>("node_loop_timeout");
+ config().blackboard->get<std::chrono::milliseconds>("node_loop_timeout");
// Now that we have node_ to use, create the service client for this BT service
+ getInput("service_name", service_name_);
service_client_ = node_->create_client<ServiceT>(service_name_);
// Make sure the server is actually there before continuing
@@ -73,9 +51,20 @@ public:
RCLCPP_INFO(node_->get_logger(), "\"%s\" BtServiceNode initialized",
service_node_name_.c_str());
+ }
+
+ BtServiceNode() = delete;
+
+ virtual ~BtServiceNode()
+ {
+ }
- // Give user a chance for initialization and get the service name
- on_init();
+ // Any BT node that accepts parameters must provide a requiredNodeParameters method
+ static BT::PortsList providedPorts()
+ {
+ return {
+ BT::InputPort<std::string>("service_name", "please_set_service_name_in_BT_Node")
+ };
}
// The main override required by a BT service
@@ -100,11 +89,6 @@ public:
request_ = std::make_shared<typename ServiceT::Request>();
}
- // Perform local initialization such as getting values from the blackboard
- virtual void on_init()
- {
- }
-
protected:
std::string service_name_, service_node_name_;
typename std::shared_ptr<rclcpp::Client<ServiceT>> service_client_;
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/clear_costmap_service.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/clear_costmap_service.hpp
index 759e0d618196953baf415767411f1e3ab3419205..a0532980f92ebab5cd6e24a0329fc11267c45528 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/clear_costmap_service.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/clear_costmap_service.hpp
@@ -28,15 +28,10 @@ namespace nav2_behavior_tree
class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>
{
public:
- explicit ClearEntireCostmapService(
+ ClearEntireCostmapService(
const std::string & service_node_name,
- const BT::NodeParameters & params)
- : BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>(service_node_name, params)
- {
- getParam<std::string>("service_name", service_name_);
- }
-
- void on_init() override
+ const BT::NodeConfiguration & conf)
+ : BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>(service_node_name, conf)
{
}
};
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp
index 32483fd4f2aa675a607477479f40b01c5f0337f5..91e8bc488274641e1dd8dece446c97dcbeb93cf4 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/compute_path_to_pose_action.hpp
@@ -28,27 +28,37 @@ namespace nav2_behavior_tree
class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePathToPose>
{
public:
- explicit ComputePathToPoseAction(const std::string & action_name)
- : BtActionNode<nav2_msgs::action::ComputePathToPose>(action_name)
+ ComputePathToPoseAction(
+ const std::string & action_name,
+ const BT::NodeConfiguration & conf)
+ : BtActionNode<nav2_msgs::action::ComputePathToPose>(action_name, conf)
{
}
void on_tick() override
{
- goal_.pose = *(blackboard()->get<geometry_msgs::msg::PoseStamped::SharedPtr>("goal"));
+ getInput("goal", goal_.pose);
}
void on_success() override
{
- *(blackboard()->get<nav_msgs::msg::Path::SharedPtr>("path")) = result_.result->path;
+ setOutput("path", result_.result->path);
if (first_time_) {
first_time_ = false;
} else {
- blackboard()->set<bool>("path_updated", true); // NOLINT
+ config().blackboard->set("path_updated", true);
}
}
+ static BT::PortsList providedPorts()
+ {
+ return {
+ BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
+ BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to")
+ };
+ }
+
private:
bool first_time_{true};
};
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/follow_path_action.hpp
index c0e35a557f574b0e3beb483bdbf5bc597b24686e..ecbe918ba9d8cb14bdd363b9bdc7f4e4e97dc2b9 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/follow_path_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/follow_path_action.hpp
@@ -27,34 +27,39 @@ namespace nav2_behavior_tree
class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
{
public:
- explicit FollowPathAction(const std::string & action_name)
- : BtActionNode<nav2_msgs::action::FollowPath>(action_name)
+ FollowPathAction(
+ const std::string & action_name,
+ const BT::NodeConfiguration & conf)
+ : BtActionNode<nav2_msgs::action::FollowPath>(action_name, conf)
{
- }
-
- void on_init() override
- {
- blackboard()->set<bool>("path_updated", false);
+ config().blackboard->set("path_updated", false);
}
void on_tick() override
{
- goal_.path = *(blackboard()->get<nav_msgs::msg::Path::SharedPtr>("path"));
+ getInput("path", goal_.path);
}
void on_loop_timeout() override
{
// Check if the goal has been updated
- if (blackboard()->get<bool>("path_updated")) {
+ if (config().blackboard->get<bool>("path_updated")) {
// Reset the flag in the blackboard
- blackboard()->set<bool>("path_updated", false); // NOLINT
+ config().blackboard->set("path_updated", false);
// Grab the new goal and set the flag so that we send the new goal to
// the action server on the next loop iteration
- goal_.path = *(blackboard()->get<nav_msgs::msg::Path::SharedPtr>("path"));
+ getInput("path", goal_.path);
goal_updated_ = true;
}
}
+
+ static BT::PortsList providedPorts()
+ {
+ return {
+ BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
+ };
+ }
};
} // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/goal_reached_condition.hpp
index 6ab038bfafe259c2d9f92162399fc216fe3076c4..f1488e7c4c0a759549178928cfdc666c7bf9c5f7 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/goal_reached_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/goal_reached_condition.hpp
@@ -30,8 +30,10 @@ namespace nav2_behavior_tree
class GoalReachedCondition : public BT::ConditionNode
{
public:
- explicit GoalReachedCondition(const std::string & condition_name)
- : BT::ConditionNode(condition_name), initialized_(false)
+ GoalReachedCondition(
+ const std::string & condition_name,
+ const BT::NodeConfiguration & conf)
+ : BT::ConditionNode(condition_name, conf), initialized_(false)
{
}
@@ -56,9 +58,9 @@ public:
void initialize()
{
- node_ = blackboard()->template get<rclcpp::Node::SharedPtr>("node");
+ node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
- tf_ = blackboard()->template get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
+ tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
initialized_ = true;
}
@@ -72,10 +74,11 @@ public:
RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
return false;
}
- // TODO(mhpanah): replace this with a function
- blackboard()->get<geometry_msgs::msg::PoseStamped::SharedPtr>("goal", goal_);
- double dx = goal_->pose.position.x - current_pose.pose.position.x;
- double dy = goal_->pose.position.y - current_pose.pose.position.y;
+
+ geometry_msgs::msg::PoseStamped goal;
+ getInput("goal", goal);
+ double dx = goal.pose.position.x - current_pose.pose.position.x;
+ double dy = goal.pose.position.y - current_pose.pose.position.y;
if ( (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_) ) {
return true;
@@ -84,6 +87,13 @@ public:
}
}
+ static BT::PortsList providedPorts()
+ {
+ return {
+ BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination")
+ };
+ }
+
protected:
void cleanup()
{
@@ -92,7 +102,6 @@ protected:
private:
rclcpp::Node::SharedPtr node_;
std::shared_ptr<tf2_ros::Buffer> tf_;
- geometry_msgs::msg::PoseStamped::SharedPtr goal_;
bool initialized_;
double goal_reached_tol_;
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/is_stuck_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/is_stuck_condition.hpp
index 249e0c85dfadba088102a14e8fa3dca4c64f763a..ee864ab841b69264740925a63a2cf3190fdf56f3 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/is_stuck_condition.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/is_stuck_condition.hpp
@@ -34,25 +34,16 @@ namespace nav2_behavior_tree
class IsStuckCondition : public BT::ConditionNode
{
public:
- explicit IsStuckCondition(const std::string & condition_name)
- : BT::ConditionNode(condition_name),
+ IsStuckCondition(
+ const std::string & condition_name,
+ const BT::NodeConfiguration & conf)
+ : BT::ConditionNode(condition_name, conf),
is_stuck_(false),
odom_history_size_(10),
current_accel_(0.0),
brake_accel_limit_(-10.0)
{
- }
-
- IsStuckCondition() = delete;
-
- ~IsStuckCondition()
- {
- RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStuckCondition BT node");
- }
-
- void onInit() override
- {
- node_ = blackboard()->template get<rclcpp::Node::SharedPtr>("node");
+ node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
odom_sub_ = node_->create_subscription<nav_msgs::msg::Odometry>("odom",
rclcpp::SystemDefaultsQoS(),
@@ -63,6 +54,13 @@ public:
RCLCPP_INFO_ONCE(node_->get_logger(), "Waiting on odometry");
}
+ IsStuckCondition() = delete;
+
+ ~IsStuckCondition()
+ {
+ RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStuckCondition BT node");
+ }
+
void onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg)
{
RCLCPP_INFO_ONCE(node_->get_logger(), "Got odometry");
@@ -145,9 +143,7 @@ public:
return false;
}
- void halt() override
- {
- }
+ static BT::PortsList providedPorts() {return {};}
private:
// The node that will be used for any ROS operations
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/navigate_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/navigate_to_pose_action.hpp
index 1b1d42c7f7fe4b7cdecdaa4d53440583fec87ccd..e3dca52f37c773cb67548144fe8da15899b8431b 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/navigate_to_pose_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/navigate_to_pose_action.hpp
@@ -30,8 +30,10 @@ namespace nav2_behavior_tree
class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPose>
{
public:
- NavigateToPoseAction(const std::string & action_name, const BT::NodeParameters & params)
- : BtActionNode<nav2_msgs::action::NavigateToPose>(action_name, params)
+ NavigateToPoseAction(
+ const std::string & action_name,
+ const BT::NodeConfiguration & conf)
+ : BtActionNode<nav2_msgs::action::NavigateToPose>(action_name, conf)
{
}
@@ -41,8 +43,8 @@ public:
geometry_msgs::msg::Point position;
geometry_msgs::msg::Quaternion orientation;
- bool have_position = getParam<geometry_msgs::msg::Point>("position", position);
- bool have_orientation = getParam<geometry_msgs::msg::Quaternion>("orientation", orientation);
+ bool have_position = getInput("position", position);
+ bool have_orientation = getInput("orientation", orientation);
if (!have_position || !have_orientation) {
RCLCPP_ERROR(node_->get_logger(),
@@ -54,10 +56,12 @@ public:
}
// Any BT node that accepts parameters must provide a requiredNodeParameters method
- static const BT::NodeParameters & requiredNodeParameters()
+ static BT::PortsList providedPorts()
{
- static BT::NodeParameters params = {{"position", "0;0;0"}, {"orientation", "0;0;0;0"}};
- return params;
+ return {
+ BT::InputPort<geometry_msgs::msg::Point>("position", "0;0;0", "Position"),
+ BT::InputPort<geometry_msgs::msg::Quaternion>("orientation", "0;0;0;0", "Orientation")
+ };
}
};
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/rate_controller_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/rate_controller_node.hpp
index 48ab6eced2f825a9754289741c6fe21c5c2fc6d9..f66e68afc4282a2258a618be12eb3d100fc3fc33 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/rate_controller_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/rate_controller_node.hpp
@@ -26,19 +26,22 @@ namespace nav2_behavior_tree
class RateController : public BT::DecoratorNode
{
public:
- RateController(const std::string & name, const BT::NodeParameters & params)
- : BT::DecoratorNode(name, params)
+ RateController(
+ const std::string & name,
+ const BT::NodeConfiguration & conf)
+ : BT::DecoratorNode(name, conf)
{
double hz = 1.0;
- getParam<double>("hz", hz);
+ getInput("hz", hz);
period_ = 1.0 / hz;
}
// Any BT node that accepts parameters must provide a requiredNodeParameters method
- static const BT::NodeParameters & requiredNodeParameters()
+ static BT::PortsList providedPorts()
{
- static BT::NodeParameters params = {{"hz", "10"}};
- return params;
+ return {
+ BT::InputPort<double>("hz", 10.0, "Rate")
+ };
}
private:
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/recovery_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/recovery_node.hpp
index dc49d85c8661d946ba432af999abbde4ffebe479..21dcdcbe0cc3ed2958db2caa2f54e8c2c652cf6d 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/recovery_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/recovery_node.hpp
@@ -35,15 +35,16 @@ namespace nav2_behavior_tree
class RecoveryNode : public BT::ControlNode
{
public:
- RecoveryNode(const std::string & name, const BT::NodeParameters & params);
+ RecoveryNode(const std::string & name, const BT::NodeConfiguration & conf);
~RecoveryNode() override = default;
// Any BT node that accepts parameters must provide a requiredNodeParameters method
- static const BT::NodeParameters & requiredNodeParameters()
+ static BT::PortsList providedPorts()
{
- static BT::NodeParameters params = {{"number_of_retries", "1"}};
- return params;
+ return {
+ BT::InputPort<int>("number_of_retries", 1, "Number of retries")
+ };
}
private:
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/reinitialize_global_localization_service.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/reinitialize_global_localization_service.hpp
index f1732596550a44b98f16deff2ee22cc2bded2175..d41ceb521510a4c1bdbd5c121067fd8aa5262d6d 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/reinitialize_global_localization_service.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/reinitialize_global_localization_service.hpp
@@ -28,16 +28,10 @@ namespace nav2_behavior_tree
class ReinitializeGlobalLocalizationService : public BtServiceNode<std_srvs::srv::Empty>
{
public:
- explicit ReinitializeGlobalLocalizationService(
+ ReinitializeGlobalLocalizationService(
const std::string & service_node_name,
- const BT::NodeParameters & params)
- : BtServiceNode<std_srvs::srv::Empty>(service_node_name, params)
- {
- // default should be reinitialize_global_localization
- getParam<std::string>("service_name", service_name_);
- }
-
- void on_init() override
+ const BT::NodeConfiguration & conf)
+ : BtServiceNode<std_srvs::srv::Empty>(service_node_name, conf)
{
}
};
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp
index daa648e8963c4b29bb14bd1c256b9de40ced4dba..38c11dc3619d2bb4dcf70544f4e1ba4b3daa193f 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp
@@ -31,24 +31,21 @@ namespace nav2_behavior_tree
class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
{
public:
- explicit SpinAction(
+ SpinAction(
const std::string & action_name,
- const BT::NodeParameters & params)
- : BtActionNode<nav2_msgs::action::Spin>(action_name, params)
- {
- }
-
- void on_init() override
+ const BT::NodeConfiguration & conf)
+ : BtActionNode<nav2_msgs::action::Spin>(action_name, conf)
{
double dist;
- getParam<double>("spin_dist", dist);
+ getInput("spin_dist", dist);
goal_.target_yaw = dist;
}
- static const BT::NodeParameters & requiredNodeParameters()
+ static BT::PortsList providedPorts()
{
- static BT::NodeParameters params = {{"spin_dist", "1.57"}};
- return params;
+ return {
+ BT::InputPort<double>("spin_dist", 1.57, "Spin distance")
+ };
}
};
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp
index ca4072db911d501995f74054fe4bcbf0f69e385f..f31a8c28bf1e60935e0f9d0282968f8b95e54d08 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/wait_action.hpp
@@ -28,15 +28,13 @@ namespace nav2_behavior_tree
class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
{
public:
- explicit WaitAction(const std::string & action_name, const BT::NodeParameters & params)
- : BtActionNode<nav2_msgs::action::Wait>(action_name, params)
- {
- }
-
- void on_init() override
+ WaitAction(
+ const std::string & action_name,
+ const BT::NodeConfiguration & conf)
+ : BtActionNode<nav2_msgs::action::Wait>(action_name, conf)
{
int duration;
- getParam<int>("wait_duration", duration);
+ getInput("wait_duration", duration);
if (duration <= 0) {
RCLCPP_WARN(node_->get_logger(), "Wait duration is negative or zero "
"(%i). Setting to positive.", duration);
@@ -47,10 +45,11 @@ public:
}
// Any BT node that accepts parameters must provide a requiredNodeParameters method
- static const BT::NodeParameters & requiredNodeParameters()
+ static BT::PortsList providedPorts()
{
- static BT::NodeParameters params = {{"wait_duration", "1"}};
- return params;
+ return {
+ BT::InputPort<int>("wait_duration", 1, "Wait time")
+ };
}
};
diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml
index bd3817aef3886edde35b9b9469a9421618c6ac4c..719d0aa5c2459b8583b538ecad40ea8955aaf149 100644
--- a/nav2_behavior_tree/package.xml
+++ b/nav2_behavior_tree/package.xml
@@ -18,7 +18,7 @@
<build_depend>rclcpp_action</build_depend>
<build_depend>rclcpp_lifecycle</build_depend>
<build_depend>std_msgs</build_depend>
- <build_depend>behaviortree_cpp</build_depend>
+ <build_depend>behaviortree_cpp_v3</build_depend>
<build_depend>builtin_interfaces</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>nav2_msgs</build_depend>
@@ -36,7 +36,7 @@
<exec_depend>rclcpp_action</exec_depend>
<exec_depend>rclcpp_lifecycle</exec_depend>
<exec_depend>std_msgs</exec_depend>
- <exec_depend>behaviortree_cpp</exec_depend>
+ <exec_depend>behaviortree_cpp_v3</exec_depend>
<exec_depend>builtin_interfaces</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>nav2_msgs</exec_depend>
diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp
index 5c3a69fa8c0237037ccdde97522ec25d25b5bb76..bd38d54e72009018e66f0e74d63d904e1d80e5d0 100644
--- a/nav2_behavior_tree/src/behavior_tree_engine.cpp
+++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp
@@ -17,7 +17,6 @@
#include <memory>
#include <string>
-#include "behaviortree_cpp/blackboard/blackboard_local.h"
#include "nav2_behavior_tree/back_up_action.hpp"
#include "nav2_behavior_tree/bt_conversions.hpp"
#include "nav2_behavior_tree/compute_path_to_pose_action.hpp"
@@ -73,7 +72,7 @@ BehaviorTreeEngine::run(
std::chrono::milliseconds loopTimeout)
{
// Parse the input XML and create the corresponding Behavior Tree
- BT::Tree tree = BT::buildTreeFromText(factory_, behavior_tree_xml, blackboard);
+ BT::Tree tree = buildTreeFromText(behavior_tree_xml, blackboard);
rclcpp::WallRate loopRate(loopTimeout);
BT::NodeStatus result = BT::NodeStatus::RUNNING;
@@ -123,15 +122,19 @@ BehaviorTreeEngine::run(
}
BT::Tree
-BehaviorTreeEngine::buildTreeFromText(std::string & xml_string, BT::Blackboard::Ptr blackboard)
+BehaviorTreeEngine::buildTreeFromText(
+ const std::string & xml_string,
+ BT::Blackboard::Ptr blackboard)
{
- return BT::buildTreeFromText(factory_, xml_string, blackboard);
+ BT::XMLParser p(factory_);
+ p.loadFromText(xml_string);
+ return p.instantiateTree(blackboard);
}
BT::NodeStatus
BehaviorTreeEngine::initialPoseReceived(BT::TreeNode & tree_node)
{
- auto initPoseReceived = tree_node.blackboard()->template get<bool>("initial_pose_received");
+ auto initPoseReceived = tree_node.config().blackboard->get<bool>("initial_pose_received");
return initPoseReceived ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
}
diff --git a/nav2_behavior_tree/src/recovery_node.cpp b/nav2_behavior_tree/src/recovery_node.cpp
index d5802ba71f19ad7a9f88fcc6df4353889aa23258..c05193e1ed0fc3afd97323936fa76e1ac075578f 100644
--- a/nav2_behavior_tree/src/recovery_node.cpp
+++ b/nav2_behavior_tree/src/recovery_node.cpp
@@ -17,10 +17,12 @@
namespace nav2_behavior_tree
{
-RecoveryNode::RecoveryNode(const std::string & name, const BT::NodeParameters & params)
-: BT::ControlNode::ControlNode(name, params), current_child_idx_(0), retry_count_(0)
+RecoveryNode::RecoveryNode(
+ const std::string & name,
+ const BT::NodeConfiguration & conf)
+: BT::ControlNode::ControlNode(name, conf), current_child_idx_(0), retry_count_(0)
{
- getParam<unsigned int>("number_of_retries", number_of_retries_);
+ getInput("number_of_retries", number_of_retries_);
}
void RecoveryNode::halt()
diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt
index 478b863e19634da96cc48bb2f539cd8606b351c1..f8ab4f2c0a1a64f1e9d0c18ab0b903738a036bba 100644
--- a/nav2_bt_navigator/CMakeLists.txt
+++ b/nav2_bt_navigator/CMakeLists.txt
@@ -11,7 +11,7 @@ find_package(geometry_msgs REQUIRED)
find_package(nav2_behavior_tree REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(nav2_msgs REQUIRED)
-find_package(behaviortree_cpp REQUIRED)
+find_package(behaviortree_cpp_v3 REQUIRED)
find_package(std_srvs REQUIRED)
find_package(nav2_util REQUIRED)
find_package(tf2_ros REQUIRED)
@@ -43,7 +43,7 @@ set(dependencies
nav2_behavior_tree
nav_msgs
nav2_msgs
- behaviortree_cpp
+ behaviortree_cpp_v3
std_srvs
nav2_util
tf2_ros
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml
index 0a09dfc913a55bf3b7f5ee77e4909b946f708b62..d4cc01745d3267b1cdccfd3ca0af1ce4eb4b0eae 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml
+++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml
@@ -8,10 +8,10 @@
<RateController hz="1.0">
<Fallback>
<GoalReached/>
- <ComputePathToPose goal="${goal}"/>
+ <ComputePathToPose goal="{goal}"/>
</Fallback>
</RateController>
- <FollowPath path="${path}"/>
+ <FollowPath path="{path}"/>
</Sequence>
</BehaviorTree>
-</root>
\ No newline at end of file
+</root>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
index e9f48f217e840c4e94db482c5ee5627a49726632..4f81bac590006fb9dd9ee9712be4d0866102da9d 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
+++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
@@ -10,13 +10,13 @@
<Fallback name="GoalReached">
<GoalReached/>
<RecoveryNode number_of_retries="1" name="ComputePath">
- <ComputePathToPose goal="${goal}" path="${path}"/>
+ <ComputePathToPose goal="{goal}" path="{path}"/>
<ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
</RecoveryNode>
</Fallback>
</RateController>
<RecoveryNode number_of_retries="1" name="FollowPath">
- <FollowPath path="${path}"/>
+ <FollowPath path="{path}"/>
<ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
</RecoveryNode>
</Sequence>
diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
index 03d352a69c2da2c16b359debad4a470f7c5c7e95..7b40f6e50ca91d5522b3dc10b9503b8f00c0b9be 100644
--- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
+++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
@@ -18,7 +18,6 @@
#include <memory>
#include <string>
-#include "behaviortree_cpp/blackboard/blackboard_local.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_behavior_tree/behavior_tree_engine.hpp"
#include "nav2_util/lifecycle_node.hpp"
@@ -65,12 +64,6 @@ protected:
// The blackboard shared by all of the nodes in the tree
BT::Blackboard::Ptr blackboard_;
- // The goal (on the blackboard) to be passed to ComputePath
- std::shared_ptr<geometry_msgs::msg::PoseStamped> goal_;
-
- // The path (on the blackboard) to be returned from ComputePath and sent to the FollowPath task
- std::shared_ptr<nav_msgs::msg::Path> path_;
-
// The XML string that defines the Behavior Tree to create
std::string xml_string_;
diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml
index 38af34754827fe08ab06f456cd5bfb59b518b649..64d508e67a0f2d2a912afac55bc24c1b10b6c9c5 100644
--- a/nav2_bt_navigator/package.xml
+++ b/nav2_bt_navigator/package.xml
@@ -17,13 +17,13 @@
<build_depend>nav2_behavior_tree</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>nav2_msgs</build_depend>
- <build_depend>behaviortree_cpp</build_depend>
+ <build_depend>behaviortree_cpp_v3</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>nav2_util</build_depend>
- <exec_depend>behaviortree_cpp</exec_depend>
+ <exec_depend>behaviortree_cpp_v3</exec_depend>
<exec_depend>rclcpp</exec_depend>
<exec_depend>rclcpp_action</exec_depend>
<exec_depend>rclcpp_lifecycle</exec_depend>
diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
index 8c468614973deff785fbd4ebca5808ff6f47627d..c06b2ea2206b67771f50db5960c27177e5a00f1a 100644
--- a/nav2_bt_navigator/src/bt_navigator.cpp
+++ b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -76,16 +76,10 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
// Create the class that registers our custom nodes and executes the BT
bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>();
- // Create the path that will be returned from ComputePath and sent to FollowPath
- goal_ = std::make_shared<geometry_msgs::msg::PoseStamped>();
- path_ = std::make_shared<nav_msgs::msg::Path>();
-
// Create the blackboard that will be shared by all of the nodes in the tree
- blackboard_ = BT::Blackboard::create<BT::BlackboardLocal>();
+ blackboard_ = BT::Blackboard::create();
// Put items on the blackboard
- blackboard_->set<geometry_msgs::msg::PoseStamped::SharedPtr>("goal", goal_); // NOLINT
- blackboard_->set<nav_msgs::msg::Path::SharedPtr>("path", path_); // NOLINT
blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_); // NOLINT
blackboard_->set<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer", tf_); // NOLINT
blackboard_->set<std::chrono::milliseconds>("node_loop_timeout", std::chrono::milliseconds(10)); // NOLINT
@@ -154,7 +148,6 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
tf_.reset();
tf_listener_.reset();
action_server_.reset();
- path_.reset();
xml_string_.clear();
tree_.reset();
blackboard_.reset();
@@ -239,7 +232,7 @@ BtNavigator::initializeGoalPose()
goal->pose.pose.position.x, goal->pose.pose.position.y);
// Update the goal pose on the blackboard
- *(blackboard_->get<geometry_msgs::msg::PoseStamped::SharedPtr>("goal")) = goal->pose;
+ blackboard_->set("goal", goal->pose);
}
void
diff --git a/tools/ros2_dependencies.repos b/tools/ros2_dependencies.repos
index a6af1cdcd4ae7f48fe702db2a60452b077b02abc..71a7cdf3dec23a7dc6de94f220ca3977302bfd10 100644
--- a/tools/ros2_dependencies.repos
+++ b/tools/ros2_dependencies.repos
@@ -2,7 +2,7 @@ repositories:
BehaviorTree.CPP:
type: git
url: https://github.com/BehaviorTree/BehaviorTree.CPP.git
- version: 2.5.2
+ version: 3.0.9
angles:
type: git
url: https://github.com/ros/angles.git