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.
Github: https://github.com/Juhyung-L/RGB-D_visual_odometry Background In a previous blog, I wrote an explanation on my implementation of a visual odometry system and I developed ( https://juhyungsprojects.blogspot.com/2023/10/visual-odometry-using-sift-feature.html ). The system used RGB image frames as its input to calculate the change in robot pose. A major shortcoming of this system was that the estimated change in the position of the robot was scale-ambiguous, meaning only the direction of motion was estimate-able, but not the magnitude. This ambiguity was a physical limitation of trying to obtain 3D information (change in x, y, z position) using a 2D input (image frames). In this blog, I will give an explanation of another visual odometry system I developed, which uses a RGB-D camera instead of a RGB camera. A RGB-D camera is basically a RGB camera with depth sensors, which allows it to obtain 3D point cloud of the scene in addition to the image. By utilizing the point cloud, th
github: https://github.com/Juhyung-L/cuda_icp Background Scan matching algorithms are ubiquitous in the field of mobile robotics. Although there are countless variations of the algorithm with each one having its benefits and drawbacks, the overarching goal of the algorithms is the same. Given two sets of points, the algorithm finds the optimal rigid body transformation (rotation and translation) that aligns one set of points onto the other. They are specifically useful in localization (estimating the robot's pose) algorithms as finding the rigid transformation between two consecutive laser scans from the same robot means finding the accurate change in position and orientation of the robot. One of the most commonly used variation of the scan matching algorithm is the iterative closest point (ICP) algorithm. ICP takes a 2-step approach to scan matching Finding the corresponding pairs of points in the two point sets Finding the rigid body transformation Finding corresponding pairs m
Github: https://github.com/Juhyung-L/bird_view Background For my university's capstone project, I was tasked with creating a device that can help our client in visualizing the surrounding environment while driving his power wheelchair. Our client suffered from paralysis below the neck, which meant that his only source of mobility was his power wheelchair. He specifically needed a device that could assist him in visualizing the back half of his wheelchair. Our team took inspiration from the bird's eye view that some car models provide. The working principle behind the bird's eye view is to use multiple wide-angle (fisheye) cameras attached to the sides of the car and computationally combine all video streams to provide a top-down view of the car. Although the system does not look too difficult to implement at first, we quickly realized that making a system that is both durable and reliable for long-term use while being attached to a moving wheelchair was extremely difficult
.jpg)
.jpg)
.jpg)
Comments
Post a Comment