Pathplanning, as the name suggests, is the process of determining how to get from one point to another. Pathplanning, also called motion planning, has applications in a number of fields, like autonomous robotics and GPS navigation, to name a couple. Before getting into the heart of the matter, let’s start with a demonstration of the Python script and interactive plot we’re going to have up and running by the end of this post:
As you can see from the video, we’re going to learn about and implement a pathplanning algorithm that tries to find the shortest path (yellow) on a grid from a start cell (the green square) to a destination cell (the red square) while navigating around obstacles (the black cells). Read on for the details.
Graphbased pathplanning
There exist a number of techniques and algorithms for pathplanning, one of which includes graphbased methods. In graph theory, a graph is defined as a structure consisting of “nodes,” i.e., points, and “edges,” i.e., lines that connect the nodes. In graphbased pathplanning, we travel from node to node along the edges, with the aim of figuring out how to get from some initial node to a destination node.
We’re going to examine a particular type of graph: the grid, also known as a lattice graph. Each cell of our grid can be thought of as a node, and the path between each node can be thought of as an edge. The defining characteristic of a grid is that all the edges are the same length. In this post, we’ll be working with a square grid.
In the figure above, left, we see our grid, which looks kind of like a crossword puzzle. The white squares represent empty cells to which we can travel. The black squares are obstacles. The green square is the start cell, and the red square is the destination cell. In the figure, top right, the graph representation of the grid is superimposed on the grid. Each orange dot is a node, and the lines between nodes are edges. In the figure, bottom right, I’ve isolated the graph representation to get a better look at it. Notice how the nodes are connected—from any given node, it’s only possible to travel up, down, right, or left, not diagonally. We don’t have to limit ourselves to this, of course. If we wanted to, we could also define our grid to have diagonal edges between nodes, but we’ll keep things simple for this example.
Finding the shortest path
Our goal will be to find the shortest path from the start cell to the destination cell. A number of methods exist to get from start to finish, including depthfirst search, which follows a single path until it either finds the destination or, more likely, hits a deadend, at which point it backtracks to the last fork in the road and follows a different path—repeat until the destination is found or until there are no paths left to explore. However, this is not guaranteed to find the shortest path, only to find a path if one exists. Another method is the breadthfirst search, which explores evenly out from the start cell and is guaranteed to find the shortest path. One of the simplest types of breadthfirst search algorithms, which we saw in the video at the beginning of this post, is sometimes referred to as the wavefront or grassfire algorithm, so named because the search path looks like a spreading shockwave or brush fire. The idea behind the algorithm is to mark each cell (node) with its distance from the start cell, exploring the cells adjacent to all cells at the current depth (distance) before moving on to the next depth:
First, we mark our start cell with “0” (see figure above, top left). Then, we look at the cells above, below, to the right, and to the left of the start cell. We mark each of the cells in those directions with a “1”, assuming they’re not obstacles or outside the boundaries of the grid, because they’re 1 step away from the start cell. Since there’s only one cell with a depth of “0”, the start cell, that’s it for the first iteration. On the next iteration, we consider each cell marked with depth “1” and look at the cells adjacent to those, then mark them with “2” to indicate that they’re 2 steps away from the start cell. We continue in this manner, only marking adjacent cells with the next depth if they’re not already marked with a smaller number, which would indicate that they’re already closer to the start cell than the current cell. This process continues until either a) the destination is found or b) all reachable cells have been explored and no path to the destination exists. If the destination is found, we can backtrack to find the shortest path by following the cells in reverse numerical order back to the start cell:
Note that, in our implementation of the algorithm, backtracking will be arbitrary—if there are multiple shortest paths, our algorithm will only choose one of them.
The code, part 1
To implement and animate the algorithm, we’ll create two files, both available on Github: Grassfire.py animGrassfire.py
First, create a file named Grassfire.py (or get it from the Github link above).


