ROS2: Path Planning Algorithms Python Implementation

Sometimes we can speed things up by providing nodes for "special" points on map, rather than "seeding" the entire map with regular grid's nodes. A good example is navigation along the existing road network: when planning such path, we simply do not have to consider points that are not located on the road.

In that case, the algorithm changes signifficantly: we choose neighbours from the list, we can use distances along the road, and instead of the keepout values of nodes, we use "quality of the road", provided with the graph edges.

Note that the algorithm below can be changed based on your task. For example, let's say your adjacency list contains a segment of a road that has quality of 128. But we want to additionally "punish" part of that segment, as it goes uphill. Something like that.

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

import math
import cv2
import numpy as np
from collections import defaultdict

class AStarGraph:

self.nodes = nodes

# This function returns a straight line distance between
# the current point and a goal point

def heuristic(self, n1, n2):
x1, y1 = nodes[n1]
x2, y2 = nodes[n2]
return math.sqrt((x1 - x2)**2 + (y1 - y2)**2)

# Based on the adjacency list and treshold value,
# return neighbours of a point

def get_neighbors(self, node, treshold):
neighbors = []
if(neighbour > treshold):
neighbors.append(neighbour)
return neighbors

def astar(self, start, goal, treshold):

openset = set()

closedset = set()

came_from = {}

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

while openset:

current = min(openset, key=lambda x: f_score[x])

if current == goal:
path = []
while current in came_from:
path.append(current)
current = came_from[current]
path.append(current)
return path[::-1]

openset.remove(current)

for neighbor in self.get_neighbors(current, treshold):
# Here is one important difference between adjacency
# list based algorithm and a grid based one: we
# adjust the heuristic value using the quality of the
# previous segment of the road. This way straight dirt
# road will not win against (slightly) curved highway.

tentative_g_score = g_score[current] + \
self.heuristic(current, neighbor) \
* 255.0 / np.maximum(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]

if neighbor not in openset:

return None

if __name__ == "__main__":

nodes = {
1: [50,  50],
2: [200, 200],
3: [300, 300],
4: [400, 400],
5: [200, 300],
6: [300, 200],
7: [200, 400],
}

# List format:
# pt_from: [ [pt_to, keepout_val], ...]
1: [ [2, 255],  [5, 255], [7, 64] ],
2: [ [1, 64], [3, 255] ],
3: [ [2, 64], [4, 64] ],
4: [ [3, 64], [5, 64],  [7, 64] ],
5: [ [1,255], [3, 64],  [4, 255] ],
6: [],
7: [ [1,128], [4, 64] ],
}

path = astar.astar(1, 4, treshold=63)
if(path is None):
else:

img = 255 * np.ones((640,640,3), dtype=np.uint8)

pt1 = nodes[src]
for dst in arrDst:
pt2 = nodes[dst]
cv2.line(img, pt1, pt2, (0,0,255), 2)

for i in range(len(path)-1):
pt1 = nodes[path[i]]
pt2 = nodes[path[i+1]]
cv2.line(img, pt1, pt2, (0,255,0), 5)

font = cv2.FONT_HERSHEY_COMPLEX_SMALL
fontScale = 1
color = (0, 255, 255)
thickness = 1
for k, v in nodes.items():
cv2.circle(img, (v, v), 10, (0,0,255), -1)
img = cv2.putText(img, str(k), (v-6, v+6), font,
fontScale, color, thickness, cv2.LINE_AA)

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

```

Let's take a look at the result. First, Let's "unblock" the straight road (which is 1-2-3-4). Note that keepout values for corresponding nodes are set to 255:

```
1: [ [2, 255],  [5, 255], [7, 64] ],
2: [ [1, 64], [3, 255] ],
3: [ [2, 64], [4, 255] ],
4: [ [3, 64], [5, 64],  [7, 64] ],
5: [ [1,255], [3, 64],  [4, 255] ],
6: [],
7: [ [1,128], [4, 64] ],
}

``` Now let's reduce quality of a 3-4 segment:

```
1: [ [2, 255],  [5, 255], [7, 64] ],
2: [ [1, 64], [3, 255] ],
3: [ [2, 64], [4, 64] ],
4: [ [3, 64], [5, 64],  [7, 64] ],
5: [ [1,255], [3, 64],  [4, 255] ],
6: [],
7: [ [1,128], [4, 64] ],
}

``` 