mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:41 -04:00
Merge pull request #297 from AtsushiSakai/issue_294
Visibility Graph Path Planner implemenation
This commit is contained in:
Atsushi Sakai
@@ -11,32 +11,42 @@ import math
|
||||
|
||||
show_animation = True
|
||||
|
||||
|
||||
class Dijkstra:
|
||||
|
||||
def __init__(self, ox, oy, reso, rr):
|
||||
def __init__(self, ox, oy, resolution, robot_radius):
|
||||
"""
|
||||
Initialize map for a star planning
|
||||
|
||||
ox: x position list of Obstacles [m]
|
||||
oy: y position list of Obstacles [m]
|
||||
reso: grid resolution [m]
|
||||
resolution: grid resolution [m]
|
||||
rr: robot radius[m]
|
||||
"""
|
||||
|
||||
self.reso = reso
|
||||
self.rr = rr
|
||||
self.min_x = None
|
||||
self.min_y = None
|
||||
self.max_x = None
|
||||
self.max_y = None
|
||||
self.x_width = None
|
||||
self.y_width = None
|
||||
self.obstacle_map = None
|
||||
|
||||
self.resolution = resolution
|
||||
self.robot_radius = robot_radius
|
||||
self.calc_obstacle_map(ox, oy)
|
||||
self.motion = self.get_motion_model()
|
||||
|
||||
class Node:
|
||||
def __init__(self, x, y, cost, pind):
|
||||
def __init__(self, x, y, cost, parent):
|
||||
self.x = x # index of grid
|
||||
self.y = y # index of grid
|
||||
self.cost = cost
|
||||
self.pind = pind # index of previous Node
|
||||
self.parent = parent # index of previous Node
|
||||
|
||||
def __str__(self):
|
||||
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
|
||||
return str(self.x) + "," + str(self.y) + "," + str(
|
||||
self.cost) + "," + str(self.parent)
|
||||
|
||||
def planning(self, sx, sy, gx, gy):
|
||||
"""
|
||||
@@ -53,39 +63,40 @@ class Dijkstra:
|
||||
ry: y position list of the final path
|
||||
"""
|
||||
|
||||
nstart = self.Node(self.calc_xyindex(sx, self.minx),
|
||||
self.calc_xyindex(sy, self.miny), 0.0, -1)
|
||||
ngoal = self.Node(self.calc_xyindex(gx, self.minx),
|
||||
self.calc_xyindex(gy, self.miny), 0.0, -1)
|
||||
start_node = self.Node(self.calc_xy_index(sx, self.min_x),
|
||||
self.calc_xy_index(sy, self.min_y), 0.0, -1)
|
||||
goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
|
||||
self.calc_xy_index(gy, self.min_y), 0.0, -1)
|
||||
|
||||
openset, closedset = dict(), dict()
|
||||
openset[self.calc_index(nstart)] = nstart
|
||||
open_set, closed_set = dict(), dict()
|
||||
open_set[self.calc_index(start_node)] = start_node
|
||||
|
||||
while 1:
|
||||
c_id = min(openset, key=lambda o: openset[o].cost)
|
||||
current = openset[c_id]
|
||||
c_id = min(open_set, key=lambda o: open_set[o].cost)
|
||||
current = open_set[c_id]
|
||||
|
||||
# show graph
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(self.calc_position(current.x, self.minx),
|
||||
self.calc_position(current.y, self.miny), "xc")
|
||||
plt.plot(self.calc_position(current.x, self.min_x),
|
||||
self.calc_position(current.y, self.min_y), "xc")
|
||||
# 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])
|
||||
if len(closedset.keys()) % 10 == 0:
|
||||
plt.gcf().canvas.mpl_connect(
|
||||
'key_release_event',
|
||||
lambda event: [exit(0) if event.key == 'escape' else None])
|
||||
if len(closed_set.keys()) % 10 == 0:
|
||||
plt.pause(0.001)
|
||||
|
||||
if current.x == ngoal.x and current.y == ngoal.y:
|
||||
if current.x == goal_node.x and current.y == goal_node.y:
|
||||
print("Find goal")
|
||||
ngoal.pind = current.pind
|
||||
ngoal.cost = current.cost
|
||||
goal_node.parent = current.parent
|
||||
goal_node.cost = current.cost
|
||||
break
|
||||
|
||||
# Remove the item from the open set
|
||||
del openset[c_id]
|
||||
del open_set[c_id]
|
||||
|
||||
# Add it to the closed set
|
||||
closedset[c_id] = current
|
||||
closed_set[c_id] = current
|
||||
|
||||
# expand search grid based on motion model
|
||||
for move_x, move_y, move_cost in self.motion:
|
||||
@@ -94,94 +105,95 @@ class Dijkstra:
|
||||
current.cost + move_cost, c_id)
|
||||
n_id = self.calc_index(node)
|
||||
|
||||
if n_id in closedset:
|
||||
if n_id in closed_set:
|
||||
continue
|
||||
|
||||
if not self.verify_node(node):
|
||||
continue
|
||||
|
||||
if n_id not in openset:
|
||||
openset[n_id] = node # Discover a new node
|
||||
if n_id not in open_set:
|
||||
open_set[n_id] = node # Discover a new node
|
||||
else:
|
||||
if openset[n_id].cost >= node.cost:
|
||||
if open_set[n_id].cost >= node.cost:
|
||||
# This path is the best until now. record it!
|
||||
openset[n_id] = node
|
||||
open_set[n_id] = node
|
||||
|
||||
rx, ry = self.calc_final_path(ngoal, closedset)
|
||||
rx, ry = self.calc_final_path(goal_node, closed_set)
|
||||
|
||||
return rx, ry
|
||||
|
||||
def calc_final_path(self, ngoal, closedset):
|
||||
def calc_final_path(self, goal_node, closed_set):
|
||||
# generate final course
|
||||
rx, ry = [self.calc_position(ngoal.x, self.minx)], [
|
||||
self.calc_position(ngoal.y, self.miny)]
|
||||
pind = ngoal.pind
|
||||
while pind != -1:
|
||||
n = closedset[pind]
|
||||
rx.append(self.calc_position(n.x, self.minx))
|
||||
ry.append(self.calc_position(n.y, self.miny))
|
||||
pind = n.pind
|
||||
rx, ry = [self.calc_position(goal_node.x, self.min_x)], [
|
||||
self.calc_position(goal_node.y, self.min_y)]
|
||||
parent = goal_node.parent
|
||||
while parent != -1:
|
||||
n = closed_set[parent]
|
||||
rx.append(self.calc_position(n.x, self.min_x))
|
||||
ry.append(self.calc_position(n.y, self.min_y))
|
||||
parent = n.parent
|
||||
|
||||
return rx, ry
|
||||
|
||||
def calc_position(self, index, minp):
|
||||
pos = index*self.reso+minp
|
||||
pos = index * self.resolution + minp
|
||||
return pos
|
||||
|
||||
def calc_xyindex(self, position, minp):
|
||||
return round((position - minp)/self.reso)
|
||||
def calc_xy_index(self, position, minp):
|
||||
return round((position - minp) / self.resolution)
|
||||
|
||||
def calc_index(self, node):
|
||||
return (node.y - self.miny) * self.xwidth + (node.x - self.minx)
|
||||
return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)
|
||||
|
||||
def verify_node(self, node):
|
||||
px = self.calc_position(node.x, self.minx)
|
||||
py = self.calc_position(node.y, self.miny)
|
||||
px = self.calc_position(node.x, self.min_x)
|
||||
py = self.calc_position(node.y, self.min_y)
|
||||
|
||||
if px < self.minx:
|
||||
if px < self.min_x:
|
||||
return False
|
||||
elif py < self.miny:
|
||||
if py < self.min_y:
|
||||
return False
|
||||
elif px >= self.maxx:
|
||||
if px >= self.max_x:
|
||||
return False
|
||||
elif py >= self.maxy:
|
||||
if py >= self.max_y:
|
||||
return False
|
||||
|
||||
if self.obmap[node.x][node.y]:
|
||||
if self.obstacle_map[node.x][node.y]:
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def calc_obstacle_map(self, ox, oy):
|
||||
|
||||
self.minx = round(min(ox))
|
||||
self.miny = round(min(oy))
|
||||
self.maxx = round(max(ox))
|
||||
self.maxy = round(max(oy))
|
||||
print("minx:", self.minx)
|
||||
print("miny:", self.miny)
|
||||
print("maxx:", self.maxx)
|
||||
print("maxy:", self.maxy)
|
||||
self.min_x = round(min(ox))
|
||||
self.min_y = round(min(oy))
|
||||
self.max_x = round(max(ox))
|
||||
self.max_y = round(max(oy))
|
||||
print("min_x:", self.min_x)
|
||||
print("min_y:", self.min_y)
|
||||
print("max_x:", self.max_x)
|
||||
print("max_y:", self.max_y)
|
||||
|
||||
self.xwidth = round((self.maxx - self.minx)/self.reso)
|
||||
self.ywidth = round((self.maxy - self.miny)/self.reso)
|
||||
print("xwidth:", self.xwidth)
|
||||
print("ywidth:", self.ywidth)
|
||||
self.x_width = round((self.max_x - self.min_x) / self.resolution)
|
||||
self.y_width = round((self.max_y - self.min_y) / self.resolution)
|
||||
print("x_width:", self.x_width)
|
||||
print("y_width:", self.y_width)
|
||||
|
||||
# obstacle map generation
|
||||
self.obmap = [[False for i in range(self.ywidth)]
|
||||
for i in range(self.xwidth)]
|
||||
for ix in range(self.xwidth):
|
||||
x = self.calc_position(ix, self.minx)
|
||||
for iy in range(self.ywidth):
|
||||
y = self.calc_position(iy, self.miny)
|
||||
self.obstacle_map = [[False for _ in range(self.y_width)]
|
||||
for _ in range(self.x_width)]
|
||||
for ix in range(self.x_width):
|
||||
x = self.calc_position(ix, self.min_x)
|
||||
for iy in range(self.y_width):
|
||||
y = self.calc_position(iy, self.min_y)
|
||||
for iox, ioy in zip(ox, oy):
|
||||
d = math.hypot(iox - x, ioy - y)
|
||||
if d <= self.rr:
|
||||
self.obmap[ix][iy] = True
|
||||
if d <= self.robot_radius:
|
||||
self.obstacle_map[ix][iy] = True
|
||||
break
|
||||
|
||||
def get_motion_model(self):
|
||||
@staticmethod
|
||||
def get_motion_model():
|
||||
# dx, dy, cost
|
||||
motion = [[1, 0, 1],
|
||||
[0, 1, 1],
|
||||
@@ -239,6 +251,7 @@ def main():
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(rx, ry, "-r")
|
||||
plt.pause(0.01)
|
||||
plt.show()
|
||||
|
||||
|
||||
|
||||
0
PathPlanning/VisibilityRoadMap/__init__.py
Normal file
0
PathPlanning/VisibilityRoadMap/__init__.py
Normal file
43
PathPlanning/VisibilityRoadMap/geometry.py
Normal file
43
PathPlanning/VisibilityRoadMap/geometry.py
Normal file
@@ -0,0 +1,43 @@
|
||||
class Geometry:
|
||||
class Point:
|
||||
def __init__(self, x, y):
|
||||
self.x = x
|
||||
self.y = y
|
||||
|
||||
@staticmethod
|
||||
def is_seg_intersect(p1, q1, p2, q2):
|
||||
|
||||
def on_segment(p, q, r):
|
||||
if ((q.x <= max(p.x, r.x)) and (q.x >= min(p.x, r.x)) and
|
||||
(q.y <= max(p.y, r.y)) and (q.y >= min(p.y, r.y))):
|
||||
return True
|
||||
return False
|
||||
|
||||
def orientation(p, q, r):
|
||||
val = (float(q.y - p.y) * (r.x - q.x)) - (
|
||||
float(q.x - p.x) * (r.y - q.y))
|
||||
if val > 0:
|
||||
return 1
|
||||
if val < 0:
|
||||
return 2
|
||||
return 0
|
||||
|
||||
# Find the 4 orientations required for
|
||||
# the general and special cases
|
||||
o1 = orientation(p1, q1, p2)
|
||||
o2 = orientation(p1, q1, q2)
|
||||
o3 = orientation(p2, q2, p1)
|
||||
o4 = orientation(p2, q2, q1)
|
||||
|
||||
if (o1 != o2) and (o3 != o4):
|
||||
return True
|
||||
if (o1 == 0) and on_segment(p1, p2, q1):
|
||||
return True
|
||||
if (o2 == 0) and on_segment(p1, q2, q1):
|
||||
return True
|
||||
if (o3 == 0) and on_segment(p2, p1, q2):
|
||||
return True
|
||||
if (o4 == 0) and on_segment(p2, q1, q2):
|
||||
return True
|
||||
|
||||
return False
|
||||
222
PathPlanning/VisibilityRoadMap/visibility_road_map.py
Normal file
222
PathPlanning/VisibilityRoadMap/visibility_road_map.py
Normal file
@@ -0,0 +1,222 @@
|
||||
"""
|
||||
|
||||
Visibility Road Map Planner
|
||||
|
||||
author: Atsushi Sakai (@Atsushi_twi)
|
||||
|
||||
"""
|
||||
|
||||
import os
|
||||
import sys
|
||||
import math
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
from PathPlanning.VisibilityRoadMap.geometry import Geometry
|
||||
|
||||
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
|
||||
"/../VoronoiRoadMap/")
|
||||
from dijkstra_search import DijkstraSearch
|
||||
|
||||
show_animation = True
|
||||
|
||||
|
||||
class VisibilityRoadMap:
|
||||
|
||||
def __init__(self, robot_radius, do_plot=False):
|
||||
self.robot_radius = robot_radius
|
||||
self.do_plot = do_plot
|
||||
|
||||
def planning(self, start_x, start_y, goal_x, goal_y, obstacles):
|
||||
|
||||
nodes = self.generate_graph_node(start_x, start_y, goal_x, goal_y,
|
||||
obstacles)
|
||||
|
||||
road_map_info = self.generate_road_map_info(nodes, obstacles)
|
||||
|
||||
if self.do_plot:
|
||||
self.plot_road_map(nodes, road_map_info)
|
||||
plt.pause(1.0)
|
||||
|
||||
rx, ry = DijkstraSearch(show_animation).search(
|
||||
start_x, start_y,
|
||||
goal_x, goal_y,
|
||||
[node.x for node in nodes],
|
||||
[node.y for node in nodes],
|
||||
road_map_info
|
||||
)
|
||||
|
||||
return rx, ry
|
||||
|
||||
def generate_graph_node(self, start_x, start_y, goal_x, goal_y, obstacles):
|
||||
|
||||
# add start and goal as nodes
|
||||
nodes = [DijkstraSearch.Node(start_x, start_y),
|
||||
DijkstraSearch.Node(goal_x, goal_y, 0, None)]
|
||||
|
||||
# add vertexes in configuration space as nodes
|
||||
for obstacle in obstacles:
|
||||
|
||||
cvx_list, cvy_list = self.calc_vertexes_in_configuration_space(
|
||||
obstacle.x_list, obstacle.y_list)
|
||||
|
||||
for (vx, vy) in zip(cvx_list, cvy_list):
|
||||
nodes.append(DijkstraSearch.Node(vx, vy))
|
||||
|
||||
for node in nodes:
|
||||
plt.plot(node.x, node.y, "xr")
|
||||
|
||||
return nodes
|
||||
|
||||
def calc_vertexes_in_configuration_space(self, x_list, y_list):
|
||||
x_list = x_list[0:-1]
|
||||
y_list = y_list[0:-1]
|
||||
cvx_list, cvy_list = [], []
|
||||
|
||||
n_data = len(x_list)
|
||||
|
||||
for index in range(n_data):
|
||||
offset_x, offset_y = self.calc_offset_xy(
|
||||
x_list[index - 1], y_list[index - 1],
|
||||
x_list[index], y_list[index],
|
||||
x_list[(index + 1) % n_data], y_list[(index + 1) % n_data],
|
||||
)
|
||||
cvx_list.append(offset_x)
|
||||
cvy_list.append(offset_y)
|
||||
|
||||
return cvx_list, cvy_list
|
||||
|
||||
def generate_road_map_info(self, nodes, obstacles):
|
||||
|
||||
road_map_info_list = []
|
||||
|
||||
for target_node in nodes:
|
||||
road_map_info = []
|
||||
for node_id, node in enumerate(nodes):
|
||||
if np.hypot(target_node.x - node.x,
|
||||
target_node.y - node.y) <= 0.1:
|
||||
continue
|
||||
|
||||
is_valid = True
|
||||
for obstacle in obstacles:
|
||||
if not self.is_edge_valid(target_node, node, obstacle):
|
||||
is_valid = False
|
||||
break
|
||||
if is_valid:
|
||||
road_map_info.append(node_id)
|
||||
|
||||
road_map_info_list.append(road_map_info)
|
||||
|
||||
return road_map_info_list
|
||||
|
||||
@staticmethod
|
||||
def is_edge_valid(target_node, node, obstacle):
|
||||
|
||||
for i in range(len(obstacle.x_list) - 1):
|
||||
p1 = Geometry.Point(target_node.x, target_node.y)
|
||||
p2 = Geometry.Point(node.x, node.y)
|
||||
p3 = Geometry.Point(obstacle.x_list[i], obstacle.y_list[i])
|
||||
p4 = Geometry.Point(obstacle.x_list[i + 1], obstacle.y_list[i + 1])
|
||||
|
||||
if Geometry.is_seg_intersect(p1, p2, p3, p4):
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def calc_offset_xy(self, px, py, x, y, nx, ny):
|
||||
p_vec = math.atan2(y - py, x - px)
|
||||
n_vec = math.atan2(ny - y, nx - x)
|
||||
offset_vec = math.atan2(math.sin(p_vec) + math.sin(n_vec),
|
||||
math.cos(p_vec) + math.cos(
|
||||
n_vec)) + math.pi / 2.0
|
||||
offset_x = x + self.robot_radius * math.cos(offset_vec)
|
||||
offset_y = y + self.robot_radius * math.sin(offset_vec)
|
||||
return offset_x, offset_y
|
||||
|
||||
@staticmethod
|
||||
def plot_road_map(nodes, road_map_info_list):
|
||||
for i, node in enumerate(nodes):
|
||||
for index in road_map_info_list[i]:
|
||||
plt.plot([node.x, nodes[index].x],
|
||||
[node.y, nodes[index].y], "-b")
|
||||
|
||||
|
||||
class ObstaclePolygon:
|
||||
|
||||
def __init__(self, x_list, y_list):
|
||||
self.x_list = x_list
|
||||
self.y_list = y_list
|
||||
|
||||
self.close_polygon()
|
||||
self.make_clockwise()
|
||||
|
||||
def make_clockwise(self):
|
||||
if not self.is_clockwise():
|
||||
self.x_list = list(reversed(self.x_list))
|
||||
self.y_list = list(reversed(self.y_list))
|
||||
|
||||
def is_clockwise(self):
|
||||
n_data = len(self.x_list)
|
||||
eval_sum = sum([(self.x_list[i + 1] - self.x_list[i]) *
|
||||
(self.y_list[i + 1] + self.y_list[i])
|
||||
for i in range(n_data - 1)])
|
||||
eval_sum += (self.x_list[0] - self.x_list[n_data - 1]) * \
|
||||
(self.y_list[0] + self.y_list[n_data - 1])
|
||||
return eval_sum >= 0
|
||||
|
||||
def close_polygon(self):
|
||||
is_x_same = self.x_list[0] == self.x_list[-1]
|
||||
is_y_same = self.y_list[0] == self.y_list[-1]
|
||||
if is_x_same and is_y_same:
|
||||
return # no need to close
|
||||
|
||||
self.x_list.append(self.x_list[0])
|
||||
self.y_list.append(self.y_list[0])
|
||||
|
||||
def plot(self):
|
||||
plt.plot(self.x_list, self.y_list, "-k")
|
||||
|
||||
|
||||
def main():
|
||||
print(__file__ + " start!!")
|
||||
|
||||
# start and goal position
|
||||
sx, sy = 10.0, 10.0 # [m]
|
||||
gx, gy = 50.0, 50.0 # [m]
|
||||
|
||||
robot_radius = 5.0 # [m]
|
||||
|
||||
obstacles = [
|
||||
ObstaclePolygon(
|
||||
[20.0, 30.0, 15.0],
|
||||
[20.0, 20.0, 30.0],
|
||||
),
|
||||
ObstaclePolygon(
|
||||
[40.0, 45.0, 50.0, 40.0],
|
||||
[50.0, 40.0, 20.0, 40.0],
|
||||
),
|
||||
ObstaclePolygon(
|
||||
[20.0, 30.0, 30.0, 20.0],
|
||||
[40.0, 45.0, 60.0, 50.0],
|
||||
)
|
||||
]
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(sx, sy, "or")
|
||||
plt.plot(gx, gy, "ob")
|
||||
for ob in obstacles:
|
||||
ob.plot()
|
||||
plt.axis("equal")
|
||||
plt.pause(1.0)
|
||||
|
||||
rx, ry = VisibilityRoadMap(robot_radius, do_plot=show_animation).planning(
|
||||
sx, sy, gx, gy, obstacles)
|
||||
|
||||
if show_animation: # pragma: no cover
|
||||
plt.plot(rx, ry, "-r")
|
||||
plt.pause(0.1)
|
||||
plt.show()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
139
PathPlanning/VoronoiRoadMap/dijkstra_search.py
Normal file
139
PathPlanning/VoronoiRoadMap/dijkstra_search.py
Normal file
@@ -0,0 +1,139 @@
|
||||
"""
|
||||
|
||||
Dijkstra Search library
|
||||
|
||||
author: Atsushi Sakai (@Atsushi_twi)
|
||||
|
||||
"""
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
|
||||
class DijkstraSearch:
|
||||
class Node:
|
||||
"""
|
||||
Node class for dijkstra search
|
||||
"""
|
||||
|
||||
def __init__(self, x, y, cost=None, parent=None, edge_ids=None):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.cost = cost
|
||||
self.parent = parent
|
||||
self.edge_ids = edge_ids
|
||||
|
||||
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, node_x, node_y, edge_ids_list):
|
||||
"""
|
||||
Search shortest path
|
||||
|
||||
sx: start x positions [m]
|
||||
sy: start y positions [m]
|
||||
gx: goal x position [m]
|
||||
gx: goal x position [m]
|
||||
node_x: node x position
|
||||
node_y: node y position
|
||||
edge_ids_list: edge_list each item includes a list of edge ids
|
||||
"""
|
||||
|
||||
start_node = self.Node(sx, sy, 0.0, -1)
|
||||
goal_node = self.Node(gx, gy, 0.0, -1)
|
||||
current_node = None
|
||||
|
||||
open_set, close_set = dict(), dict()
|
||||
open_set[self.find_id(node_x, node_y, start_node)] = start_node
|
||||
|
||||
while True:
|
||||
if self.has_node_in_set(close_set, goal_node):
|
||||
print("goal is found!")
|
||||
goal_node.parent = current_node.parent
|
||||
goal_node.cost = current_node.cost
|
||||
break
|
||||
elif not open_set:
|
||||
print("Cannot find path")
|
||||
break
|
||||
|
||||
current_id = min(open_set, key=lambda o: open_set[o].cost)
|
||||
current_node = open_set[current_id]
|
||||
|
||||
# show graph
|
||||
if self.show_animation and len(
|
||||
close_set.keys()) % 2 == 0: # pragma: no cover
|
||||
plt.plot(current_node.x, current_node.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.1)
|
||||
|
||||
# Remove the item from the open set
|
||||
del open_set[current_id]
|
||||
# Add it to the closed set
|
||||
close_set[current_id] = current_node
|
||||
|
||||
# expand search grid based on motion model
|
||||
for i in range(len(edge_ids_list[current_id])):
|
||||
n_id = edge_ids_list[current_id][i]
|
||||
dx = node_x[n_id] - current_node.x
|
||||
dy = node_y[n_id] - current_node.y
|
||||
d = math.hypot(dx, dy)
|
||||
node = self.Node(node_x[n_id], node_y[n_id],
|
||||
current_node.cost + d, current_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] = node
|
||||
else:
|
||||
open_set[n_id] = node
|
||||
|
||||
# generate final course
|
||||
rx, ry = self.generate_final_path(close_set, goal_node)
|
||||
|
||||
return rx, ry
|
||||
|
||||
@staticmethod
|
||||
def generate_final_path(close_set, goal_node):
|
||||
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
|
||||
rx, ry = rx[::-1], ry[::-1] # reverse it
|
||||
return rx, ry
|
||||
|
||||
def has_node_in_set(self, target_set, node):
|
||||
for key in target_set:
|
||||
if self.is_same_node(target_set[key], node):
|
||||
return True
|
||||
return False
|
||||
|
||||
def find_id(self, node_x_list, node_y_list, target_node):
|
||||
for i, _ in enumerate(node_x_list):
|
||||
if self.is_same_node_with_xy(node_x_list[i], node_y_list[i],
|
||||
target_node):
|
||||
return i
|
||||
|
||||
@staticmethod
|
||||
def is_same_node_with_xy(node_x, node_y, node_b):
|
||||
dist = np.hypot(node_x - node_b.x,
|
||||
node_y - node_b.y)
|
||||
return dist <= 0.1
|
||||
|
||||
@staticmethod
|
||||
def is_same_node(node_a, node_b):
|
||||
dist = np.hypot(node_a.x - node_b.x,
|
||||
node_b.y - node_b.y)
|
||||
return dist <= 0.1
|
||||
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,125 @@ 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, robot_radius):
|
||||
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_info = self.generate_road_map_info(
|
||||
sample_x, sample_y, robot_radius, 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,
|
||||
sample_x, sample_y,
|
||||
road_map_info)
|
||||
return rx, ry
|
||||
|
||||
def search(self, inp, k=1):
|
||||
"""
|
||||
Search NN
|
||||
def is_collision(self, sx, sy, gx, gy, rr, obstacle_kd_tree):
|
||||
x = sx
|
||||
y = sy
|
||||
dx = gx - sx
|
||||
dy = gy - sy
|
||||
yaw = math.atan2(gy - sy, gx - sx)
|
||||
d = math.hypot(dx, dy)
|
||||
|
||||
inp: input data, single frame or multi frame
|
||||
if d >= self.MAX_EDGE_LEN:
|
||||
return True
|
||||
|
||||
"""
|
||||
D = rr
|
||||
n_step = round(d / D)
|
||||
|
||||
if len(inp.shape) >= 2: # multi input
|
||||
index = []
|
||||
dist = []
|
||||
for i in range(n_step):
|
||||
ids, dist = obstacle_kd_tree.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)
|
||||
|
||||
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 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_kd_tree.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_info(self, node_x, node_y, rr, obstacle_tree):
|
||||
"""
|
||||
Road map generation
|
||||
|
||||
node_x: [m] x positions of sampled points
|
||||
node_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 +170,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()
|
||||
|
||||
|
||||
|
||||
@@ -2,6 +2,7 @@ from unittest import TestCase
|
||||
|
||||
import sys
|
||||
import os
|
||||
|
||||
sys.path.append(os.path.dirname(os.path.abspath(__file__))
|
||||
+ "/../PathPlanning/RRTStarDubins/")
|
||||
|
||||
|
||||
@@ -1,6 +1,12 @@
|
||||
import sys
|
||||
import os
|
||||
sys.path.append(os.path.dirname(os.path.abspath(__file__))
|
||||
+ "/../PathPlanning/VoronoiRoadMap/")
|
||||
|
||||
from unittest import TestCase
|
||||
from PathPlanning.VoronoiRoadMap import voronoi_road_map as m
|
||||
|
||||
|
||||
print(__file__)
|
||||
|
||||
|
||||
17
tests/test_voronoi_road_map_planner.py
Normal file
17
tests/test_voronoi_road_map_planner.py
Normal file
@@ -0,0 +1,17 @@
|
||||
import os
|
||||
import sys
|
||||
|
||||
sys.path.append(os.path.dirname(os.path.abspath(__file__))
|
||||
+ "/../PathPlanning/VisibilityRoadMap/")
|
||||
|
||||
from unittest import TestCase
|
||||
from PathPlanning.VisibilityRoadMap import visibility_road_map as m
|
||||
|
||||
print(__file__)
|
||||
|
||||
|
||||
class Test(TestCase):
|
||||
|
||||
def test1(self):
|
||||
m.show_animation = False
|
||||
m.main()
|
||||
Reference in New Issue
Block a user