After taking care of our imports and defining pi as a constant, we define a
class we’ll call Grassfire
. We’ll use the variables and methods in
this class to create and manipulate our grid, as well as to execute the
Grassfire algorithm. Our grid will be 2D numpy integer array. As discussed
above, we’ll utilize 0 to denote the start cell and positive integers to
indicate the distance of visited cells from the start cell. The negative
integers on lines 1417 are reserved for, respectively: the destination,
unvisited cells, obstacles, and cells that are part of the shortest path if the
destination is found. Note that these are class variables (which we’re
treating as constants), meaning that they are shared across all instances of a
class.


In addition to the main grid, which contains the locations of the start and
destination, obstacles, distances of visited cells, etc., we’ll also need
a corresponding grid that translates that information into colors. This
colorGrid
will simply be an RGB pixel map, with each pixel representing one
cell of our grid. If the main grid has dimensions MxN, then the colorGrid
will
have dimensions MxNx3, where the third axis contains the RGB values for each
cell. Lines 2126 define the RGB colors for each cell type.


The random_grid()
method is the only method in our class that actually returns
a fresh, randomized grid. Observe that we prefix class attributes (variables)
with the class name, e.g., Grassfire.UNVIS
. We use the numpy function
random_sample()
to generate an array of floats from 0 to 1, obstacleGrid
. On
line 34, we initialize our main grid, grid
, to an array representing
unvisited cells. Since obstacleGrid
and grid
have the same dimensions, we
find the values of obstacleGrid
less than our obstacle probability threshold
and set those elements in grid
to be obstacles on line 35. The comparative
statement obstacleGrid <= obstacleProb
produces a Boolean array of the same
size as obstacleGrid
, where elements satisfying the condition are True and the
rest are False. Since this Boolean array is also the same size as grid
, we can
use it to index into grid
and modify only those elements where the condition
is True.
Finally, on line 38, we use the method set_start_dest()
, which we’ll
define next, to randomly select start and destination locations. Note that we
supply the set_start_dest()
method with grid
without an assignment and
without worrying about the return value of set_start_dest()
. In other words,
notice how we didn’t write line 38 as grid = self.set_start_dest(grid)
.
That’s because numpy arrays are passed by reference, not by value. This means
that passing grid
to a function doesn’t pass a copy of grid
—instead,
it passes a reference to the original array itself, so that any changes
set_start_dest()
makes to grid
are made to the original array.


In the method set_start_dest()
, we use the function random.randint()
to
select a random integer within the range of the total number of cells in our
grid. This single integer is essentially a flattened index, i.e., the index of
an element in the grid if the 2D grid were flattened into a 1D array by
arranging all the rows next to one another. The numpy function unravel_index()
takes this 1D index and converts it into a pair of 2D indices (note that the
function can be used to convert an index into a set of indices in any number of
dimensions, not just two dimensions).


As we discussed above, the function color_grid()
returns an RGB pixel array to
visually represent grid
. In the reset_grid()
method, lines 8283
demonstrate that Boolean arrays can be added to one another.
Finally, we’ll define the methods that actually implement the algorithm, beginning with two helper methods followed by the main pathfinding method. On each iteration, our implementation of the algorithm will check the cells adjacent to every cell matching the current depth. If any of these adjacent cells are unvisited or have a value greater than the current depth + 1, they will be updated to a value of current depth + 1 (indicating that they’re 1 step farther from the start cell than cells of the current depth). We will keep track of how many adjacent cells are updated during each iteration; if, after going through all cells at the current depth, the number of updated cells is 0, it means we’ve explored all visitable cells that can be reached and that no path to the destination exists. During this process, we’ll also be checking to see if the destination is found.


First, the helper method _check_adjacent()
which, given a grid and a cell
within the grid, checks the four cells adjacent to the given cell. Note the use
of sine and cosine to achieve this compactly. Also observe how, in Python, we
can write comparative statements like 0 <= rowToCheck < rows
instead of
splitting them into two statements.


The second helper method, _backtrack()
, is similar to the previous method,
except it modifies the first matching adjacent cell to the value given by the
PATH
constant, then returns a tuple containing the row and column of the
modified cell. The modified cell will be used as the input to the next call of
_backtrack()
. This will repeat until we’ve arrived back at the start cell.


Now the main event: find_path()
. You may notice a couple peculiarities in this
method:

