Frontier-Based Exploration Tutorial in ROS & C++
Using ROS, grid-map and C++, this tutorial will show you how to implement frontier-based exploration on your robot. The full code can be requested by emailing kashishgarg9@gmail.com
GRASP Laboratory third floor located in Philadelphia, PA
After being able to generate an occupancy grid like the one shown above, we can begin writing our node for frontier exploration!
In the image above there are three colors to note, the gray, the purple and the black
gray - unknown space
purple - known space, occupied
black - known space, free
What we are interested in expanded are called frontiers. They are the black spaces that border the gray, in other words, the free space that neighbors unknown space.
There are 5 main steps to perform in a loop:
Perform Flood/Seed Fill to determine frontiers
Determine Clusters of these frontiers that are greater than a set threshold
Determine the centroids of these clusters
Perform A* to find the centroid closest to our current position
Send position/velocity goal to centroid
In the figure above, it can be shown the full group of frontiers, the clusters, and their respective centroids.
Step 0. Architecture
Before we begin implementing the steps outlined above, let's talk about how we intend to design our ROS node. From a bird's eye view, what we need are two Subscribers and one Publisher. The two Subscribers are to get the occupancy grid and our robot's current position. The Publisher is to be able to send our robot position goals that we will have deduced from the steps outlined above.
The other major point to bring up is that we will be using the grid_map library from Anybotics. This library brings in a handful of incredibly useful tools when handling grids.
Step 1. Find Frontiers
To perform frontier-based exploration, we must first determine what the frontiers of our map are! To do this, we will perform the flood-fill algorithm.
This is what my code looks like:
In the code above, let's start with the flood_fill() method. There are two techniques to implement this algorithm, iteratively and recursively. For exploration, you want to make sure to go down the iterative route, as the recursive will, without-doubt, lead you to memory-allocation issues.Â
With that in mind, let's get started! We begin with using a std::stack object, a data structure that utilizes LIFO (Last In First Out) architecture to define the visited_indices. We begin at the robot's position and grab all neighboring indices and placing them into visited_indices. While the std::stack is not empty, grab the top member, mark it as "visited" in the visited_cells layer of the grid_map object, and check if it is_frontier(). The code for this method is shown below (left figure). If it is a frontier cell, add it to the global member, frontier_indices (just a std::vector). From there, look at all neighbors of the current cell iter and if they are not visited and not obstacles and not unknown, add to the std::stack object
Step 2. Determine Frontier Clusters
After detecting all of the frontiers in an occupancy grid, we need to determine where there are "clusters". By clusters, I mean groups of frontiers which all have neighbors with at least one other frontier within the group.
The code for cluster_detection is shown below. We store clusters in std::vector<std::vector<grid_map::Index>> and acts as a list of the different cluster sets. We iterate through the frontiers and start a std::stack and appends to the stack as it discovers neighbors that are also frontiers. We have to make sure to mark these frontiers as visited so that we don't generate redundant std::stacks. We also employ a limit for how big the cluster needs to be to be considered a cluster.
Step 3. Find Centroids of Clusters
Probably the easiest part of this process, we need to just simply determine the centroid for each cluster. This is simply done by adding all of the x and y values and dividing by the number of elements to get an average value for each element.
Step 4. A* to each Centroid
Now, we need to determine which Frontier Centroid is closest to the robot at its current position. The way this is done is by performing A* to each centroid and determine which output has the smallest path. A* is well documented and will not be rewritten here. Below is a video of the output of all of this work.