github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-14 01:28:23 -05:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #295 from AtsushiSakai/issue_293

Browse Source 
fix #293
This commit is contained in:
Atsushi Sakai
2020-02-22 23:36:11 +09:00
committed by GitHub
parent 06438d46c5 7c8a940ebf
commit e8ffc01cc7
2 changed files with 81 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
Mapping/grid_map_lib/grid_map_lib.py
View File

@@ -32,10 +32,8 @@ class GridMap:
self.center_x = center_x
self.center_y = center_y
self.left_lower_x = self.center_x - \
(self.width / 2.0) * self.resolution
self.left_lower_y = self.center_y - \
(self.height / 2.0) * self.resolution
self.left_lower_x = self.center_x - self.width / 2.0 * self.resolution
self.left_lower_y = self.center_y - self.height / 2.0 * self.resolution
self.ndata = self.width * self.height
self.data = [init_val] * self.ndata
@@ -51,7 +49,7 @@ class GridMap:
grid_ind = self.calc_grid_index_from_xy_index(x_ind, y_ind)
if 0 <= grid_ind <= self.ndata:
if 0 <= grid_ind < self.ndata:
return self.data[grid_ind]
else:
return None
@@ -99,7 +97,6 @@ class GridMap:
"""
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)
@@ -163,7 +160,7 @@ class GridMap:
val = self.get_value_from_xy_index(xind, yind)
if val >= occupied_val:
if val is None or val >= occupied_val:
return True
else:
return False
@@ -200,12 +197,22 @@ class GridMap:
if not min_x < iox < max_x:
continue
if (y[i1] + (y[i2] - y[i1]) / (x[i2] - x[i1])
* (iox - x[i1]) - ioy) > 0.0:
tmp1 = (y[i2] - y[i1]) / (x[i2] - x[i1])
if (y[i1] + tmp1 * (iox - x[i1]) - ioy) > 0.0:
inside = not inside
return inside
def print_grid_map_info(self):
print("width:", self.width)
print("height:", self.height)
print("resolution:", self.resolution)
print("center_x:", self.center_x)
print("center_y:", self.center_y)
print("left_lower_x:", self.left_lower_x)
print("left_lower_y:", self.left_lower_y)
print("ndata:", self.ndata)
def plot_grid_map(self, ax=None):
grid_data = np.reshape(np.array(self.data), (self.height, self.width))

112
PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py
View File

@@ -30,22 +30,24 @@ class SweepSearcher:
RIGHT = 1
LEFT = -1
def __init__(self, mdirection, sdirection, xinds_goaly, goaly):
self.moving_direction = mdirection
self.sweep_direction = sdirection
def __init__(self,
moving_direction, sweep_direction, x_inds_goal_y, goal_y):
self.moving_direction = moving_direction
self.sweep_direction = sweep_direction
self.turing_window = []
self.update_turning_window()
self.xinds_goaly = xinds_goaly
self.goaly = goaly
self.xinds_goaly = x_inds_goal_y
self.goaly = goal_y
def move_target_grid(self, cxind, cyind, gmap):
nxind = self.moving_direction + cxind
nyind = cyind
# found safe grid
if not gmap.check_occupied_from_xy_index(nxind, nyind, occupied_val=0.5):
if not gmap.check_occupied_from_xy_index(nxind, nyind,
occupied_val=0.5):
return nxind, nyind
else: # occupided
else: # occupied
ncxind, ncyind = self.find_safe_turning_grid(cxind, cyind, gmap)
if (ncxind is None) and (ncyind is None):
# moving backward
@@ -56,33 +58,39 @@ class SweepSearcher:
return None, None
else:
# keep moving until end
while not gmap.check_occupied_from_xy_index(ncxind + self.moving_direction, ncyind, occupied_val=0.5):
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
def find_safe_turning_grid(self, cxind, cyind, gmap):
for (dxind, dyind) in self.turing_window:
for (d_x_ind, d_y_ind) in self.turing_window:
nxind = dxind + cxind
nyind = dyind + cyind
next_x_ind = d_x_ind + cxind
next_y_ind = d_y_ind + cyind
# found safe grid
if not gmap.check_occupied_from_xy_index(nxind, nyind, occupied_val=0.5):
return nxind, nyind
if not gmap.check_occupied_from_xy_index(next_x_ind, next_y_ind,
occupied_val=0.5):
return next_x_ind, next_y_ind
return None, None
def is_search_done(self, gmap):
def is_search_done(self, grid_map):
for ix in self.xinds_goaly:
if not gmap.check_occupied_from_xy_index(ix, self.goaly, occupied_val=0.5):
if not grid_map.check_occupied_from_xy_index(ix, self.goaly,
occupied_val=0.5):
return False
# all lower grid is occupied
return True
def update_turning_window(self):
# turning window definition
# robot can move grid based on it.
self.turing_window = [
(self.moving_direction, 0.0),
(self.moving_direction, self.sweep_direction),
@@ -95,22 +103,24 @@ class SweepSearcher:
self.update_turning_window()
def search_start_grid(self, grid_map):
xinds = []
x_inds = []
y_ind = 0
if self.sweep_direction == self.SweepDirection.DOWN:
xinds, y_ind = search_free_grid_index_at_edge_y(grid_map, from_upper=True)
x_inds, y_ind = search_free_grid_index_at_edge_y(
grid_map, from_upper=True)
elif self.sweep_direction == self.SweepDirection.UP:
xinds, y_ind = search_free_grid_index_at_edge_y(grid_map, from_upper=False)
x_inds, y_ind = search_free_grid_index_at_edge_y(
grid_map, from_upper=False)
if self.moving_direction == self.MovingDirection.RIGHT:
return min(xinds), y_ind
return min(x_inds), y_ind
elif self.moving_direction == self.MovingDirection.LEFT:
return max(xinds), y_ind
return max(x_inds), y_ind
raise ValueError("self.moving direction is invalid ")
def find_sweep_direction_and_start_posi(ox, oy):
def find_sweep_direction_and_start_position(ox, oy):
# find sweep_direction
max_dist = 0.0
vec = [0.0, 0.0]
@@ -182,61 +192,65 @@ def search_free_grid_index_at_edge_y(grid_map, from_upper=False):
def setup_grid_map(ox, oy, reso, sweep_direction, offset_grid=10):
width = math.ceil((max(ox) - min(ox)) / reso) + offset_grid
height = math.ceil((max(oy) - min(oy)) / reso) + offset_grid
center_x = np.mean(ox)
center_y = np.mean(oy)
center_x = (np.max(ox)+np.min(ox))/2.0
center_y = (np.max(oy)+np.min(oy))/2.0
grid_map = GridMap(width, height, reso, center_x, center_y)
grid_map.print_grid_map_info()
grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
grid_map.expand_grid()
xinds_goaly = []
goaly = 0
x_inds_goal_y = []
goal_y = 0
if sweep_direction == SweepSearcher.SweepDirection.UP:
xinds_goaly, goaly = search_free_grid_index_at_edge_y(grid_map, from_upper=True)
x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y(grid_map,
from_upper=True)
elif sweep_direction == SweepSearcher.SweepDirection.DOWN:
xinds_goaly, goaly = search_free_grid_index_at_edge_y(grid_map, from_upper=False)
x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y(grid_map,
from_upper=False)
return grid_map, xinds_goaly, goaly
return grid_map, x_inds_goal_y, goal_y
def sweep_path_search(sweep_searcher, gmap, grid_search_animation=False):
def sweep_path_search(sweep_searcher, grid_map, grid_search_animation=False):
# search start grid
cxind, cyind = sweep_searcher.search_start_grid(gmap)
if not gmap.set_value_from_xy_index(cxind, cyind, 0.5):
cxind, cyind = sweep_searcher.search_start_grid(grid_map)
if not grid_map.set_value_from_xy_index(cxind, cyind, 0.5):
print("Cannot find start grid")
return [], []
x, y = gmap.calc_grid_central_xy_position_from_xy_index(cxind, cyind)
x, y = grid_map.calc_grid_central_xy_position_from_xy_index(cxind, cyind)
px, py = [x], [y]
fig, ax = None, None
if grid_search_animation:
fig, ax = plt.subplots()
# for stopping simulation with the esc key.
fig.canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
lambda event: [
exit(0) if event.key == 'escape' else None])
while True:
cxind, cyind = sweep_searcher.move_target_grid(cxind, cyind, gmap)
cxind, cyind = sweep_searcher.move_target_grid(cxind, cyind, grid_map)
if sweep_searcher.is_search_done(gmap) or (cxind is None or cyind is None):
if sweep_searcher.is_search_done(grid_map) or (
cxind is None or cyind is None):
print("Done")
break
x, y = gmap.calc_grid_central_xy_position_from_xy_index(
x, y = grid_map.calc_grid_central_xy_position_from_xy_index(
cxind, cyind)
px.append(x)
py.append(y)
gmap.set_value_from_xy_index(cxind, cyind, 0.5)
grid_map.set_value_from_xy_index(cxind, cyind, 0.5)
if grid_search_animation:
gmap.plot_grid_map(ax=ax)
grid_map.plot_grid_map(ax=ax)
plt.pause(1.0)
gmap.plot_grid_map()
grid_map.plot_grid_map()
return px, py
@@ -245,13 +259,16 @@ def planning(ox, oy, reso,
moving_direction=SweepSearcher.MovingDirection.RIGHT,
sweeping_direction=SweepSearcher.SweepDirection.UP,
):
sweep_vec, sweep_start_posi = find_sweep_direction_and_start_posi(ox, oy)
sweep_vec, sweep_start_posi = find_sweep_direction_and_start_position(
ox, oy)
rox, roy = convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_posi)
gmap, xinds_goaly, goaly = setup_grid_map(rox, roy, reso, sweeping_direction)
gmap, xinds_goaly, goaly = setup_grid_map(rox, roy, reso,
sweeping_direction)
sweep_searcher = SweepSearcher(moving_direction, sweeping_direction, xinds_goaly, goaly)
sweep_searcher = SweepSearcher(moving_direction, sweeping_direction,
xinds_goaly, goaly)
px, py = sweep_path_search(sweep_searcher, gmap)
@@ -270,8 +287,9 @@ def planning_animation(ox, oy, reso): # pragma: no cover
for ipx, ipy in zip(px, py):
plt.cla()
# 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.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(ox, oy, "-xb")
plt.plot(px, py, "-r")
plt.plot(ipx, ipy, "or")
@@ -285,6 +303,7 @@ def planning_animation(ox, oy, reso): # pragma: no cover
plt.axis("equal")
plt.grid(True)
plt.pause(0.1)
plt.close()
def main(): # pragma: no cover
@@ -306,7 +325,6 @@ def main(): # pragma: no cover
planning_animation(ox, oy, reso)
plt.show()
print("done!!")