From 2214a04a61b1dadc2f2518b29a2237bb3c6f190c Mon Sep 17 00:00:00 2001
From: Davide Faconti <davide.faconti@gmail.com>
Date: Fri, 27 Mar 2020 01:08:59 +0100
Subject: [PATCH] BehaviorTree refactoring (#1606)
* Proposed refactoring to avoid issues with CoroAction
* correctly haltAllActions (related to #1600)
* not really needed and will be deprecated soon
* Applying changes suggested in the comments of #1606
- fix haltAllActions
- changes method signature on_success()
- reverts the changes made here:
https://github.com/ros-planning/navigation2/pull/1515/files
* fix warnings and errors
* make uncrustify happy?
* Update bt_navigator.cpp
* Update bt_navigator.cpp
* uncrustify fix
Co-authored-by: daf@blue-ocean-robotics.com <Davide Faconti>
---
.../behavior_tree_engine.hpp | 20 ++--
.../nav2_behavior_tree/bt_action_node.hpp | 107 +++++++++++-------
.../nav2_behavior_tree/bt_service_node.hpp | 4 +-
.../action/compute_path_to_pose_action.cpp | 3 +-
.../nav2_bt_navigator/bt_navigator.hpp | 2 +
nav2_bt_navigator/src/bt_navigator.cpp | 16 +--
6 files changed, 93 insertions(+), 59 deletions(-)
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 dccf466e..6d2220db 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
@@ -44,21 +44,19 @@ public:
const std::string & xml_string,
BT::Blackboard::Ptr blackboard);
+ // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state
void haltAllActions(BT::TreeNode * root_node)
{
- auto visitor = [](BT::TreeNode * node) {
- if (auto action = dynamic_cast<BT::CoroActionNode *>(node)) {
- action->halt();
- }
- };
- BT::applyRecursiveVisitor(root_node, visitor);
- }
+ // this halt signal should propagate through the entire tree.
+ root_node->halt();
+ root_node->setStatus(BT::NodeStatus::IDLE);
- // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state
- void resetTree(BT::TreeNode * root_node)
- {
+ // but, just in case...
auto visitor = [](BT::TreeNode * node) {
- node->setStatus(BT::NodeStatus::IDLE);
+ if (node->status() == BT::NodeStatus::RUNNING) {
+ node->halt();
+ node->setStatus(BT::NodeStatus::IDLE);
+ }
};
BT::applyRecursiveVisitor(root_node, visitor);
}
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 2be52da7..b08ff173 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
@@ -26,14 +26,14 @@ namespace nav2_behavior_tree
{
template<class ActionT>
-class BtActionNode : public BT::CoroActionNode
+class BtActionNode : public BT::ActionNodeBase
{
public:
BtActionNode(
const std::string & xml_tag_name,
const std::string & action_name,
const BT::NodeConfiguration & conf)
- : BT::CoroActionNode(xml_tag_name, conf), action_name_(action_name)
+ : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name)
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
@@ -96,66 +96,71 @@ public:
}
// Called upon successful completion of the action. A derived class can override this
- // method to put a value on the blackboard, for example
- virtual void on_success()
+ // method to put a value on the blackboard, for example.
+ virtual BT::NodeStatus on_success()
{
+ return BT::NodeStatus::SUCCESS;
}
- // The main override required by a BT action
- BT::NodeStatus tick() override
+ // Called when a the action is aborted. By default, the node will return FAILURE.
+ // The user may override it to return another value, instead.
+ virtual BT::NodeStatus on_aborted()
{
- on_tick();
+ return BT::NodeStatus::FAILURE;
+ }
-new_goal_received:
- goal_result_available_ = false;
- auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
- send_goal_options.result_callback =
- [this](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result) {
- if (result.code != rclcpp_action::ResultCode::ABORTED) {
- goal_result_available_ = true;
- result_ = result;
- }
- };
+ // Called when a the action is cancelled. By default, the node will return SUCCESS.
+ // The user may override it to return another value, instead.
+ virtual BT::NodeStatus on_cancelled()
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
- auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);
+ // The main override required by a BT action
+ BT::NodeStatus tick() override
+ {
+ // first step to be done only at the beginning of the Action
+ if (status() == BT::NodeStatus::IDLE) {
+ // setting the status to RUNNING to notify the BT Loggers (if any)
+ setStatus(BT::NodeStatus::RUNNING);
- if (rclcpp::spin_until_future_complete(node_, future_goal_handle) !=
- rclcpp::executor::FutureReturnCode::SUCCESS)
- {
- throw std::runtime_error("send_goal failed");
- }
+ // user defined callback
+ on_tick();
- goal_handle_ = future_goal_handle.get();
- if (!goal_handle_) {
- throw std::runtime_error("Goal was rejected by the action server");
+ on_new_goal_received();
}
- while (rclcpp::ok() && !goal_result_available_) {
+ // The following code corresponds to the "RUNNING" loop
+ if (rclcpp::ok() && !goal_result_available_) {
+ // user defined callback. May modify the value of "goal_updated_"
on_wait_for_result();
- auto status = goal_handle_->get_status();
- if (goal_updated_ && (status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
- status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
+ auto goal_status = goal_handle_->get_status();
+ if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
+ goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
{
goal_updated_ = false;
- goto new_goal_received;
+ on_new_goal_received();
}
- // Yield to any other nodes
- setStatusRunningAndYield();
rclcpp::spin_some(node_);
+
+ // check if, after invoking spin_some(), we finally received the result
+ if (!goal_result_available_) {
+ // Yield this Action, returning RUNNING
+ return BT::NodeStatus::RUNNING;
+ }
}
switch (result_.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
- on_success();
- return BT::NodeStatus::SUCCESS;
+ return on_success();
case rclcpp_action::ResultCode::ABORTED:
- return BT::NodeStatus::FAILURE;
+ return on_aborted();
case rclcpp_action::ResultCode::CANCELED:
- return BT::NodeStatus::SUCCESS;
+ return on_cancelled();
default:
throw std::logic_error("BtActionNode::Tick: invalid status value");
@@ -178,7 +183,6 @@ new_goal_received:
}
setStatus(BT::NodeStatus::IDLE);
- CoroActionNode::halt();
}
protected:
@@ -202,6 +206,33 @@ protected:
return false;
}
+
+ void on_new_goal_received()
+ {
+ goal_result_available_ = false;
+ auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
+ send_goal_options.result_callback =
+ [this](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result) {
+ if (result.code != rclcpp_action::ResultCode::ABORTED) {
+ goal_result_available_ = true;
+ result_ = result;
+ }
+ };
+
+ auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);
+
+ if (rclcpp::spin_until_future_complete(node_, future_goal_handle) !=
+ rclcpp::executor::FutureReturnCode::SUCCESS)
+ {
+ throw std::runtime_error("send_goal failed");
+ }
+
+ goal_handle_ = future_goal_handle.get();
+ if (!goal_handle_) {
+ throw std::runtime_error("Goal was rejected by the action server");
+ }
+ }
+
const std::string action_name_;
typename std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
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 39ef9c19..8fc3ed55 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
@@ -26,13 +26,13 @@ namespace nav2_behavior_tree
{
template<class ServiceT>
-class BtServiceNode : public BT::CoroActionNode
+class BtServiceNode : public BT::SyncActionNode
{
public:
BtServiceNode(
const std::string & service_node_name,
const BT::NodeConfiguration & conf)
- : BT::CoroActionNode(service_node_name, conf), service_node_name_(service_node_name)
+ : BT::SyncActionNode(service_node_name, conf), service_node_name_(service_node_name)
{
node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
index b8028892..1071e5d9 100644
--- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
+++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
@@ -47,7 +47,7 @@ public:
getInput("planner_id", goal_.planner_id);
}
- void on_success() override
+ BT::NodeStatus on_success() override
{
setOutput("path", result_.result->path);
@@ -56,6 +56,7 @@ public:
} else {
config().blackboard->set("path_updated", true);
}
+ return BT::NodeStatus::SUCCESS;
}
static BT::PortsList providedPorts()
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 0ac93a92..fe542f5f 100644
--- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
+++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
@@ -110,6 +110,8 @@ protected:
void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose);
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
+ BT::Tree tree_;
+
// The blackboard shared by all of the nodes in the tree
BT::Blackboard::Ptr blackboard_;
diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
index dc478cb6..0d9f03dd 100644
--- a/nav2_bt_navigator/src/bt_navigator.cpp
+++ b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -129,6 +129,9 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
RCLCPP_DEBUG(get_logger(), "Behavior Tree file: '%s'", bt_xml_filename.c_str());
RCLCPP_DEBUG(get_logger(), "Behavior Tree XML: %s", xml_string_.c_str());
+ // Create the Behavior Tree from the XML input
+ tree_ = bt_->buildTreeFromText(xml_string_, blackboard_);
+
return nav2_util::CallbackReturn::SUCCESS;
}
@@ -159,7 +162,6 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
// TODO(orduno) Fix the race condition between the worker thread ticking the tree
// and the main thread resetting the resources, see #1344
-
goal_sub_.reset();
client_node_.reset();
self_client_.reset();
@@ -172,6 +174,7 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
plugin_lib_names_.clear();
xml_string_.clear();
blackboard_.reset();
+ bt_->haltAllActions(tree_.root_node);
bt_.reset();
RCLCPP_INFO(get_logger(), "Completed Cleaning up");
@@ -211,11 +214,7 @@ BtNavigator::navigateToPose()
return action_server_->is_cancel_requested();
};
-
- // Create the Behavior Tree from the XML input
- BT::Tree tree = bt_->buildTreeFromText(xml_string_, blackboard_);
-
- RosTopicLogger topic_logger(client_node_, tree);
+ RosTopicLogger topic_logger(client_node_, tree_);
auto on_loop = [&]() {
if (action_server_->is_preempt_requested()) {
@@ -227,7 +226,10 @@ BtNavigator::navigateToPose()
};
// Execute the BT that was previously created in the configure step
- nav2_behavior_tree::BtStatus rc = bt_->run(&tree, on_loop, is_canceling);
+ nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling);
+ // Make sure that the Bt is not in a running state from a previous execution
+ // note: if all the ControlNodes are implemented correctly, this is not needed.
+ bt_->haltAllActions(tree_.root_node);
switch (rc) {
case nav2_behavior_tree::BtStatus::SUCCEEDED:
--
GitLab