The algorithm is contained in a generator function,
find_path_generator()
, which is nested within and returned byfind_path()
. For more information on generator functions and generators in Python, read my earlier post on Kmeans clustering. The reason for this is that we’ll use the matplotlib animation module’sFuncAnimation
function to animate the algorithm, and we’ll supply it with a generator function to update the plot at every iteration. But, from the matplotlib documentation on FuncAnimation, the generator function we supply cannot have any input arguments. Essentially, matplotlib will call the generator function to produce a generator. The generator will be used to update the plot. However, matplotlib doesn’t allow us to provide input arguments to the generator function, which is a problem because our algorithm needs one input argument: the grid on which it’s supposed to operate, i.e.,grid
. Nesting the generator functionfind_path_generator()
withinfind_path()
allows us to get circumvent this restriction, which brings us to peculiarity number two. 
We supply
find_path()
with the input argumentgrid
. On line 145, we putgrid
(or, more accurately, a reference to it) in a dictionary,nonlocalDict
. Then, on line 147, in the nested generator functionfind_path_generator()
, we extractgrid
from the dictionary and proceed to use it normally. Why is this necessary? In Python 2, scope rules for nested functions are a little unintuitive. The inner function,find_path_generator()
, technically has access to all the variables defined in the outer function,find_path()
. However, Python 2 doesn’t allow a nested function to access variables in the outer function’s namespace unless it’s in some kind of container, like a dictionary. Python 3 solves this issue with thenonlocal
statement. If we were writing this code to work only in Python 3, we could eliminate line 145 and replace line 147 with the statementnonlocal grid
. This is kind of like using theglobal
statement in a function to allow accessing a global variable. But, to ensure compatibility with Python 2, we instead putgrid
into a dictionary, then extract it from the dictionary in the nested function.
A final note on this function: it contains two yield
statements (the second
one only comes into play if the destination is found). This just highlights the
fact that we can use as many yield statements as we wish in a generator
function.
The code, part 2
Now, create a file named animGrassfire.py (or get it from Github). This file will utilize the class we just defined to animate the algorithm using matplotlib.


First, we take care of the necessary imports, set some initial parameters,
instantiate the Grassfire
class, and use it to initialize our grid
and
colorGrid
.


On lines 2021, we initialize the matplotlib imshow plot, which will display
the pixel array colorGrid
, and get the axis handle for the axis that’s created
to house it. The keyword argument interpolation='nearest'
ensures that each
cell appears as a discrete pixel. Line 22 has nothing to do with our
pathplanning grid, but rather refers to the axis gridlines, which will create
the appearance of distinct cells. Lines 2729 initialize the parameter
labels below the grid on the plot (see the video at the beginning of this post).
The function update_annotations()
will update these text labels if the user
presses a key. The function set_axis_properties()
updates the axis limits and
axis gridlines if the user creates a new grid. The gridPlot
imshow pixel array
limits must be updated separately on line 37. We call each of these
functions once on lines 4546 to initialize the plot.


Now, we make the figure interactive to allow the user to change parameters on
the fly and/or randomize the grid or start/destination cells. See the previous
blog post on animating inverse kinematics for a brief introduction to
interactive plotting. To prevent conflicts with default matplotlib keyboard
shortcuts, we disconnect all key bindings on line 49, allowing us to
reassign keys as we wish without any unwanted behavior. The docstring at the
beginning of our on_key_press()
function explains how we want to plot to
respond to the keypresses we specify. Note that we reference ani
, the
matplotlib animation object, even though we haven’t defined it yet. Also note
that, on line 73, if the user presses Ctrl to randomly generate a new grid,
we have to provide a new generator function to the animation object by modifying
its _iter_gen
parameter. It isn’t apparent from the matplotlib documentation
that this is required, nor is it apparent how to do it, but perusing the
matplotlib animation module source code helped make these facts clearer.


The rest of the code is fairly selfexplanatory if you read the
FuncAnimation documentation. We show the plot with the argument block=True
to ensure the figure remains open until the user chooses to exit the script by
pressing Enter (irony intended).
Hopefully, you now have a better understanding of the Grassfire algorithm, interactive plotting in Python with the animation module, or both!