This is a complete replacement of a previous blog post. At the very end of this article you will find the small bit of XML code which defines the behavior tree described in this article.
I’m trying to develop a skill in using behavior trees as the logic to move Puck. As part of that learning, I have developed a behavior tree that attempts to move Puck so that one of its edges it is within 6 inches (0.1254 meters) from either a left or right wall in my hallway. The basic strategy will be, if Puck is not already near a wall, to turn left and drive up to some “maximum distance” looking for a wall ahead and, then turn right so that it is heading in the original direction. If a wall wasn’t found to the left, Puck will then turn right and search for up to twice that maximum distance for a wall and then turn left to be, again, facing in the original direction. The reason to travel up to twice the original maximum distance this second time is that the robot needs to move “maximum distance” just to get back to where the robot was when it first began searching to the left.
Here is my first attempt at such a behavior tree.
Assuming you are not well-versed in behavior trees, let me try to explain what the tree represents.
First, behavior trees consist of a tree of nodes. Interpretation (execution) of the behavior tree begins by “ticking” the top node in the tree and, depending on the kind of node, that node may “tick” one or more of the child nodes, recursively. A ticked node returns a status of either RUNNING, SUCCESS or FAILURE. The ticked status of a child node is used to decide what a particular node does next. I hope this will be clearer as I describe the above tree. Also, look at the link at the beginning for a more complete description of how behavior trees work.
Behavior tree nodes can communicate with each other via a “blackboard”, which is a simple key/value store. There are various ways to write the value of a key name into the blackboard, and nodes can get the value of any key name. Not delved into here, there is a way of remapping key/values between nodes so that nodes can be written as more generic pieces of code, taking parameters.
The robot will begin execution at the Root node which will then attempt to tick the IfThenElse node. This node begins by interrogating the status of the first child node and, if its status is SUCCESS, it will execute the second child node, else instead it executes the third child node. IfThenElse returns as its status the status of either the second or third child, whichever one was chosen.
The first node of the IfThenElse node is a FoundLeftOrRightWall node, a custom node I crafted for Puck. FoundLeftOrRightWall works by reading the various sensors of Puck to try to find the nearest distance from the edge of the robot to the left and right walls near to the robot. FoundLeftOrRightWall takes a parameter called “within_distance”, which was given a hard-coded value of 0.1254 meters (6 inches) for this node as shown. If either the left or right wall is less than or equal to 0.1254 meters away from the edge of the robot, FoundLeftOrRightWall will return SUCCESS else it returns FAILURE.
If Puck is already within 6 inches of the left or right wall, the second node, AlwaysSuccess is executed which, as you might guess, always return SUCCESS, which will be propagated to the IfThenElse node which itself propagates SUCCESS to the root node and we’re done. This whole behavior tree is attempting to position the robot within 6 inches of a left or right wall. If Puck is already there, nothing additional needs to be done.
If Puck is not within 6 inches of a wall to start, it needs to get there, so instead of the AlwaysSuccess being ticked, Sequence will be ticked.
A Sequence has any number of child nodes, they being RetryUntilSuccesful and IfThenElse in the above. A Sequence node begins by putting itself into the RUNNING state. It then ticks the first child node. If that node returns FAILURE, the Sequence stops and returns FAILURE itself, which is propagated to the parent node (IfThenElse in this case). If the child returns RUNNING, then Sequence will just keep ticking that child until either SUCCESS or FAILURE is returned. If SUCCESS is returned, Sequence will then go on to the next child node and continue until there are no more child nodes, at which point Sequence itself returns SUCCESS, because all of the child nodes were successful.
Sequence, above, will then begin by ticking the RetryUntilSuccesful node. In the picture above, “FindWallToTheLeft” is just a name I gave to the node so I have an idea what the node is trying to accomplish.
The way a RetryUntilSuccesful node works is it will attempt to execute the child node (an IfThenElse node here) up to “num_attempts” (hard coded as 3 attempts here) times. If the child node succeeds during any of those 3 attempts, RetryUntilSuccesful will stop ticking the child node and return SUCCESS to the parent node (Sequence in this case).
So the RetryUntilSuccesful is going to make up to 3 attempts to find the left wall. It makes an attempt by ticking the IfThenElse node which tests to see if FoundLeftOrRightWall sees a wall within 0.1254 meters. If so, AlwaysSuccess is returned else a subtree will be executed called FindWallInDirection. That subtree will be described below but basically it attempts to find a wall in the given direction by turning in the indicated direction, moving up to some maximum distance looking for a wall, and then turning back to the original direction.
You should get then that the RetryUntilSuccesful is going to make up to 3 attempts to find a wall to the left of the original orientation of Puck. If it finds a wall, it returns SUCCESS to Sequence, which will then go on to its second child, another IfThenElse.
And I can see now that his is wrong. Remember that the way Sequence works is that it will sequentially execute each child node as long as each child node returns SUCCESS. If RetryUntilSuccesful returns FAILURE because it didn’t find a wall to the left, then Sequence stops ticking the child nodes and itself returns FAILURE.
I won’t correct the tree in this post. But it shows how using the graphical representation of a tree, and trying to explain it to someone, can uncover errors in the behavior tree. Instead, the fix is to insert a node between Sequence and RetryUntilSuccesful which ignores the status of RetryUntilSuccesful and always returns a SUCCESS status.
Going on to the second child of Sequence, it is going to use IfThenElse to see if the first child found a wall to the left and, if not, will use a similar RetryUntilSuccesful child node to search to the right. Note that this second RetryUntilSuccesful node, that I named “FindWallToRight” will make up to 6 attempts to find a wall to the right whereas the first RetryUntilSuccesful only made up to 3 attempts. That is because we may need 3 RetryUntilSuccesful attempts just to get back to the original position of Puck when we started looking for a wall, then we make up to 3 more attempts to find a wall to the right.
There is no error here by not including a forced success parent node for the second RetryUntilSuccesful node. If we fail to find a wall to the right, the whole tree needs to return FAILURE.
Now I’ll describe the FindWallInDirection subtree which is reused in two places via the parent behavior tree. This allows function-like behavior without having to code a common bit of tree behavior in more than one place.
FindWallInDirection is going to execute a sequence of two child nodes. There is an error in this subtree, which I’ll explain as we go along.
The first child node of Sequence is an Switch2 node. This node examines a blackboard variable, “rotate_first_in_direction” and will chose one of the child nodes depending on which of the “case_1” or “case_2” values matches the “rotate_in_first_direction” value. If the value is “left”, the first node will be ticked, which is a custom node I created called Turn that will turn 90 degrees, which is a turn to the left. If the value is “right”, the second node will be ticked ,which will Turn -90 degrees, which is a turn to the right. If the value is neither “left” or “right”, the third child, AlwaysSuccess is ticked, so no turn is made at the start.
The purpose of FindWallInDirection is to make an initial turn, attempt to find a wall ahead of the robot within some maximum distance, and then reverse the turn so the robot is pointing back in the original orientation. What is missing in this subtree is the action that makes that restoring turn. Again, this shows the value of creating a graphical representation of the behavior tree and then trying to explain it to someone. I will not attempt to fix the tree in this blog post.
After the initial turn is made, the Sequence node executes the second child, a RetryUntilSuccesful node which is going to make up to 6 attempts to move forward a bit and see if a wall is found ahead of the robot. This does so by executing an IfThenElse node which starts by testing if a wall is found ahead of the robot via the custom node I created, FrontWallFound, which takes a “within_distance” parameter. If the front wall is near, rather than just returning SUCCESS, I chose to tick the SaySomething node to emit a message to the log, which will also return SUCCESS as a side effect. If the wall isn’t found ahead of the robot, the custom node I wrote, MoveForward is ticked to move the robot ahead 0.0762 meters (3 inches).
Here is the XML code file used in this article
<root main_tree_to_execute="FindWall">
<BehaviorTree ID="FindWallInDirection">
<Sequence>
<Switch2 variable="{rotate_first_in_direction}" case_1="left" case_2="right">
<Turn degrees="90" />
<Turn degrees="-90" />
<AlwaysSuccess />
</Switch2>
<RetryUntilSuccesful num_attempts="6" name="MoveForward3Inches">
<IfThenElse>
<FrontWallFound within_distance="0.1524" />
<SaySomething message="MoveUntilWallFound success" />
<MoveForward distance="0.0762" />
</IfThenElse>
</RetryUntilSuccesful>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="FindWall">
<IfThenElse>
<FoundLeftOrRightWall within_distance="0.1254" />
<AlwaysSuccess />
<Sequence>
<RetryUntilSuccesful num_attempts="3" name="FindWallToLeft">
<IfThenElse>
<FoundLeftOrRightWall within_distance="0.1254" />
<AlwaysSuccess />
<SubTree ID="FindWallInDirection" rotate_first_in_direction="left" />
</IfThenElse>
</RetryUntilSuccesful>
<IfThenElse>
<FoundLeftOrRightWall within_distance="0.1254" />
<AlwaysSuccess />
<RetryUntilSuccesful num_attempts="6" name="FindWallToRight">
<IfThenElse>
<FoundLeftOrRightWall within_distance="0.1254" />
<AlwaysSuccess />
<SubTree ID="FindWallInDirection" rotate_first_in_direction="right" />
</IfThenElse>
</RetryUntilSuccesful>
</IfThenElse>
</Sequence>
</IfThenElse>
</BehaviorTree>
</root>Here is a proposed new XML with fixes for the two errors:
<?xml version="1.0"?>
<root main_tree_to_execute="FindWall">
<!-- ////////// -->
<BehaviorTree ID="FindWall">
<IfThenElse>
<Action ID="FoundLeftOrRightWall" within_distance="0.1254"/>
<AlwaysSuccess name="AlreadyNearAWall"/>
<Sequence name="SearchForWallLeftThenRight">
<ForceSuccess>
<RetryUntilSuccesful name="SearchWallLeft" num_attempts="3">
<IfThenElse>
<Action ID="FoundLeftOrRightWall" within_distance="0.1254"/>
<AlwaysSuccess name="FoundAWallLeft"/>
<SubTree ID="FindWallInDirection" name="MoveLeftLookingForWall" direction="left"/>
</IfThenElse>
</RetryUntilSuccesful>
</ForceSuccess>
<IfThenElse>
<Action ID="FoundLeftOrRightWall" within_distance="0.1254"/>
<AlwaysSuccess/>
<RetryUntilSuccesful name="FindWallToRight" num_attempts="6">
<IfThenElse>
<Action ID="FoundLeftOrRightWall" within_distance="0.1254"/>
<AlwaysSuccess/>
<SubTree ID="FindWallInDirection" direction="right"/>
</IfThenElse>
</RetryUntilSuccesful>
</IfThenElse>
</Sequence>
</IfThenElse>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="FindWallInDirection">
<Sequence>
<Switch2 case_1="left" case_2="right" name="MakeInitialTurn" variable="{direction}">
<Action ID="Turn" degrees="90"/>
<Action ID="Turn" degrees="-90"/>
<AlwaysSuccess/>
</Switch2>
<ForceSuccess>
<RetryUntilSuccesful name="MoveForwards3Inches" num_attempts="6">
<IfThenElse>
<Action ID="FoundFrontWall" within_distance="0.1524"/>
<AlwaysSuccess name="MoveForwards3InchesSuccess"/>
<ForceFailure>
<Action ID="MoveForwards" distance="0.0762"/>
</ForceFailure>
</IfThenElse>
</RetryUntilSuccesful>
</ForceSuccess>
<Switch2 case_1="left" case_2="right" name="RestoreToInitialDirection" variable="{direction}">
<Action ID="Turn" degrees="-90"/>
<Action ID="Turn" degrees="+90"/>
<AlwaysSuccess/>
</Switch2>
<Action ID="FoundFrontWall" name="ForceFinalSubtreeStatus" within_distance="0.1524"/>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<SubTree ID="FindWallInDirection">
<inout_port name="rotate_first_in_direction"/>
</SubTree>
<Action ID="FoundFrontWall">
<input_port name="within_distance"/>
</Action>
<Action ID="FoundLeftOrRightWall">
<input_port name="within_distance"/>
</Action>
<Action ID="MoveForwards">
<input_port name="distance"/>
</Action>
<Action ID="SaySomething">
<input_port name="message"/>
</Action>
<Action ID="Turn">
<input_port name="degrees"/>
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>