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, once onTreeCreated() has been called for
the goal. The result also carries the FAILURE node status. An exception
thrown before, e.g. by the creation of the tree, is handled as before.

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 a node threw an exception while the tree was ticked: 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,10 @@ 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;
+
   // Loop until something happens with ROS or the node completes
   try
   {
@@ -198,6 +202,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);
@@ -264,6 +269,29 @@ 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)
+    {
+      // A node that throws while the tree is ticked leaves the other 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,9 @@ 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. A node throws an exception while the tree is ticked. The tree is halted, and
+   the callback receives FAILURE. It is not called when the exception is thrown
+   before `onTreeCreated`, e.g. by the creation of the tree.
 
 Argument `was_cancelled`is true in the 1st case, false otherwise.