ROS2: Path Planning Algorithms Python Implementation

# A-Star Algorithm: grid

Generally, when we use A* algorithm, there are two approaches: we either use the adjacent list of nodes (example: we have a network of paved roads and we don't want our robot to drive anywhere else). This is also a more efficient approach, as we should only consider limited number of nodes, ideally, only road intersections.

Second approach places the regular grid on the entire map. It works when our robot explores the unknown area, or when it runs in the ourdoors crossroad environment (or when it cleans the parking lot and doesn't care about lines).

The grid has more nodes than the adjacency list, but it allows thorough exploration.

In this chapter, we are going to take a look at the grid approach, while the next chapter is dedicated to using adjacency list.

a_star.py

```
# (c) robotics.snowcron.com

import math
import cv2
import numpy as np

class AStar():

def __init__(self, map):
# Map is a grid with keepout values in its cells
self.map = map

# To keep the code consistent with "keepout map" format,
# I will pass keepout values, but what I really need is
# a reverse keepout values, so here it is:

self.cost_map = 255.0 / np.maximum(self.map, 1)

self.height, self.width = map.shape

def heuristic(self, a, b):
# Note: we can use Manhattan distance instead
return math.sqrt((b[0] - a[0])**2 + (b[1] - a[1])**2)

def get_neighbors(self, node, treshold):
r, c = node

# We can move sideways or diagonal
directions = [(r-1, c), (r+1, c), (r, c-1), (r, c+1)]

neighbors = []
for r, c in directions:
# Note how we ignore nodes that are not drivable
if 0 <= r < self.height and
0 <= c < self.width and self.map[r,c] > treshold:
neighbors.append((r, c))

return neighbors

def astar(self, start, goal, treshold):
# We keep track of visited nodes,
# as well as nodes next to them

openset = set()
closedset = set()

# This set will be used to backtrack the solution,
# so after we reach our destination, we can get
# a "winner" path

came_from = {}

# Scores used to calculate rating

g_score = {start: 0}
f_score = {start: self.heuristic(start, goal)}

# Here comes a rather traditional breath first solution

while openset:

# We pick the candidate with smallest "price"
current = min(openset, key=lambda x: f_score[x])

# If the goal reached, backtrack our steps
# and get the solution
if current == goal:
path = []
while current in came_from:
path.append(current)
current = came_from[current]

return path[::-1]

# Done with node; move it from one set to another
openset.remove(current)

# For neighbours, calculate ratings
for neighbor in self.get_neighbors(current, treshold):
tentative_g_score = g_score[current] \
+ self.cost_map[neighbor[0],neighbor[1]]
if neighbor in closedset
and tentative_g_score >= g_score[neighbor]:
continue

if neighbor not in openset \
or tentative_g_score < g_score[neighbor]:
came_from[neighbor] = current
g_score[neighbor] = tentative_g_score
f_score[neighbor] = g_score[neighbor] \
+ self.heuristic(neighbor, goal)

if neighbor not in openset:

return None

# This function can be used for testing

if __name__ == "__main__":

# Create and fill the map with fully
# "drivable" cells
map = 255*np.ones((20,20), dtype=np.uint8)

# Note the way I used partially drivable surface
# with keepout values of 65 and 128, to create
# a "roadblock" with a narrow area that is less
# thick". It forces the algorithm to go through that
# area: think of crossing the muggy area: you will
# likely do it at the most narrow part.

map[2:5, 3] = 32

map[8, 0:14] = 65
map[8, 17:20] = 128

map[9, 0:15] = 65
map[9, 16:20] = 128

map[9, 0:20] = 128

map[16:20, 16] = 0

start = (0, 0)
goal = (19, 19)

astar = AStar(map)
path = astar.astar(start, goal, treshold=64)

# Show path - scale image up
map_display = cv2.resize(map, (640,640),
interpolation=cv2.INTER_NEAREST)
map_display = cv2.cvtColor(map_display, cv2.COLOR_GRAY2RGB)

for node in path:
x, y = node[1]*32, node[0]*32
cv2.rectangle(map_display, (x,y), (x+32,y+32), (0,0,255), -1)

cv2.imshow("Path", map_display)
cv2.waitKey(0)

```

Grid based A* algorithm, especially when implemented using Python, has severe speed limitations. First thing that comes to mind is using NumPy to speed it up, second thing is using C++.

Note, that there is a 3rd possibility: we can use RRT (rapidly exploring random tree) algorithm, that yeilds (almost) the same result, but is signifficantly faster.