TreeExecutionServer status is misleading on exception

[b-adkins]

Problem Statement

On exceptions, BT::TreeExecutionServer doesn't tell anybody. Notably, the tree view stays stuck in RUNNING in Groot2 forever.

Detailed Behavior

The BT::TreeExecutionServer catches exceptions from TreeNodes. It dutifully logs them to ROS at the ERROR level. However, it does not tell external interfaces about this. The last reported status is RUNNING in:

• Any BT::StatusChangeLogger
• Groot2 (which uses the Groot2Publisher, a child of StatusChangeLogger)

To reproduce

1. Make a custom behavior. Throw an exception with it.
2. Add it to a behavior tree
3. Run Groot2 in Real-time monitor mode
4. Run the BT::TreeExecutionServer. Send a tree action goal with a ros2 action send_goal command. The action client should report failure.
5. Observe the server's ROS log (for a BT::StatusChangeLogger like in the tutorial). The last status is RUNNING
6. Run Groot2 introspection (free for small trees) and see that the most recent node is highlighted in orange.

Cause

Most of TreeExecutionServer::execute is wrapped in a try block that catches all exceptions and reports them to the ROS action server. It skips ticking the tree and thus the events that trigger a BT::StatusChangeLogger, as well as many of the callbacks that application developers might use to report status.

Suggested workaround

One could write their custom behaviors to not throw exceptions. Or...

Since you are using BT::TreeExecutionServer, it does report exceptions - via the ROS2 action interface. Either use the action client or the hidden action topic to see that the goal has failed.

Suggested fix

The many tick callbacks make it possible to appear to the server that you're ticking the tree when actually you skipped a tick. If they return FAILURE, the server stops ticking -- there are no fallbacks from the root node. Following that philosophy, once in the main loop, an exception would be equivalent to a failed tick. So I think it makes sense to return NodeStatus::FAILURE on exceptions too.