Use native library haltTree() (#3950)

* Use native library haltTree() * lint --------- Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
ros-navigation · Nov 10, 2023 · 5053226 · 5053226
1 parent be370db
commit 5053226
Show file tree

Hide file tree

Showing 4 changed files with 8 additions and 19 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
@@ -85,9 +85,9 @@ class BehaviorTreeEngine
 
   /**
    * @brief Function to explicitly reset all BT nodes to initial state
-   * @param root_node Pointer to BT root node
+   * @param tree Tree to halt
    */
-  void haltAllActions(BT::TreeNode * root_node);
+  void haltAllActions(BT::Tree & tree);
 
 protected:
   // The factory that will be used to dynamically construct the behavior tree

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
@@ -177,10 +177,11 @@ class BtActionServer
   /**
    * @brief Function to halt the current tree. It will interrupt the execution of RUNNING nodes
    * by calling their halt() implementation (only for Async nodes that may return RUNNING)
+   * This should already done for all the exit states of the action but preemption
    */
   void haltTree()
   {
-    tree_.rootNode()->halt();
+    tree_.haltTree();
   }
 
 protected:

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -203,7 +203,7 @@ bool BtActionServer<ActionT>::on_cleanup()
   plugin_lib_names_.clear();
   current_bt_xml_filename_.clear();
   blackboard_.reset();
-  bt_->haltAllActions(tree_.rootNode());
+  bt_->haltAllActions(tree_);
   bt_.reset();
   return true;
 }
@@ -281,7 +281,7 @@ void BtActionServer<ActionT>::executeCallback()
 
   // 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_.rootNode());
+  bt_->haltAllActions(tree_);
 
   // Give server an opportunity to populate the result message or simple give
   // an indication that the action is complete.

diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp
@@ -90,22 +90,10 @@ BehaviorTreeEngine::createTreeFromFile(
 
 // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state
 void
-BehaviorTreeEngine::haltAllActions(BT::TreeNode * root_node)
+BehaviorTreeEngine::haltAllActions(BT::Tree & tree)
 {
-  if (!root_node) {
-    return;
-  }
-
   // this halt signal should propagate through the entire tree.
-  root_node->halt();
-
-  // but, just in case...
-  auto visitor = [](BT::TreeNode * node) {
-      if (node->status() == BT::NodeStatus::RUNNING) {
-        node->halt();
-      }
-    };
-  BT::applyRecursiveVisitor(root_node, visitor);
+  tree.haltTree();
 }
 
 }  // namespace nav2_behavior_tree