mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-14 16:57:58 -05:00
268b3e8123
Added a simple boolean variable to track the state of the algorithm and return something empty when no path is found. Added and fixed (partially) the documentation
331 lines
7.7 KiB
Python
331 lines
7.7 KiB
Python
"""
|
|
|
|
Probablistic Road Map (PRM) Planner
|
|
|
|
author: Atsushi Sakai (@Atsushi_twi)
|
|
|
|
"""
|
|
|
|
import random
|
|
import math
|
|
import numpy as np
|
|
import scipy.spatial
|
|
import matplotlib.pyplot as plt
|
|
|
|
# parameter
|
|
N_SAMPLE = 500 # number of sample_points
|
|
N_KNN = 10 # number of edge from one sampled point
|
|
MAX_EDGE_LEN = 30.0 # [m] Maximum edge length
|
|
|
|
show_animation = True
|
|
|
|
|
|
class Node:
|
|
"""
|
|
Node class for dijkstra search
|
|
"""
|
|
|
|
def __init__(self, x, y, cost, pind):
|
|
self.x = x
|
|
self.y = y
|
|
self.cost = cost
|
|
self.pind = pind
|
|
|
|
def __str__(self):
|
|
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
|
|
|
|
|
|
class KDTree:
|
|
"""
|
|
Nearest neighbor search class with KDTree
|
|
"""
|
|
|
|
def __init__(self, data):
|
|
# store kd-tree
|
|
self.tree = scipy.spatial.cKDTree(data)
|
|
|
|
def search(self, inp, k=1):
|
|
"""
|
|
Search NN
|
|
|
|
inp: input data, single frame or multi frame
|
|
|
|
"""
|
|
|
|
if len(inp.shape) >= 2: # multi input
|
|
index = []
|
|
dist = []
|
|
|
|
for i in inp.T:
|
|
idist, iindex = self.tree.query(i, k=k)
|
|
index.append(iindex)
|
|
dist.append(idist)
|
|
|
|
return index, dist
|
|
|
|
dist, index = self.tree.query(inp, k=k)
|
|
return index, dist
|
|
|
|
def search_in_distance(self, inp, r):
|
|
"""
|
|
find points with in a distance r
|
|
"""
|
|
|
|
index = self.tree.query_ball_point(inp, r)
|
|
return index
|
|
|
|
|
|
def PRM_planning(sx, sy, gx, gy, ox, oy, rr):
|
|
|
|
obkdtree = KDTree(np.vstack((ox, oy)).T)
|
|
|
|
sample_x, sample_y = sample_points(sx, sy, gx, gy, rr, ox, oy, obkdtree)
|
|
if show_animation:
|
|
plt.plot(sample_x, sample_y, ".b")
|
|
|
|
road_map = generate_roadmap(sample_x, sample_y, rr, obkdtree)
|
|
|
|
rx, ry = dijkstra_planning(
|
|
sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y)
|
|
|
|
return rx, ry
|
|
|
|
|
|
def is_collision(sx, sy, gx, gy, rr, okdtree):
|
|
x = sx
|
|
y = sy
|
|
dx = gx - sx
|
|
dy = gy - sy
|
|
yaw = math.atan2(gy - sy, gx - sx)
|
|
d = math.sqrt(dx**2 + dy**2)
|
|
|
|
if d >= MAX_EDGE_LEN:
|
|
return True
|
|
|
|
D = rr
|
|
nstep = round(d / D)
|
|
|
|
for i in range(nstep):
|
|
idxs, dist = okdtree.search(np.array([x, y]).reshape(2, 1))
|
|
if dist[0] <= rr:
|
|
return True # collision
|
|
x += D * math.cos(yaw)
|
|
y += D * math.sin(yaw)
|
|
|
|
# goal point check
|
|
idxs, dist = okdtree.search(np.array([gx, gy]).reshape(2, 1))
|
|
if dist[0] <= rr:
|
|
return True # collision
|
|
|
|
return False # OK
|
|
|
|
|
|
def generate_roadmap(sample_x, sample_y, rr, obkdtree):
|
|
"""
|
|
Road map generation
|
|
|
|
sample_x: [m] x positions of sampled points
|
|
sample_y: [m] y positions of sampled points
|
|
rr: Robot Radius[m]
|
|
obkdtree: KDTree object of obstacles
|
|
"""
|
|
|
|
road_map = []
|
|
nsample = len(sample_x)
|
|
skdtree = KDTree(np.vstack((sample_x, sample_y)).T)
|
|
|
|
for (i, ix, iy) in zip(range(nsample), sample_x, sample_y):
|
|
|
|
index, dists = skdtree.search(
|
|
np.array([ix, iy]).reshape(2, 1), k=nsample)
|
|
inds = index[0]
|
|
edge_id = []
|
|
# print(index)
|
|
|
|
for ii in range(1, len(inds)):
|
|
nx = sample_x[inds[ii]]
|
|
ny = sample_y[inds[ii]]
|
|
|
|
if not is_collision(ix, iy, nx, ny, rr, obkdtree):
|
|
edge_id.append(inds[ii])
|
|
|
|
if len(edge_id) >= N_KNN:
|
|
break
|
|
|
|
road_map.append(edge_id)
|
|
|
|
# plot_road_map(road_map, sample_x, sample_y)
|
|
|
|
return road_map
|
|
|
|
|
|
def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
|
|
"""
|
|
sx: start x position [m]
|
|
sy: start y position [m]
|
|
gx: goal x position [m]
|
|
gy: goal y position [m]
|
|
ox: x position list of Obstacles [m]
|
|
oy: y position list of Obstacles [m]
|
|
rr: robot radius [m]
|
|
road_map: ??? [m]
|
|
sample_x: ??? [m]
|
|
sample_y: ??? [m]
|
|
|
|
@return: Two lists of path coordinates ([x1, x2, ...], [y1, y2, ...]), empty list when no path was found
|
|
"""
|
|
|
|
nstart = Node(sx, sy, 0.0, -1)
|
|
ngoal = Node(gx, gy, 0.0, -1)
|
|
|
|
openset, closedset = dict(), dict()
|
|
openset[len(road_map) - 2] = nstart
|
|
|
|
path_found = True
|
|
|
|
while True:
|
|
if not openset:
|
|
print("Cannot find path")
|
|
path_found = False
|
|
break
|
|
|
|
c_id = min(openset, key=lambda o: openset[o].cost)
|
|
current = openset[c_id]
|
|
|
|
# show graph
|
|
if show_animation and len(closedset.keys()) % 2 == 0:
|
|
plt.plot(current.x, current.y, "xg")
|
|
plt.pause(0.001)
|
|
|
|
if c_id == (len(road_map) - 1):
|
|
print("goal is found!")
|
|
ngoal.pind = current.pind
|
|
ngoal.cost = current.cost
|
|
break
|
|
|
|
# Remove the item from the open set
|
|
del openset[c_id]
|
|
# Add it to the closed set
|
|
closedset[c_id] = current
|
|
|
|
# expand search grid based on motion model
|
|
for i in range(len(road_map[c_id])):
|
|
n_id = road_map[c_id][i]
|
|
dx = sample_x[n_id] - current.x
|
|
dy = sample_y[n_id] - current.y
|
|
d = math.sqrt(dx**2 + dy**2)
|
|
node = Node(sample_x[n_id], sample_y[n_id],
|
|
current.cost + d, c_id)
|
|
|
|
if n_id in closedset:
|
|
continue
|
|
# Otherwise if it is already in the open set
|
|
if n_id in openset:
|
|
if openset[n_id].cost > node.cost:
|
|
openset[n_id].cost = node.cost
|
|
openset[n_id].pind = c_id
|
|
else:
|
|
openset[n_id] = node
|
|
|
|
if path_found is False:
|
|
return [], []
|
|
|
|
# generate final course
|
|
rx, ry = [ngoal.x], [ngoal.y]
|
|
pind = ngoal.pind
|
|
while pind != -1:
|
|
n = closedset[pind]
|
|
rx.append(n.x)
|
|
ry.append(n.y)
|
|
pind = n.pind
|
|
|
|
return rx, ry
|
|
|
|
|
|
def plot_road_map(road_map, sample_x, sample_y): # pragma: no cover
|
|
|
|
for i, _ in enumerate(road_map):
|
|
for ii in range(len(road_map[i])):
|
|
ind = road_map[i][ii]
|
|
|
|
plt.plot([sample_x[i], sample_x[ind]],
|
|
[sample_y[i], sample_y[ind]], "-k")
|
|
|
|
|
|
def sample_points(sx, sy, gx, gy, rr, ox, oy, obkdtree):
|
|
maxx = max(ox)
|
|
maxy = max(oy)
|
|
minx = min(ox)
|
|
miny = min(oy)
|
|
|
|
sample_x, sample_y = [], []
|
|
|
|
while len(sample_x) <= N_SAMPLE:
|
|
tx = (random.random() * (maxx - minx)) + minx
|
|
ty = (random.random() * (maxy - miny)) + miny
|
|
|
|
index, dist = obkdtree.search(np.array([tx, ty]).reshape(2, 1))
|
|
|
|
if dist[0] >= rr:
|
|
sample_x.append(tx)
|
|
sample_y.append(ty)
|
|
|
|
sample_x.append(sx)
|
|
sample_y.append(sy)
|
|
sample_x.append(gx)
|
|
sample_y.append(gy)
|
|
|
|
return sample_x, sample_y
|
|
|
|
|
|
def main():
|
|
print(__file__ + " start!!")
|
|
|
|
# start and goal position
|
|
sx = 10.0 # [m]
|
|
sy = 10.0 # [m]
|
|
gx = 50.0 # [m]
|
|
gy = 50.0 # [m]
|
|
robot_size = 5.0 # [m]
|
|
|
|
ox = []
|
|
oy = []
|
|
|
|
for i in range(60):
|
|
ox.append(i)
|
|
oy.append(0.0)
|
|
for i in range(60):
|
|
ox.append(60.0)
|
|
oy.append(i)
|
|
for i in range(61):
|
|
ox.append(i)
|
|
oy.append(60.0)
|
|
for i in range(61):
|
|
ox.append(0.0)
|
|
oy.append(i)
|
|
for i in range(40):
|
|
ox.append(20.0)
|
|
oy.append(i)
|
|
for i in range(40):
|
|
ox.append(40.0)
|
|
oy.append(60.0 - i)
|
|
|
|
if show_animation:
|
|
plt.plot(ox, oy, ".k")
|
|
plt.plot(sx, sy, "^r")
|
|
plt.plot(gx, gy, "^c")
|
|
plt.grid(True)
|
|
plt.axis("equal")
|
|
|
|
rx, ry = PRM_planning(sx, sy, gx, gy, ox, oy, robot_size)
|
|
|
|
assert rx, 'Cannot found path'
|
|
|
|
if show_animation:
|
|
plt.plot(rx, ry, "-r")
|
|
plt.show()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|