I started writing up a summary of how the A* path-finding algorithm works, and then came across this site by Ray Wenderlich. I realised I couldn't get across the key points anywhere near as clearly as he has done, so I'll strongly encourage you to read his version before going any further.
It's really important to understand what is happening so you can manipulate the approach for your particular use case.
I found some code online from Christian Careaga that does a great job of runing the A* algorithm over a grid of data, from a starting point to a goal point, finding it's way around 'walls' and providing you the final list of steps for the shortest route. So we'll use this to get started!
Once, we've found the best route we'll plot this over our grid to get a visualisation of the path it found.
First though, let's create our grid and plot it so we know what we're dealing with.
Let's create a 20x20 numpy array filled with 1's and 0's as below. The 0's will be positions that we're allowed to travel on, and the 1's will be walls.
Let's also specify that we want to start in the top left corner (denoted in the plot with a yellow star), and we want to travel to the top right corner (red star).
It's worth noting that with data in this format, the origin (location (0,0)) is at the top left rather than the bottom left
import numpy as np
import heapq
import numpy as np import matplotlib.pyplot as plt from matplotlib.pyplot import figure
grid = np.array([ [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 1], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [1, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0], [0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])
# start point and goal start = (0,0) goal = (0,19)
# plot map and path fig, ax = plt.subplots(figsize=(12,12)) ax.imshow(grid, cmap=plt.cm.Dark2) ax.scatter(start[1],start[0], marker = "*", color = "yellow", s = 200) ax.scatter(goal[1],goal[0], marker = "*", color = "red", s = 200) plt.show()
So now we know what we need to achieve - we need to implement the path finding algorithm to search through the grid and find the best route from our start point to our goal.
The code that Christian created has two main functions, one for the heuristic (remember from Ray's article F = G + H - our method need to score all our potential next moves although we do it slightly different to Ray here...) and one for the main path-finding logic. I'll run through both in a bit of detail.
Heuristic Function:
def heuristic(a, b): return np.sqrt((b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2)
All this is doing is taking two points, a and b. We use this primarily to calculate our H metric. In Ray's example, he was using Manhattan distance, i.e. the number of up/down/right/left's you'd need to take. In our scenario, we're going to allow diagonal movement, so instead of Manhattan distance we're just going to use straight line distance as our heuristic.
To do this, we just apply Pythagoras's theorem to find the distance between two points, based on us knowing the difference of the two along the x and y axis.
Note: We will also use this function when updating our G score (the movement cost from the starting point to our current point/potential neighbours)
Main Path Finding Function:
def astar(array, start, goal):
neighbors = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)]
close_set = set() came_from = {} gscore = {start:0} fscore = {start:heuristic(start, goal)} oheap = []
heapq.heappush(oheap, (fscore[start], start)) while oheap: current = heapq.heappop(oheap)[1] if current == goal: data = [] while current in came_from: data.append(current) current = came_from[current] return data
close_set.add(current) for i, j in neighbors: neighbor = current[0] + i, current[1] + j tentative_g_score = gscore[current] + heuristic(current, neighbor) if 0 <= neighbor[0] < array.shape[0]: if 0 <= neighbor[1] < array.shape[1]: if array[neighbor[0]][neighbor[1]] == 1: continue else: # array bound y walls continue else: # array bound x walls continue if neighbor in close_set and tentative_g_score >= gscore.get(neighbor, 0): continue if tentative_g_score < gscore.get(neighbor, 0) or neighbor not in [i[1]for i in oheap]: came_from[neighbor] = current gscore[neighbor] = tentative_g_score fscore[neighbor] = tentative_g_score + heuristic(neighbor, goal) heapq.heappush(oheap, (fscore[neighbor], neighbor))
This function takes three arguments, array (our grid), the start location, and the goal location. I'll break the rest of it into chunks when running through it
1. Neighbours list
neighbors = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)]
We first provide a list of possible neighbours, (or more specifically the movement distances to our neighbours at any point, i.e (0,1) in the neighbours list would refer to "move). This is a list of all 8 directional movements from any location; up, up/right, right, down/right, down, down/left, left, up/left
2. Set up
close_set = set() came_from = {} gscore = {start:0} fscore = {start:heuristic(start, goal)} oheap = []
heapq.heappush(oheap, (fscore[start], start))
- close_set: You'll remember from Ray's article, that we need a closed list. This is a list where we write down the positions that we do not have to consider again. After each subsequent step, we append the current position to this list
- came_from: A dictionary that contains the all the route paths we've taken in each iteration. This contains our 'parent' positions. When we reach the destination, we lookup the shortest path from this object.
- gscore: A dictionary that contains our G scores by iteration
- fscore: A dictionary that contains our F scores by iteration
- oheap: Our open list, containing all the positions that are being considered to find the shortest path
- heapq.heappush: Pushing the start position and F score onto the open list
3. The loop
while oheap:
We want to check for available positions to move to until there are no more options left
4. Extract the position of the neighbour with the smallest F score
current = heapq.heappop(oheap)[1]
heapq.heappop returns the smallest item from the heap (i.e. in this case, the one with the smallest F score in our open list), and we extract element 1 which is the position
5. If we've reached the goal (i.e. our current position = the goal position - extract and return the shortest path
if current == goal: data = [] while current in came_from: data.append(current) current = came_from[current] return data
6. Otherwise... (i.e. we've not reached the goal)
a) Add the current position to the closed list
close_set.add(current)
b) Loop through all the possible neighbours, calculating their G score
for i, j in neighbors: neighbor = current[0] + i, current[1] + j tentative_g_score = gscore[current] + heuristic(current, neighbor)
c) But, if the neighbour is outside the grid (i.e. the potential neighbour is above, below, left, or right of the bounds we've set) then ignore this neighbour and continue the loop
if 0 <= neighbor[0] < array.shape[0]: if 0 <= neighbor[1] < array.shape[1]: if array[neighbor[0]][neighbor[1]] == 1: continue else: # array bound y walls continue else: # array bound x walls continue
d) Also, if the neighbour is in the closed set and the G score is greater than the G score's for that position then ignore and continue the loop if neighbor in close_set and tentative_g_score >= gscore.get(neighbor, 0): continue
e) If the G score for the neighbour is less than the other G score's for that position OR if this neighbour is not in the open list (i.e. a new, untested position) then update our lists and add to the open list if tentative_g_score < gscore.get(neighbor, 0) or neighbor not in [i[1]for i in oheap]: came_from[neighbor] = current gscore[neighbor] = tentative_g_score fscore[neighbor] = tentative_g_score + heuristic(neighbor, goal) heapq.heappush(oheap, (fscore[neighbor], neighbor))
Done - hopefully that explanation makes sense, if not let me know and we can discuss further!
To explore any parts of the function that are unclear, I'd recommend throwing in print (i.e. print(oheap) statements within so you can see what is happening each iteration - this is how I figured it out, so it may help you too.
Calling our function, and finding the shortest path
As mentioned above, this function takes three arguments, array (our grid), the start location, and the goal location, so let's give it a whirl...
route = astar(grid, start, goal) print(route)
[(0, 19), (1, 18), (2, 18), (3, 18), (4, 18), (5, 18), (6, 18), (7, 17), (8, 16), (9, 15), (9, 14), (10, 13), (10, 12), (11, 11), (12, 10), (13, 11), (13, 12), (13, 13), (14, 14), (15, 14), (16, 14), (17, 13), (18, 12), (18, 11), (18, 10), (18, 9), (18, 8), (18, 7), (18, 6), (18, 5), (18, 4), (18, 3), (18, 2), (17, 1), (16, 1), (15, 1), (14, 2), (13, 3), (12, 4), (11, 5), (10, 5), (9, 5), (8, 5), (7, 4), (6, 3), (5, 2), (4, 2), (3, 2), (2, 1), (1, 1)]
This is the entire list of positions for the shortest route, as found by the algorithm.
Those with a keen eye will notice that it's backwards, i.e. it's starting at the end and working backwards. Those with a keener eye will notice that it's also missing the start position. We can fix both these things easily.
Add start position:
route = route + [start]
Reverse the order:
route = route[::-1]
Now if we print this list, we get what we want...
print(route)
[(0, 0), (1, 1), (2, 1), (3, 2), (4, 2), (5, 2), (6, 3), (7, 4), (8, 5), (9, 5), (10, 5), (11, 5), (12, 4), (13, 3), (14, 2), (15, 1), (16, 1), (17, 1), (18, 2), (18, 3), (18, 4), (18, 5), (18, 6), (18, 7), (18, 8), (18, 9), (18, 10), (18, 11), (18, 12), (17, 13), (16, 14), (15, 14), (14, 14), (13, 13), (13, 12), (13, 11), (12, 10), (11, 11), (10, 12), (10, 13), (9, 14), (9, 15), (8, 16), (7, 17), (6, 18), (5, 18), (4, 18), (3, 18), (2, 18), (1, 18), (0, 19)]
Let's plot this now, to visualise the route:
import matplotlib.pyplot as plt from matplotlib.pyplot import figure
#extract x and y coordinates from route list x_coords = [] y_coords = []
for i in (range(0,len(route))): x = route[i][0] y = route[i][1] x_coords.append(x) y_coords.append(y)
# plot map and path fig, ax = plt.subplots(figsize=(20,20)) ax.imshow(grid, cmap=plt.cm.Dark2) ax.scatter(start[1],start[0], marker = "*", color = "yellow", s = 200) ax.scatter(goal[1],goal[0], marker = "*", color = "red", s = 200) ax.plot(y_coords,x_coords, color = "black") plt.show()
I think that's pretty cool. It's also very impressive how quickly it gets there considering all the options it considers in the process.
This is quite a simple implementation of path-finding. The function is completely fine, but I guess you could say the grid we've used is a bit simplistic in regards to real world scenarios. In this example, we were able to travel freely in any direction, but this may not always be the case - while there isn't a wall there, perhaps the terrain is too steep, or perhaps a road was closed. What about if you could travel in one direction but not another, i.e. a one way street?
These are things I'd like to cover in part 2.
In part 3, I'm hoping to take this logic and convert it into a 3D world, i.e. considering x,y, and z coordinates. Watch this space...
FULL CODE:
############################################################################## # import packages ##############################################################################
import numpy as np import heapq import matplotlib.pyplot as plt from matplotlib.pyplot import figure
############################################################################## # plot grid ##############################################################################
grid = np.array([ [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 1], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [1, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0], [0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])
# start point and goal start = (0,0) goal = (0,19)
############################################################################## # heuristic function for path scoring ##############################################################################
def heuristic(a, b): return np.sqrt((b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2)
############################################################################## # path finding function ##############################################################################
def astar(array, start, goal):
neighbors = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)]
close_set = set() came_from = {} gscore = {start:0} fscore = {start:heuristic(start, goal)} oheap = []
heapq.heappush(oheap, (fscore[start], start)) while oheap:
current = heapq.heappop(oheap)[1]
if current == goal: data = [] while current in came_from: data.append(current) current = came_from[current] return data
close_set.add(current) for i, j in neighbors: neighbor = current[0] + i, current[1] + j tentative_g_score = gscore[current] + heuristic(current, neighbor) if 0 <= neighbor[0] < array.shape[0]: if 0 <= neighbor[1] < array.shape[1]: if array[neighbor[0]][neighbor[1]] == 1: continue else: # array bound y walls continue else: # array bound x walls continue if neighbor in close_set and tentative_g_score >= gscore.get(neighbor, 0): continue if tentative_g_score < gscore.get(neighbor, 0) or neighbor not in [i[1]for i in oheap]: came_from[neighbor] = current gscore[neighbor] = tentative_g_score fscore[neighbor] = tentative_g_score + heuristic(neighbor, goal) heapq.heappush(oheap, (fscore[neighbor], neighbor)) return False
route = astar(grid, start, goal) route = route + [start] route = route[::-1] print(route)
############################################################################## # plot the path ##############################################################################
#extract x and y coordinates from route list x_coords = [] y_coords = []
for i in (range(0,len(route))): x = route[i][0] y = route[i][1] x_coords.append(x) y_coords.append(y)
# plot map and path fig, ax = plt.subplots(figsize=(20,20)) ax.imshow(grid, cmap=plt.cm.Dark2) ax.scatter(start[1],start[0], marker = "*", color = "yellow", s = 200) ax.scatter(goal[1],goal[0], marker = "*", color = "red", s = 200) ax.plot(y_coords,x_coords, color = "black") plt.show()