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

first release grid based_sweep_planner.py

Browse Source
This commit is contained in:
Atsushi Sakai
2019-07-06 10:32:00 +09:00
parent 474ffef84b
commit b06e42fe41
3 changed files with 91 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
Mapping/grid_map_lib/grid_map_lib.py
View File

@@ -98,9 +98,13 @@ class GridMap:
:param val: grid value
"""
if (x_ind is None) or (y_ind is None):
print(x_ind, y_ind)
return False, False
grid_ind = int(y_ind * self.width + x_ind)
if 0 <= grid_ind <= self.ndata:
if 0 <= grid_ind < self.ndata:
self.data[grid_ind] = val
return True # OK
else:
@@ -164,6 +168,23 @@ class GridMap:
else:
return False
def expand_grid(self):
xinds, yinds = [], []
for ix in range(self.width):
for iy in range(self.height):
if self.check_occupied_from_xy_index(ix, iy):
xinds.append(ix)
yinds.append(iy)
for (ix, iy) in zip(xinds, yinds):
self.set_value_from_xy_index(ix + 1, iy, val=1.0)
self.set_value_from_xy_index(ix, iy + 1, val=1.0)
self.set_value_from_xy_index(ix + 1, iy + 1, val=1.0)
self.set_value_from_xy_index(ix - 1, iy, val=1.0)
self.set_value_from_xy_index(ix, iy - 1, val=1.0)
self.set_value_from_xy_index(ix - 1, iy - 1, val=1.0)
@staticmethod
def check_inside_polygon(iox, ioy, x, y):

17
PathPlanning/AStar/a_star.py
View File

@@ -9,9 +9,10 @@ See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
"""
import matplotlib.pyplot as plt
import math
import matplotlib.pyplot as plt
show_animation = True
@@ -66,6 +67,10 @@ class AStarPlanner:
open_set[self.calc_grid_index(nstart)] = nstart
while 1:
if len(open_set) == 0:
print("Open set is empty..")
break
c_id = min(
open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal, open_set[o]))
current = open_set[c_id]
@@ -89,25 +94,25 @@ class AStarPlanner:
# Add it to the closed set
closed_set[c_id] = current
# expand search grid based on motion model
# expand_grid search grid based on motion model
for i, _ in enumerate(self.motion):
node = self.Node(current.x + self.motion[i][0],
current.y + self.motion[i][1],
current.cost + self.motion[i][2], c_id)
n_id = self.calc_grid_index(node)
# If the node is in closed_set, do nothing
if n_id in closed_set:
continue
# If the node is not safe, do nothing
if not self.verify_node(node):
continue
if n_id in closed_set:
continue
if n_id not in open_set:
open_set[n_id] = node # discovered a new node
else:
if open_set[n_id].cost >= node.cost:
if open_set[n_id].cost > node.cost:
# This path is the best until now. record it
open_set[n_id] = node

121
PathPlanning/GridBasedSweepCCP/grid_based_sweep_planner.py
View File

