Project

General

Profile

The Fiacre model of the major BT nodes

We assume you have some familiarities with the Fiacre langage. If not, you can check the Fiacre web site for Tutorial, examples, primer, etc. You can also check the Fiacre section of this paper (section 4), or these slides withdrawn from a course on V&V in robotics.

Each BT node from a BT model is translated in a Fiacre process. For each BT node, there is also a BTnode structure defined, and all theses structure are stored in an array indexed with the name of the BT. This array BTnode[] is a shared variable between all the Fiacre process. In the following drawings BTnode[self] is the record for the current BT node. Each BTnode record has two fields of interest caller and rstatus. caller is initialized at None and will be set to the BT node which ticks/calls it. It is set back to None when its execution (for the current BT tick) is finished. rstatus contains the last returned status by the considered BT node. It may also be set to halt_me, when a BT node wants to halt its children.

In the following Action and Condition nodes are presented in their TINA version (offline verification) but also Hippo version, i.e. the runtime version. The other nodes are strictly the same between the Hippo and TINA version.

Fiacre BT nodes

Most BT node are organized like the one on the next figure. They have a start node where the process waits until another process passes it the tick (i.e. calls it): on (BTnode[self].caller <> None), it then checks if it has been instructed to halt, if yes, it halts and sets rstatus to failure and returns to sleep (give the tick back to its caller). If not asked to halt, it goes to the tick node does what this nodes must do (see below), sets rstatus to success, failure or running for this tick, and goes back to sleep by setting BTnode[self].caller := None (give the tick back to its caller).

Fiacre BT node

Most BT nodes presented here are present even in our simplest simplest drone nodel (drone1) One can check the resulting Fiacre models:

Condition node

Condition nodes are the simplest one. They can only return success or failure, so no need for halting transition.

Condition node

When run, the hippo version will call an external (C/C++ function), which is expected to return either success or failure. Note that Condition node are supposed to be fast, hence they are linked to a Fiacre external function (i.e. not a Fiacre task which would execute in its own thread).

Condition node (Hippo version)

Action node

Action nodes are more complex, they can return running when the action is taking some time, hence they can also be halted.

Action node

The Action node Hippo version must provide two C/C++ function:

  • One to perform the action, it can return success, failure or running with the obvious meaning. This functions is executed as a Fiacre task, hence a transiton to start it (from Start Task to Sync Task) and a transition to sync on its returned value (from Sync Task to Dispatch). It can also takes some arguments. Of course, if the action takes some time, it is expected to run in the background and the node should return running for the current tick, and subsequent ones, until success or failure is returned.

  • One to halt the action while it is running. This function is defined as a Fiacre external function which returns right away after halting the action.

Action node (Hippo version)

Decorator node

Decorator nodes only have one child, which is ticked/called by setting their caller field to the current BTnode, and then waiting until this caller field is back to None, which means the child has finished its tick execution. Of course, depending on the particular decorator (Inverter, ForceFailure, ForceSuccess, KeepRunningUntilFailure, RetryUntilSuccessful, RateController, Repeat, etc), the transitions from Child done to Success, Failure or Running will be adapted to the particular decorator. For example, the Inverter one will swap the Success and Failure transitions, while leaving untouched the Running one.

Decorator node

Sequence node

The Sequence node ticks/executes its child in sequence as they succeed, until one fails. It then returns failure. If all succeed, it returns success. If one child returns running, it returns running. Upon return of the tick this child is ticked/called again.

The Fiacre process has a local variable next_seq initialized at 1, which holds which node will be ticked/called when the current node is ticked/called again. From the Tick Node, we proceed to the Childnext_seq state, which ticks/calls the proper child. We wait until this child is done caller = None and check its returned status rstatus.

  • success, we proceed to the next child Childnext_seq+1, if is is the last child to success, we return success.

  • failure, we return failure.

  • running, we set next_seq to the proper value and return running.

Note that setting BTnode[self].caller := None will inform our caller that we are done with this tick, and ready to be called/ticked again

