diff --git a/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp b/behaviortree_ros2/include/behaviortree_ros2/tree_execution_server.hpp index a7e0a19..9c783ec 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 6701eea..428a0fa 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 e7347cd..4b170ab 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.