Background
Navigating through real-life terrain is full of unexpected obstacles. Things that are hard to catch with the onboard sensors cannot be avoided. In my specific case, I only had a 2D LiDAR on my robot, so any obstacle that came below the horizontal rays of the LiDAR were not detected. That is just one example. There are countless other ways in which a robot can fail to avoid obstacles before reaching its destination. A robust way to prevent such issues is to add a recovery behavior. A recovery behavior is a set of predefined actions the robot performs when it has detected a failure. For instance, if the robot stops moving because it has hit an undetected obstacle, it can back up a short distance, turn, replan its route, then try again.
Implementation
Behavior Tree
A good C++ library to use when adding a recovery behavior to your robot is BehaviorTreeCPP
Figure 1: Illustration of a behavior tree for entering a room.
In a behavior tree, the leaf nodes represent actions and the non-leaf nodes dictate when the leaf nodes are executed. The example tree shown above can be applied to a robot trying to enter a room. First, the robot checks if the door is open. If not, it tries to open the door, retrying up to 5 times. Then, it enters the room and closes the door. The isDoorOpen, OpenDoor, EnterRoom, and CloseDoor nodes are all nodes that execute an action. The non-leaf nodes: Fallback, Retry, and Sequence nodes control when the leaf nodes are executed.
In practice, these nodes are like plugins, a piece of code that is compiled into a shared library and loaded dynamically. This way, individual actions of the robot can be compiled separately and modularized to facilitate creating complex behavior trees.
In this project, the behavior tree will be used to add a recovery behavior. The robot will first try to navigate to the goal pose, but when it detects a failure (ex. the robot is not moving) it will initiate the recovery behavior to get the robot out of whatever situation it is in. The behavior tree for this is shown below.
Figure 2: Behavior tree for navigating a mapped environment with recovery behavior.
Detailed Explanation
The behavior tree works is by sending a "tick" from the root node all the way to the leaf node. A leaf node that receives a tick is executed. Then the leaf node sends back its node status, which is propagated up the tree all the way to the root node.
Figure 3: Left most subtree of the behavior tree. It is responsible for finding the path to the goal pose.
This is the left most subtree in the behavior tree. The ComputePathToPose node is the global path planner, which finds the optimal path to the goal pose on each tick. This node can send back one of three node statuses: Running, Success, and Failure. The Recovery node receives this status and does one of two things. If it received Success, it propagates the status up to its parent node. If it received Failure, it ticks its second child: ClearEntireGlobalCostmap. The logic behind this subtree is that if computing the path to the goal is unsuccessful, we will clear the global costmap, meaning resetting the global costmap back to its original state, then trying again. This recovery process is repeated a predefined number of times.
Figure 4: Left half of the behavior tree. It is responsible for finding a path to the goal pose and following the path.
The same tree is set up for the FollowPath node, which is the local path planner node. These two subtrees are ticked by the PipelineSequence node from left to right (global path planner is ticked first since the local path planner needs the path produced by the global path planner). This whole tree is the left half of the entire behavior tree and it implements the robot's ability to navigate to the goal pose.
Figure 5: Right half of the behavior tree. It is responsible for the recovery behavior.
On the right half is the recovery subtree. When either the ComputePathToPose or the FollowPath node fail more times than allowed by the recovery node, the Failure status is propagated up to the PipelineSequence node and then to the root node. Upon receiving the Failure status, the root node, which is a recovery node, ticks its second child. The Sequence node ticks the Wait and BackUp nodes in sequence, which are the nodes that execute the recovery behavior. The ReactiveFallback node is used so that if the goal pose changes during the recovery, it will be halted immediately.
ROS-specific Details
Figure 6: ROS integration of BehaviorTreeCPP
The BehaviorTreeCPP library is integrated into the ROS environment by making the leaf nodes be a client (action or service). For example, the ComputePathToPose node is a client of the ComputePathToPose action. On each tick to the node, it sends an action request to the server to compute the path to goal pose. The same is true for FollowPath, ClearEntireGlobalCostmap, ClearEntireLocalCostmap, Wait, and Backup: they are all action or service clients that send a request to the server when ticked.
Result
The behavior tree was tested on an obstacle course with unmapped obstacles.
Figure 7: Map of obstacle course.
Figure 8: Actual obstacle course. There are four unmapped obstacles.
The obstacle course consisted of both mapped obstacles (red) and unmapped obstacles (blue). The robot started at the left end of the course and was set to navigate to the right end.
Comments