Adds onLoopBeforeTick callback to BT::TreeExecutionServer.

[b-adkins]

Changes

Adds virtual bool onLoopBeforeTick() callback to the BT::TreeExecutionServer, that runs before the behavior tree tick() and, if it returns false, skips the tick. It is the natural counterpart to onLoopAfterTick(BT::NodeStatus).

Problem Statement

TL:DR; In my opinion, TreeExecutionServer's interface needs this callback to be a flexible base class.

Design Thoughts

BT::TreeExecutionServer, like many classes in the BehaviorTree ecosystem is locked down with private member variables and the PImpl pattern. I believe that it is clearly designed to only be extended via overriding the callbacks. Thus, I think it is vital that it has full expressiveness in its callbacks alone.

User Story

I am using BehaviorTree.CPP and .ROS for a real-world robotics project, including the BT::TreeExecutionServer. Most of my changes are in a child class of the BT::TreeExecutionServer, called ChildServer here. We are visualizing trees, live, using Groot2.

For the foreseeable future, we have non-reactive behavior trees that rely on many layers of sequence behavior nodes. To make it possible to go slowly with the robot, one behavior at a time, I implemented pause and resume ROS services of type std_srv/Trigger. These set ChildServer::isPaused accordingly.

To implement pause, you have to prevent the behavior tree from being ticked. BehaviorTree.CPP lets you inject a pretick callback on any behavior node. From the selection of available callbacks in BT.CPP and BT::TreeExecutionServer, it was the only option I had. (I would have much preferred that the execute() loop call a protected function virtual serverTick(), that I could then overload to conditionally invoke the parent's function.) My callback was a member function of ChildServer. It blocked using a spinning loop on a ROS timer that regularly polled rclcpp::ok and ChildServer::isPaused. (The alternative was to return failure - which I rejected because the goal is to not tick, not to fail.)

The action server was blocked when I only wanted to avoid ticking the tree. I found it impossible to replicate the same checks performed by the loop of BT::TreeExecutionServer::execute. I couldn't poll the action goal state because the action server is buried in the PImpl, and the goal_handle is passed between ROS callbacks and not stored in the class. I would have also liked to invoke stop_action() from my code, but it was a lambda hidden inside execute.

This approach crashed sometimes. My best guesses for why:

• There is no way to preserve the existing callback or enqueue another. This means that Groot2/the groot protocol can replace a callback you were relying on for control flow via its breakpoints feature. If the callback is removed while the goal execution execute thread is in it - which it always is while you're paused - it can cause crashes.
• If the action goal is cancelled while paused, the tree can be cleaned up while the execute thread is still in the callback. When it returns to TreeNode::executeTick for a node that doesn't exist any more, it can crash.

Discussion

This PR is to add a virtual bool onLoopBeforeTick() callback that runs before the behavior tree tick and, if it returns false, skips the tick.

Benefits:

• More straightforward for a custom server to modify flow control. Instead of a blocking inner loop, it returns quickly (skipping the tick) and re-uses the rest of the execute loop.
• The server can load a tree but never tick it
• Does not conflict with another user of TreeNode preTickCallbacks, like Groot
• Makes it easy to make the TreeExecutionServer into a state machine
• A last-ditch callback before tree execution begins. If you need to do something after the groot publisher has been created but before a tick, this callback can invoke whatever you want once and then set a flag. (In some of my experiments, it was very difficult to both have sane tree behavior and have the status change logger render sane things in Groot)
• Other creative uses we haven't considered

In conclusion, I think this change makes TreeExecutionServer a more robust and flexible base class.