mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-15 00:28:35 -05:00
clean up voronoi road map
This commit is contained in:
Atsushi Sakai
106
PathPlanning/VoronoiRoadMap/dijkstra_search.py
Normal file
106
PathPlanning/VoronoiRoadMap/dijkstra_search.py
Normal file
@@ -0,0 +1,106 @@
|
||||
"""
|
||||
|
||||
Dijkstra Search library
|
||||
|
||||
author: Atsushi Sakai (@Atsushi_twi)
|
||||
|
||||
"""
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import math
|
||||
|
||||
|
||||
class DijkstraSearch:
|
||||
|
||||
class Node:
|
||||
"""
|
||||
Node class for dijkstra search
|
||||
"""
|
||||
|
||||
def __init__(self, x, y, cost, parent):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.cost = cost
|
||||
self.parent = parent
|
||||
|
||||
def __str__(self):
|
||||
return str(self.x) + "," + str(self.y) + "," + str(
|
||||
self.cost) + "," + str(self.parent)
|
||||
|
||||
def __init__(self, show_animation):
|
||||
self.show_animation = show_animation
|
||||
|
||||
def search(self, sx, sy, gx, gy, road_map, sample_x, sample_y):
|
||||
"""
|
||||
gx: goal x position [m]
|
||||
gx: goal x position [m]
|
||||
ox: x position list of Obstacles [m]
|
||||
oy: y position list of Obstacles [m]
|
||||
reso: grid resolution [m]
|
||||
rr: robot radius[m]
|
||||
"""
|
||||
|
||||
start_node = self.Node(sx, sy, 0.0, -1)
|
||||
goal_node = self.Node(gx, gy, 0.0, -1)
|
||||
|
||||
open_set, close_set = dict(), dict()
|
||||
open_set[len(road_map) - 2] = start_node
|
||||
|
||||
while True:
|
||||
if not open_set:
|
||||
print("Cannot find path")
|
||||
break
|
||||
|
||||
c_id = min(open_set, key=lambda o: open_set[o].cost)
|
||||
current = open_set[c_id]
|
||||
|
||||
# show graph
|
||||
if self.show_animation and len(
|
||||
close_set.keys()) % 2 == 0: # pragma: no cover
|
||||
plt.plot(current.x, current.y, "xg")
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect(
|
||||
'key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
plt.pause(0.001)
|
||||
|
||||
if c_id == (len(road_map) - 1):
|
||||
print("goal is found!")
|
||||
goal_node.parent = current.parent
|
||||
goal_node.cost = current.cost
|
||||
break
|
||||
|
||||
# Remove the item from the open set
|
||||
del open_set[c_id]
|
||||
# Add it to the closed set
|
||||
close_set[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.hypot(dx, dy)
|
||||
node = self.Node(sample_x[n_id], sample_y[n_id],
|
||||
current.cost + d, c_id)
|
||||
|
||||
if n_id in close_set:
|
||||
continue
|
||||
# Otherwise if it is already in the open set
|
||||
if n_id in open_set:
|
||||
if open_set[n_id].cost > node.cost:
|
||||
open_set[n_id].cost = node.cost
|
||||
open_set[n_id].parent = c_id
|
||||
else:
|
||||
open_set[n_id] = node
|
||||
|
||||
# generate final course
|
||||
rx, ry = [goal_node.x], [goal_node.y]
|
||||
parent = goal_node.parent
|
||||
while parent != -1:
|
||||
n = close_set[parent]
|
||||
rx.append(n.x)
|
||||
ry.append(n.y)
|
||||
parent = n.parent
|
||||
|
||||
return rx, ry
|
||||
49
PathPlanning/VoronoiRoadMap/kdtree.py
Normal file
49
PathPlanning/VoronoiRoadMap/kdtree.py
Normal file
@@ -0,0 +1,49 @@
|
||||
"""
|
||||
|
||||
Kd tree Search library
|
||||
|
||||
author: Atsushi Sakai (@Atsushi_twi)
|
||||
|
||||
"""
|
||||
|
||||
import scipy.spatial
|
||||
|
||||
|
||||
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
|
||||
@@ -10,252 +10,124 @@ import math
|
||||
import numpy as np
|
||||
import scipy.spatial
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
# parameter
|
||||
N_KNN = 10 # number of edge from one sampled point
|
||||
MAX_EDGE_LEN = 30.0 # [m] Maximum edge length
|
||||
from dijkstra_search import DijkstraSearch
|
||||
from kdtree import KDTree
|
||||
|
||||
show_animation = True
|
||||
|
||||
|
||||
class Node:
|
||||
"""
|
||||
Node class for dijkstra search
|
||||
"""
|
||||
class VoronoiRoadMapPlanner:
|
||||
|
||||
def __init__(self, x, y, cost, pind):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.cost = cost
|
||||
self.pind = pind
|
||||
def __init__(self):
|
||||
# parameter
|
||||
self.N_KNN = 10 # number of edge from one sampled point
|
||||
self.MAX_EDGE_LEN = 30.0 # [m] Maximum edge length
|
||||
|
||||
def __str__(self):
|
||||
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
|
||||
def planning(self, sx, sy, gx, gy, ox, oy, rr):
|
||||
obstacle_tree = KDTree(np.vstack((ox, oy)).T)
|
||||
|
||||
sample_x, sample_y = self.voronoi_sampling(sx, sy, gx, gy, ox, oy)
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(sample_x, sample_y, ".b")
|
||||
|
||||
class KDTree:
|
||||
"""
|
||||
Nearest neighbor search class with KDTree
|
||||
"""
|
||||
road_map = self.generate_road_map(sample_x, sample_y, rr, obstacle_tree)
|
||||
|
||||
def __init__(self, data):
|
||||
# store kd-tree
|
||||
self.tree = scipy.spatial.cKDTree(data)
|
||||
rx, ry = DijkstraSearch(show_animation).search(sx, sy, gx, gy, road_map,
|
||||
sample_x, sample_y)
|
||||
|
||||
def search(self, inp, k=1):
|
||||
"""
|
||||
Search NN
|
||||
return rx, ry
|
||||
|
||||
inp: input data, single frame or multi frame
|
||||
def is_collision(self, sx, sy, gx, gy, rr, obstacle_kdtree):
|
||||
x = sx
|
||||
y = sy
|
||||
dx = gx - sx
|
||||
dy = gy - sy
|
||||
yaw = math.atan2(gy - sy, gx - sx)
|
||||
d = math.hypot(dx, dy)
|
||||
|
||||
"""
|
||||
if d >= self.MAX_EDGE_LEN:
|
||||
return True
|
||||
|
||||
if len(inp.shape) >= 2: # multi input
|
||||
index = []
|
||||
dist = []
|
||||
D = rr
|
||||
n_step = round(d / D)
|
||||
|
||||
for i in inp.T:
|
||||
idist, iindex = self.tree.query(i, k=k)
|
||||
index.append(iindex)
|
||||
dist.append(idist)
|
||||
for i in range(n_step):
|
||||
ids, dist = obstacle_kdtree.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)
|
||||
|
||||
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 VRM_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: # pragma: no cover
|
||||
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.hypot(dx, dy)
|
||||
|
||||
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))
|
||||
# goal point check
|
||||
ids, dist = obstacle_kdtree.search(np.array([gx, gy]).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
|
||||
|
||||
return False # OK
|
||||
def generate_road_map(self, node_x, node_y, rr, obstacle_tree):
|
||||
"""
|
||||
Road map generation
|
||||
|
||||
sample_x: [m] x positions of sampled points
|
||||
sample_y: [m] y positions of sampled points
|
||||
rr: Robot Radius[m]
|
||||
obstacle_tree: KDTree object of obstacles
|
||||
"""
|
||||
|
||||
def generate_roadmap(sample_x, sample_y, rr, obkdtree):
|
||||
"""
|
||||
Road map generation
|
||||
road_map = []
|
||||
n_sample = len(node_x)
|
||||
node_tree = KDTree(np.vstack((node_x, node_y)).T)
|
||||
|
||||
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
|
||||
"""
|
||||
for (i, ix, iy) in zip(range(n_sample), node_x, node_y):
|
||||
|
||||
road_map = []
|
||||
nsample = len(sample_x)
|
||||
skdtree = KDTree(np.vstack((sample_x, sample_y)).T)
|
||||
index, dists = node_tree.search(
|
||||
np.array([ix, iy]).reshape(2, 1), k=n_sample)
|
||||
|
||||
for (i, ix, iy) in zip(range(nsample), sample_x, sample_y):
|
||||
inds = index[0]
|
||||
edge_id = []
|
||||
|
||||
index, dists = skdtree.search(
|
||||
np.array([ix, iy]).reshape(2, 1), k=nsample)
|
||||
for ii in range(1, len(inds)):
|
||||
nx = node_x[inds[ii]]
|
||||
ny = node_y[inds[ii]]
|
||||
|
||||
inds = index[0]
|
||||
edge_id = []
|
||||
# print(index)
|
||||
if not self.is_collision(ix, iy, nx, ny, rr, obstacle_tree):
|
||||
edge_id.append(inds[ii])
|
||||
|
||||
for ii in range(1, len(inds)):
|
||||
nx = sample_x[inds[ii]]
|
||||
ny = sample_y[inds[ii]]
|
||||
if len(edge_id) >= self.N_KNN:
|
||||
break
|
||||
|
||||
if not is_collision(ix, iy, nx, ny, rr, obkdtree):
|
||||
edge_id.append(inds[ii])
|
||||
road_map.append(edge_id)
|
||||
|
||||
if len(edge_id) >= N_KNN:
|
||||
break
|
||||
# plot_road_map(road_map, sample_x, sample_y)
|
||||
|
||||
road_map.append(edge_id)
|
||||
return road_map
|
||||
|
||||
# plot_road_map(road_map, sample_x, sample_y)
|
||||
@staticmethod
|
||||
def plot_road_map(road_map, sample_x, sample_y): # pragma: no cover
|
||||
|
||||
return road_map
|
||||
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 dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
|
||||
"""
|
||||
gx: goal x position [m]
|
||||
gx: goal x position [m]
|
||||
ox: x position list of Obstacles [m]
|
||||
oy: y position list of Obstacles [m]
|
||||
reso: grid resolution [m]
|
||||
rr: robot radius[m]
|
||||
"""
|
||||
@staticmethod
|
||||
def voronoi_sampling(sx, sy, gx, gy, ox, oy):
|
||||
oxy = np.vstack((ox, oy)).T
|
||||
|
||||
nstart = Node(sx, sy, 0.0, -1)
|
||||
ngoal = Node(gx, gy, 0.0, -1)
|
||||
# generate voronoi point
|
||||
vor = scipy.spatial.Voronoi(oxy)
|
||||
sample_x = [ix for [ix, _] in vor.vertices]
|
||||
sample_y = [iy for [_, iy] in vor.vertices]
|
||||
|
||||
openset, closedset = dict(), dict()
|
||||
openset[len(road_map) - 2] = nstart
|
||||
sample_x.append(sx)
|
||||
sample_y.append(sy)
|
||||
sample_x.append(gx)
|
||||
sample_y.append(gy)
|
||||
|
||||
while True:
|
||||
if not openset:
|
||||
print("Cannot find path")
|
||||
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: # pragma: no cover
|
||||
plt.plot(current.x, current.y, "xg")
|
||||
# for stopping simulation with the esc key.
|
||||
plt.gcf().canvas.mpl_connect('key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
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.hypot(dx, dy)
|
||||
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
|
||||
|
||||
# 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):
|
||||
oxy = np.vstack((ox, oy)).T
|
||||
|
||||
# generate voronoi point
|
||||
vor = scipy.spatial.Voronoi(oxy)
|
||||
sample_x = [ix for [ix, iy] in vor.vertices]
|
||||
sample_y = [iy for [ix, iy] in vor.vertices]
|
||||
|
||||
sample_x.append(sx)
|
||||
sample_y.append(sy)
|
||||
sample_x.append(gx)
|
||||
sample_y.append(gy)
|
||||
|
||||
return sample_x, sample_y
|
||||
return sample_x, sample_y
|
||||
|
||||
|
||||
def main():
|
||||
@@ -297,12 +169,14 @@ def main():
|
||||
plt.grid(True)
|
||||
plt.axis("equal")
|
||||
|
||||
rx, ry = VRM_planning(sx, sy, gx, gy, ox, oy, robot_size)
|
||||
rx, ry = VoronoiRoadMapPlanner().planning(sx, sy, gx, gy, ox, oy,
|
||||
robot_size)
|
||||
|
||||
assert rx, 'Cannot found path'
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(rx, ry, "-r")
|
||||
plt.pause(0.1)
|
||||
plt.show()
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user