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 dccf466e8cc..6d2220db4f0 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 @@ class BehaviorTreeEngine 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(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 2be52da7982..b08ff173343 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 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("node"); @@ -96,66 +96,71 @@ class BtActionNode : public BT::CoroActionNode } // 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::SendGoalOptions(); - send_goal_options.result_callback = - [this](const typename rclcpp_action::ClientGoalHandle::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 @@ class BtActionNode : public BT::CoroActionNode } setStatus(BT::NodeStatus::IDLE); - CoroActionNode::halt(); } protected: @@ -202,6 +206,33 @@ class BtActionNode : public BT::CoroActionNode return false; } + + void on_new_goal_received() + { + goal_result_available_ = false; + auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = + [this](const typename rclcpp_action::ClientGoalHandle::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> 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 39ef9c191ba..8fc3ed55011 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 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("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 b8028892cbf..1071e5d99ff 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 @@ class ComputePathToPoseAction : public BtActionNodepath); @@ -56,6 +56,7 @@ class ComputePathToPoseAction : public BtActionNodeset("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 0ac93a922b5..fe542f5f63b 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 @@ class BtNavigator : public nav2_util::LifecycleNode void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose); rclcpp::Subscription::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 dc478cb61a1..0d9f03dd490 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: