github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

implement visibility_road_map planner

Browse Source
This commit is contained in:
Atsushi Sakai
2020-02-29 14:46:43 +09:00
parent cfd83841d7
commit ee423e8b94
8 changed files with 300 additions and 169 deletions
Download Patch File Download Diff File Expand all files Collapse all files

157
PathPlanning/Dijkstra/dijkstra.py
View File

@@ -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.resolution = resolution
self.robot_radius = robot_radius
self.calc_obstacle_map(ox, oy)
self.motion = self.get_motion_model()
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
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:
elif py < self.min_y:
return False
elif px >= self.maxx:
elif px >= self.max_x:
return False
elif py >= self.maxy:
elif 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("minx:", self.min_x)
print("miny:", self.min_y)
print("maxx:", self.max_x)
print("maxy:", 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("xwidth:", self.x_width)
print("ywidth:", 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
View File

44
PathPlanning/VisibilityRoadMap/geometry.py Normal file
View File

@@ -0,0 +1,44 @@
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
elif val < 0:
return 2
else:
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

135
PathPlanning/VisibilityGraphPlanner/visibility_graph_planner.py → PathPlanning/VisibilityRoadMap/visibility_road_map.py
View File

@@ -1,6 +1,6 @@
"""
Visibility Graph Planner
Visibility Road Map Planner
author: Atsushi Sakai (@Atsushi_twi)
@@ -8,35 +8,47 @@ author: Atsushi Sakai (@Atsushi_twi)
import os
import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../VoronoiRoadMap/")
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 VisibilityGraphPlanner:
class VisibilityRoadMap:
def __init__(self, robot_radius):
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.extract_graph_node(start_x, start_y, goal_x, goal_y,
obstacles)
graph = self.generate_graph(nodes, obstacles)
# rx, ry = DijkstraSearch().search(graph)
nodes = self.generate_graph_node(start_x, start_y, goal_x, goal_y,
obstacles)
rx, ry = [], []
road_map_info = self.generate_road_map_info(nodes, obstacles)
if show_animation:
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 extract_graph_node(self, start_x, start_y, goal_x, goal_y, obstacles):
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),
@@ -57,11 +69,11 @@ class VisibilityGraphPlanner:
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]
x_list = x_list[0:-1]
y_list = y_list[0:-1]
cvx_list, cvy_list = [], []
n_data=len(x_list)
n_data = len(x_list)
for index in range(n_data):
offset_x, offset_y = self.calc_offset_xy(
@@ -74,31 +86,39 @@ class VisibilityGraphPlanner:
return cvx_list, cvy_list
def generate_graph(self, nodes, obstacles):
def generate_road_map_info(self, nodes, obstacles):
graph = []
road_map_info_list = []
for target_node in nodes:
for 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):
print("bb")
is_valid = False
break
print(target_node, node)
print("aa")
plt.plot([target_node.x, node.x],[target_node.y, node.y], "-r")
if is_valid:
road_map_info.append(node_id)
return graph
road_map_info_list.append(road_map_info)
def is_edge_valid(self, target_node, node, obstacle):
return road_map_info_list
for i in range(len(obstacle.x_list)-1):
p1 = np.array([target_node.x, target_node.y])
p2 = np.array([node.x, node.y])
p3 = np.array([obstacle.x_list[i], obstacle.y_list[i]])
p4 = np.array([obstacle.y_list[i+1], obstacle.y_list[i+1]])
@staticmethod
def is_edge_valid(target_node, node, obstacle):
if is_seg_intersect(p1, p2, p3, p4):
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
@@ -108,25 +128,18 @@ class VisibilityGraphPlanner:
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
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")
def is_seg_intersect(a1, a2, b1, b2):
xdiff = [a1[0] - a2[0], b1[0] - b2[0]]
ydiff = [a1[1] - a2[1], b1[1] - b2[1]]
def det(a, b):
return a[0] * b[1] - a[1] * b[0]
div = det(xdiff, ydiff)
if div == 0:
return False
else:
return True
class ObstaclePolygon:
@@ -161,7 +174,7 @@ class ObstaclePolygon:
self.y_list.append(self.y_list[0])
def plot(self):
plt.plot(self.x_list, self.y_list, "-b")
plt.plot(self.x_list, self.y_list, "-k")
def main():
@@ -170,26 +183,36 @@ def main():
# 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(
[30.0, 45.0, 50.0, 40.0],
[50.0, 40.0, 20.0, 40.0],
)]
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, 70.0, 50.0],
)
]
if show_animation: # pragma: no cover
plt.plot(sx, sy, "or")
plt.plot(gx, gy, "ob")
[ob.plot() for ob in obstacles]
plt.pause(1.0)
rx, ry = VisibilityRoadMap(robot_radius, do_plot=show_animation).planning(
sx, sy, gx, gy, obstacles)
rx, ry = VisibilityGraphPlanner(robot_radius).planning(sx, sy, gx, gy,
obstacles)
# assert rx, 'Cannot found path'
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")
plt.pause(0.1)
plt.axis("equal")
plt.show()

93
PathPlanning/VoronoiRoadMap/dijkstra_search.py
View File

@@ -8,20 +8,21 @@ 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):
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(
@@ -30,71 +31,79 @@ class DijkstraSearch:
def __init__(self, show_animation):
self.show_animation = show_animation
def search(self, sx, sy, gx, gy, road_map, sample_x, sample_y):
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]
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
rr: robot radius[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[len(road_map) - 2] = start_node
open_set[self.find_id(node_x, node_y, start_node)] = start_node
while True:
if not open_set:
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
c_id = min(open_set, key=lambda o: open_set[o].cost)
current = open_set[c_id]
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.x, current.y, "xg")
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.001)
if c_id == (len(road_map) - 1):
print("goal is found!")
goal_node.parent = current.parent
goal_node.cost = current.cost
break
plt.pause(0.1)
# Remove the item from the open set
del open_set[c_id]
del open_set[current_id]
# Add it to the closed set
close_set[c_id] = current
close_set[current_id] = current_node
# 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
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(sample_x[n_id], sample_y[n_id],
current.cost + d, c_id)
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].cost = node.cost
open_set[n_id].parent = c_id
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:
@@ -102,5 +111,29 @@ class DijkstraSearch:
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 range(len(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

23
PathPlanning/VoronoiRoadMap/voronoi_road_map.py
View File

@@ -23,21 +23,22 @@ class VoronoiRoadMapPlanner:
self.N_KNN = 10 # number of edge from one sampled point
self.MAX_EDGE_LEN = 30.0 # [m] Maximum edge length
def planning(self, sx, sy, gx, gy, ox, oy, rr):
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")
road_map = self.generate_road_map(sample_x, sample_y, rr, obstacle_tree)
rx, ry = DijkstraSearch(show_animation).search(sx, sy, gx, gy, road_map,
sample_x, sample_y)
road_map_info = self.generate_road_map_info(
sample_x, sample_y, robot_radius, obstacle_tree)
rx, ry = DijkstraSearch(show_animation).search(sx, sy, gx, gy,
sample_x, sample_y,
road_map_info)
return rx, ry
def is_collision(self, sx, sy, gx, gy, rr, obstacle_kdtree):
def is_collision(self, sx, sy, gx, gy, rr, obstacle_kd_tree):
x = sx
y = sy
dx = gx - sx
@@ -52,25 +53,25 @@ class VoronoiRoadMapPlanner:
n_step = round(d / D)
for i in range(n_step):
ids, dist = obstacle_kdtree.search(np.array([x, y]).reshape(2, 1))
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)
# goal point check
ids, dist = obstacle_kdtree.search(np.array([gx, gy]).reshape(2, 1))
ids, dist = obstacle_kd_tree.search(np.array([gx, gy]).reshape(2, 1))
if dist[0] <= rr:
return True # collision
return False # OK
def generate_road_map(self, node_x, node_y, rr, obstacle_tree):
def generate_road_map_info(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
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
"""