From b0ae01d1eb05e8ed33237fc8ebb3d2b8f0b820dc Mon Sep 17 00:00:00 2001 From: Giovanni Remigi Date: Fri, 9 Oct 2026 05:11:35 +0200 Subject: [PATCH] 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 --- .../tree_execution_server.hpp | 4 +- .../src/tree_execution_server.cpp | 37 ++++++++++++++++++- behaviortree_ros2/tree_execution_server.md | 5 +++ 3 files changed, 43 insertions(+), 3 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp index a7e0a190..9c783ec2 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp +++ b/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 diff --git a/behaviortree_ros2/src/tree_execution_server.cpp b/behaviortree_ros2/src/tree_execution_server.cpp index 6701eea8..428a0fa6 100644 --- a/behaviortree_ros2/src/tree_execution_server.cpp +++ b/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(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; } diff --git a/behaviortree_ros2/tree_execution_server.md b/behaviortree_ros2/tree_execution_server.md index e7347cd5..4b170ab6 100644 --- a/behaviortree_ros2/tree_execution_server.md +++ b/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.