BehaviorTree refactoring

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -49,13 +49,13 @@ class BehaviorTreeEngine
   {
     // this halt signal should propagate through the entire tree.
     root_node->halt();
+    root_node->setStatus(BT::NodeStatus::IDLE);
 
     // but, just in case...
     auto visitor = [](BT::TreeNode * node) {
-        if (auto action = dynamic_cast<BT::CoroActionNode *>(node)) {
-          if( action->status()==BT::NodeStatus::RUNNING) {
-            action->halt();
-          }
+        if( node->status()==BT::NodeStatus::RUNNING) {
+            node->halt();
+            node->setStatus(BT::NodeStatus::IDLE);
         }
       };
     BT::applyRecursiveVisitor(root_node, visitor);

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -96,9 +96,10 @@ class BtActionNode : public BT::ActionNodeBase
   }
 
   // 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;
   }
 
   // Called when a the action is aborted. By default, the node will return FAILURE.
@@ -157,8 +158,7 @@ class BtActionNode : public BT::ActionNodeBase
 
     switch (result_.code) {
       case rclcpp_action::ResultCode::SUCCEEDED:
-        on_success();
-        return BT::NodeStatus::SUCCESS;
+        return on_success();
 
       case rclcpp_action::ResultCode::ABORTED:
         return on_aborted();

nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp

@@ -47,7 +47,7 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
     getInput("planner_id", goal_.planner_id);
   }
 
-  void on_success() override
+  BT::NodeStatus on_success() override
   {
     setOutput("path", result_.result->path);
 
@@ -56,6 +56,7 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
     } else {
       config().blackboard->set("path_updated", true);
     }
+    return BT::NodeStatus::SUCCESS;
   }
 
   static BT::PortsList providedPorts()

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<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_;
 

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;
 }
 
@@ -172,6 +175,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 +215,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 +227,7 @@ 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);
 
   switch (rc) {
     case nav2_behavior_tree::BtStatus::SUCCEEDED:
@@ -243,6 +243,7 @@ BtNavigator::navigateToPose()
     case nav2_behavior_tree::BtStatus::CANCELED:
       RCLCPP_INFO(get_logger(), "Navigation canceled");
       action_server_->terminate_all();
+      bt_->haltAllActions(tree_.root_node);
       break;
 
     default: