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

fixed the bug and code clean up.

Browse Source
This commit is contained in:
Atsushi Sakai
2020-02-22 23:17:25 +09:00
parent 41d744c67f
commit 7c8a940ebf
2 changed files with 36 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
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
@@ -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)
@@ -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))

48
PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py
View File

@@ -30,7 +30,8 @@ class SweepSearcher:
RIGHT = 1
LEFT = -1
def __init__(self, moving_direction, sweep_direction, x_inds_goal_y, goal_y):
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 = []
@@ -88,6 +89,8 @@ class SweepSearcher:
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),
@@ -189,35 +192,34 @@ 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,
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,
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
@@ -229,26 +231,26 @@ def sweep_path_search(sweep_searcher, gmap, grid_search_animation=False):
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 (
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
@@ -307,11 +309,6 @@ def planning_animation(ox, oy, reso): # pragma: no cover
def main(): # pragma: no cover
print("start!!")
ox = [0.0, 50.0, 50.0, 0.0, 0.0]
oy = [0.0, 0.0, 50.0, 50.0, 0.0]
reso = 0.4
planning_animation(ox, oy, reso)
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
@@ -328,7 +325,6 @@ def main(): # pragma: no cover
planning_animation(ox, oy, reso)
plt.show()
print("done!!")