Halt the tree and complete the execution when a node throws

When a node threw an exception while the tree was ticked, execute() aborted
the goal and returned at once: the tree was not halted, and
onTreeExecutionCompleted() was not called. Nodes still running, e.g. an
action whose goal was sent, were left running, and a server that cleans up
or reports the end of a run in onTreeExecutionCompleted() never learned that
the run had ended.

Halt the tree and call onTreeExecutionCompleted(FAILURE, false) there too,
as for every other end of a run, for any exception thrown once
onTreeCreated() has returned: by a node while the tree is ticked, or by
onLoopAfterTick() or onLoopFeedback(). The result also carries the FAILURE
node status. An exception thrown before, e.g. by the creation of the tree,
is handled as before. If onTreeExecutionCompleted() itself throws while the
execution is stopped, it is not called a second time.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

Author: kineticsystem

behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp

@@ -125,7 +125,9 @@ class TreeExecutionServer
 
   /**
    * @brief onTreeExecutionCompleted is a callback invoked after the tree execution is completed,
-   * i.e. if it returned SUCCESS/FAILURE or if the action was cancelled by the Action Client.
+   * i.e. if it returned SUCCESS/FAILURE, if the action was cancelled by the Action Client,
+   * or if an exception was thrown after onTreeCreated(), e.g. by a node while the tree was
+   * ticked or by another callback: the tree is then halted, and the status is FAILURE.
    *
    * @param status The status of the tree after the last tick
    * @param was_cancelled True if the action was cancelled by the Action Client

behaviortree_ros2/src/tree_execution_server.cpp

@@ -186,6 +186,13 @@ void TreeExecutionServer::execute(
     executeRegistration();
   }
 
+  // Whether onTreeCreated() was called for this goal: only then is there a tree
+  // of this goal to halt, and an execution to complete, if an exception is thrown.
+  bool tree_created = false;
+  // Whether onTreeExecutionCompleted() was called for this goal, so that it is
+  // not called again if it throws.
+  bool execution_completed = false;
+
   // Loop until something happens with ROS or the node completes
   try
   {
@@ -198,6 +205,7 @@ void TreeExecutionServer::execute(
 
     // call user defined function after the tree has been created
     onTreeCreated(p_->tree);
+    tree_created = true;
     p_->groot_publisher.reset();
     p_->groot_publisher =
         std::make_shared<BT::Groot2Publisher>(p_->tree, p_->params.groot2_port);
@@ -209,10 +217,11 @@ void TreeExecutionServer::execute(
 
     // operations to be done if the tree execution is aborted, either by
     // cancel requested or by onLoopAfterTick()
-    auto stop_action = [this, &action_result](BT::NodeStatus status,
-                                              const std::string& message) {
+    auto stop_action = [this, &action_result, &execution_completed](
+                           BT::NodeStatus status, const std::string& message) {
       p_->tree.haltTree();
       action_result->node_status = ConvertNodeStatus(status);
+      execution_completed = true;
       // override the message value if the user defined function returns it
       if(auto msg = onTreeExecutionCompleted(status, true))
       {
@@ -264,6 +273,30 @@ void TreeExecutionServer::execute(
   {
     action_result->return_message = std::string("Behavior Tree exception:") + ex.what();
     RCLCPP_ERROR(kLogger, action_result->return_message.c_str());
+    status = BT::NodeStatus::FAILURE;
+    action_result->node_status = ConvertNodeStatus(status);
+    if(tree_created && !execution_completed)
+    {
+      // An exception thrown after onTreeCreated(), e.g. by a node while the tree
+      // is ticked or by a user defined callback, leaves the nodes as they were,
+      // e.g. an action whose goal still runs: halt them, as when the execution
+      // is cancelled.
+      try
+      {
+        p_->tree.haltTree();
+      }
+      catch(const std::exception& halt_ex)
+      {
+        RCLCPP_ERROR(kLogger, "Exception while halting the Behavior Tree: %s",
+                     halt_ex.what());
+      }
+      // The execution ends here too: let the user clean up, as for any other end.
+      // Override the message value if the user defined function returns it
+      if(auto msg = onTreeExecutionCompleted(status, false))
+      {
+        action_result->return_message = msg.value();
+      }
+    }
     goal_handle->abort(action_result);
     return;
   }

behaviortree_ros2/tree_execution_server.md

@@ -53,6 +53,11 @@ This happens if:
 1. Ticking the tree returns SUCCESS/FAILURE
 2. The `rclcpp_action::Client` cancels the action.
 3. Callback `onLoopAfterTick`cancels the execution.
+4. An exception is thrown after `onTreeCreated` returns, e.g. by a node while the
+   tree is ticked, or by `onLoopAfterTick` or `onLoopFeedback`. The tree is halted,
+   and the callback receives FAILURE. It is not called when the exception is thrown
+   before, e.g. by the creation of the tree or by `onTreeCreated` itself, and it is
+   not called a second time when it throws itself.
 
 Argument `was_cancelled`is true in the 1st case, false otherwise.