Sequence node
Note There exist variants of Sequence: ReactiveSequence, SequenceWithMemory. They alter the way the sequence is ticked after Failure or Running are returned. For example ReactiveSequence always restarts the sequence from the beginning after one child has returned Running. The goal here is not to list all the variants, just to give an hint on how these are transformed in Fiacre. Of course, the generated FIacre code implements the proper semantics for each variant.

Fallback node

The Fallback node executes/ticks its child in sequence as they fail, until one succeed. It then returns success. If all fail, it returns failure. If one child returns running, it returns running. Upon return of the tick this child is again ticked/called. As shown on the figure, it is the symmetric of the Sequence node, thus its behavior is just the mirror of the Sequence one described above.

Fallback node
Note There is also variant of Fallback: ReactiveFallback. It alters the way the fallback is restarted/reticked after Running is returned. For example ReactiveFallback always restarts the sequence from the beginning after one child has returned Running.

Parallel node

The Parallel node can specify how many children m out of n (all children) must succeed for the Parallel node to succeed (the ParallelAll node specify they all must succeed n = m). From this, we see that the implementation just ticks all the node, and then keep track of how many fail, succeed or return running. If any final condition for success or failure is met enough BTnode[child1].rstatus = success) or (enough BTnode[childn].rstatus = failure, then one proceeds to the corresponding state (and possibly halt any running children), otherwise, this node return running. (enough …​ success) is true when success >= m and (enough …​ failure) is true when failure > n - m .

Parallel node

Halting nodes

There are a number of situations where one can decide to halt a running branch (e.g. when a Parallel fails (or succeeds), what do we do of the still running children, similarly when a ReactiveSequence fails, one children may still be running. Hence, any node which can have one or more running children (e.g. the Parallel node can have multiple running children) must also provide mechanism to halt and to propagate the halting mechanism to its children. We have seen how it is implemented for the Action node. For all the control and decorator node, it is quite simple, we check which children is currently running, and if it is, we set BTnode[child].rstatus := halt_me and ticks it with: BTnode[child].caller := self. Upon return, we return failure (unless the halt has been initiated by the current node on a success, e.g., Parallel with success on m < n).

Fancy nodes

Beyond the regular nodes one finds in the most popular implementations, there are more fancy nodes defined. For example the ROS2 Nav2 stack defines the following new node types which we implemented and briefly describe:

  • Control nodes:

    • Recovery 2 children a and b, when a fails, b is attempted. If b succeed, we go back to a. When both fails, the node fails. Node succeeds when a succeeds. (# of retries can be specified for how many time one can execute b on a recovery).

    • PipelineSequence When a child returns running, restart from the beginning of the seq. If any returns failure, fails and halt any running. Success progress and are kept in memory of the Pileline. If the last node succeed, then return success (and halt any runnings left).

    • RoundRobin children: a, b and c. Tick each of them in turn. A child running, return back to this child. A child fail, try the next one. One returns success, on the next call, tick the next node…​ Keep doing this until all nodes return failure.

  • Decorator nodes:

    • RateController passes the tick only when rate has been reached, return running othewise.

    • SingleTrigger not implemented (but trivial) as we did not find it in the examples.

All these implemented nodes are used in the example presented in the ROS2 Nav2 BT Section.

Combining BT nodes to build the complete BT

The complete Fiacre behavior tree is built by composing in parallel all the individual Fiacre process (one for each node) in a component. The communication between the nodes (passing ticks and returning results) is done thru the shared variable BTnode[node]: the array (one element for each node) of BTNode structure/record and their field (rstatus and caller) holding this information for each node.

For example the simple BT on the following figure:

Simple BT

will be translated in the following Fiacre model.

Note Note the BTnode[] array shared between all the Fiacre processes.

(Click on the image to open it in its own window and click again to zoom in)

Combining BT nodes

If you synthesize the Hippo version, you will get the following model. All nodes remain the same, except the Condition and Action ones, which are replaced by their executable version.

Note Note the C/C++ functions which will be linked to the final executable.

(Click on the image to open it in its own window and click again to zoom in)

Combining BT nodes (Hippo version)