@@ -12,7 +12,12 @@ import matplotlib.pyplot as plt
import numpy as np
sys.path.append(os.path.relpath("../../Mapping/grid_map_lib/"))
from grid_map_lib import GridMap
try:
from grid_map_lib import GridMap
except ImportError:
exit()
do_animation = True
class SweepSearcher:
@@ -35,7 +40,7 @@ class SweepSearcher:
return nxind, nyind
else: # occupided
ncxind, ncyind = self.find_safe_turning_grid(cxind, cyind, gmap)
if not ncxind and not ncyind:
if (ncxind is None) and (ncyind is None):
# moving backward
ncxind = -self.moving_direction + cxind
ncyind = cyind
@@ -43,6 +48,8 @@ class SweepSearcher:
# moved backward, but the grid is occupied by obstacle
return None, None
else:
while not gmap.check_occupied_from_xy_index(ncxind + self.moving_direction, ncyind, occupied_val=0.5):
ncxind += self.moving_direction
self.swap_moving_direction()
return ncxind, ncyind
@@ -55,7 +62,6 @@ class SweepSearcher:
# found safe grid
if not gmap.check_occupied_from_xy_index(nxind, nyind, occupied_val=0.5):
print(dxind, dyind)
return nxind, nyind
return None, None
@@ -81,34 +87,22 @@ class SweepSearcher:
self.update_turning_window()
def find_sweep_direction_and_start_posi(ox, oy, start_posi):
def find_sweep_direction_and_start_posi(ox, oy):
# find sweep_direction
maxd_id = None
maxd = 0.0
vec = [0.0, 0.0]
sweep_start_pos = [0.0, 0.0]
for i in range(len(ox) - 1):
dx = ox[i + 1] - ox[i]
dy = oy[i + 1] - oy[i]
d = np.sqrt(dx ** 2 + dy ** 2)
if d > maxd:
maxd_id = i
maxd = d
vec = [dx, dy]
sweep_start_pos = [ox[i], oy[i]]
# find sweep start posi
d1 = np.sqrt((ox[maxd_id] - start_posi[0]) ** 2 +
(oy[maxd_id] - start_posi[1]) ** 2)
d2 = np.sqrt((ox[maxd_id + 1] - start_posi[0]) ** 2 +
(oy[maxd_id + 1] - start_posi[1]) ** 2)
if d2 >= d1: # first point is near
sweep_start_posi = [ox[maxd_id], oy[maxd_id]]
else:
sweep_start_posi = [ox[maxd_id + 1], oy[maxd_id + 1]]
vec = [-1.0 * iv for iv in vec] # reverse direction
return vec, sweep_start_posi
return vec, sweep_start_pos
def convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_posi):
@@ -140,19 +134,26 @@ def convert_global_coordinate(x, y, sweep_vec, sweep_start_posi):
return rx, ry
def search_free_lower_y_grid_index(grid_map):
miny = None
def search_free_grid_index_at_edge_y(grid_map, from_upper=False):
yind = None
xinds = []
for iy in range(grid_map.height):
for ix in range(grid_map.width):
if from_upper:
xrange = range(grid_map.height)[::-1]
yrange = range(grid_map.width)[::-1]
else:
xrange = range(grid_map.height)
yrange = range(grid_map.width)
for iy in xrange:
for ix in yrange:
if not grid_map.check_occupied_from_xy_index(ix, iy):
miny = iy
yind = iy
xinds.append(ix)
if miny:
if yind:
break
return xinds, miny
return xinds, yind
def setup_grid_map(ox, oy, reso):
@@ -167,47 +168,33 @@ def setup_grid_map(ox, oy, reso):
grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
# fill grid
# for i in range(len(ox) - 1):
# grid_map.set_value_from_xy_pos(ox[i], oy[i], 1.0)
#
# x, y = ox[i], oy[i]
# th = math.atan2(oy[i + 1] - oy[i], ox[i + 1] - ox[i])
# d = np.sqrt((x - ox[i + 1])**2 + (y - oy[i + 1])**2)
#
# while d > reso:
# x += np.cos(th) * reso
# y += np.sin(th) * reso
# d = np.sqrt((x - ox[i + 1])**2 + (y - oy[i + 1])**2)
#
# grid_map.set_value_from_xy_pos(x, y, 1.0)
grid_map.expand_grid()
xinds, miny = search_free_lower_y_grid_index(grid_map)
# grid_map.plot_gridmap()
xinds, miny = search_free_grid_index_at_edge_y(grid_map)
return grid_map, xinds, miny
def sweep_path_search(sweep_searcher, gmap, start_posi):
sx, sy = start_posi[0], start_posi[1]
# print(sx, sy)
def search_start_grid(grid_map):
xinds, y_ind = search_free_grid_index_at_edge_y(grid_map, from_upper=True)
# search start grid
cxind, cyind = gmap.get_xy_index_from_xy_pos(sx, sy)
if gmap.check_occupied_from_xy_index(cxind, cyind):
cxind, cyind = sweep_searcher.find_safe_turning_grid(cxind, cyind, gmap)
gmap.set_value_from_xy_index(cxind, cyind, 0.5)
return min(xinds), y_ind
def sweep_path_search(sweep_searcher, gmap):
px, py = [], []
# search start grid
cxind, cyind = search_start_grid(gmap)
if not gmap.set_value_from_xy_index(cxind, cyind, 0.5):
print("Cannot find start grid")
return px, py
# fig, ax = plt.subplots()
while True:
cxind, cyind = sweep_searcher.move_target_grid(cxind, cyind, gmap)
if sweep_searcher.is_search_done(gmap) or (not cxind and not cyind):
if sweep_searcher.is_search_done(gmap) or (cxind is None or cyind is None):
print("Done")
break
@@ -220,16 +207,15 @@ def sweep_path_search(sweep_searcher, gmap, start_posi):
gmap.set_value_from_xy_index(cxind, cyind, 0.5)
# gmap.plot_grid_map(ax=ax)
# plt.pause(0.1)
# plt.pause(1.0)
gmap.plot_grid_map()
return px, py
def planning(ox, oy, reso, start_posi):
sweep_vec, sweep_start_posi = find_sweep_direction_and_start_posi(
ox, oy, start_posi)
def planning(ox, oy, reso):
sweep_vec, sweep_start_posi = find_sweep_direction_and_start_posi(ox, oy)
rox, roy = convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_posi)
@@ -240,7 +226,7 @@ def planning(ox, oy, reso, start_posi):
sweep_searcher = SweepSearcher(moving_direction, sweeping_direction, xinds_miny, miny)
px, py = sweep_path_search(sweep_searcher, gmap, start_posi)
px, py = sweep_path_search(sweep_searcher, gmap)
rx, ry = convert_global_coordinate(px, py, sweep_vec, sweep_start_posi)
@@ -250,17 +236,26 @@ def planning(ox, oy, reso, start_posi):
def main():
print("start!!")
start_posi = [0.0, 0.0]
ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0, 0.0]
oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0, 0.0]
reso = 5.0
px, py = planning(ox, oy, reso, start_posi)
px, py = planning(ox, oy, reso)
plt.subplots()
plt.plot(start_posi[0], start_posi[1], "or")
# animation
if do_animation:
for ipx, ipy in zip(px, py):
plt.cla()
plt.plot(ox, oy, "-xb")
plt.plot(px, py, "-r")
plt.plot(ipx, ipy, "or")
plt.axis("equal")
plt.grid(True)
plt.pause(0.1)
plt.cla()
plt.plot(ox, oy, "-xb")
plt.plot(px, py, "-r")
plt.axis("equal")