Behavior Tree Navigation

 Github: https://github.com/Juhyung-L/behavior_tree_navigation

This is an extension of the blog https://juhyungsprojects.blogspot.com/2024/04/dynamic-window-approach-for-local-path.html. In said blog, I explained the my implementation of dynamic obstacle avoidance in a mobile robot. The strategy was to use two planners: global and local. The global planner uses the global costmap to find an optimal path from the robot's current pose to the goal pose (A* search). The local planner then takes this path and uses the local costmap to generate a velocity command that would best follow the path while avoiding dynamic obstacles (Dynamic Window Approach). In this blog, I will explain how I added a recovery behavior that would trigger when the mobile robot gets stuck.

Background

Navigating through real-life terrain is full of unexpected obstacles. Things that are hard to catch with the onboard sensor cannot be avoided with just a local planner. In my specific case, I only had a 2D LiDAR on my robot, so any obstacle that come 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 and come to a stop before reaching its destination. A robust way to prevent such issue is to add a recovery behavior. A recovery behavior is a set of predefined controls 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 then try to move forward again. In this case, the recovery behavior is backing up.

Behavior Tree

A good C++ library to use when adding a recovery behavior to your robot is BehaviorTreeCPP.

In a behavior tree, the leaf node represents an action that can be performed 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 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.

Figure 1: Entire behavior tree.

Behavior Tree: 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 2: Left most subtree.

This is the left most subtree in the behavior tree shown in figure 1. The ComputePathToPose node is the global path planner, which finds the optimal path to the goal pose on each tick. The node can send back one of three node statuses: Running, Success, and Failure. The Recovery node receives this node 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 set number of times.

Figure 3: left half of entire behavior tree

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 to goal pose). This whole tree is the left half of the entire behavior tree and it implements the robot's ability navigating to the goal pose.

Figure 4: Right half of entire behavior tree.

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 5: ROS integration of BehaviorTreeCPP

The BehaviorTreeCPP library is integrated into the ROS environment by making the leaf nodes be an 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.

Figure 6: Map of obstacle course.

Figure 7: Actual obstacle course.

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.