mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
release grid based_sweep_coverage_path_planner.py
This commit is contained in:
Atsushi Sakai
@@ -7,6 +7,7 @@ author: Atsushi Sakai
|
||||
import math
|
||||
import os
|
||||
import sys
|
||||
from enum import IntEnum
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
@@ -21,14 +22,21 @@ do_animation = True
|
||||
|
||||
|
||||
class SweepSearcher:
|
||||
class SweepDirection(IntEnum):
|
||||
UP = 1
|
||||
DOWN = -1
|
||||
|
||||
def __init__(self, mdirection, sdirection, xinds_miny, miny):
|
||||
self.moving_direction = mdirection # +1 or -1
|
||||
self.sweep_direction = sdirection # +1 or -1
|
||||
class MovingDirection(IntEnum):
|
||||
RIGHT = 1
|
||||
LEFT = -1
|
||||
|
||||
def __init__(self, mdirection, sdirection, xinds_goaly, goaly):
|
||||
self.moving_direction = mdirection
|
||||
self.sweep_direction = sdirection
|
||||
self.turing_window = []
|
||||
self.update_turning_window()
|
||||
self.xinds_miny = xinds_miny
|
||||
self.miny = miny
|
||||
self.xinds_goaly = xinds_goaly
|
||||
self.goaly = goaly
|
||||
|
||||
def move_target_grid(self, cxind, cyind, gmap):
|
||||
|
||||
@@ -48,6 +56,7 @@ class SweepSearcher:
|
||||
# moved backward, but the grid is occupied by obstacle
|
||||
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):
|
||||
ncxind += self.moving_direction
|
||||
self.swap_moving_direction()
|
||||
@@ -67,8 +76,8 @@ class SweepSearcher:
|
||||
return None, None
|
||||
|
||||
def is_search_done(self, gmap):
|
||||
for ix in self.xinds_miny:
|
||||
if not gmap.check_occupied_from_xy_index(ix, self.miny, occupied_val=0.5):
|
||||
for ix in self.xinds_goaly:
|
||||
if not gmap.check_occupied_from_xy_index(ix, self.goaly, occupied_val=0.5):
|
||||
return False
|
||||
|
||||
# all lower grid is occupied
|
||||
@@ -86,10 +95,24 @@ class SweepSearcher:
|
||||
self.moving_direction *= -1
|
||||
self.update_turning_window()
|
||||
|
||||
def search_start_grid(self, grid_map):
|
||||
xinds = [], 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)
|
||||
elif self.sweep_direction == self.SweepDirection.UP:
|
||||
xinds, 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
|
||||
elif self.moving_direction == self.MovingDirection.LEFT:
|
||||
return max(xinds), y_ind
|
||||
|
||||
raise ValueError("self.moving direction is invalid ")
|
||||
|
||||
|
||||
def find_sweep_direction_and_start_posi(ox, oy):
|
||||
# find sweep_direction
|
||||
maxd = 0.0
|
||||
max_dist = 0.0
|
||||
vec = [0.0, 0.0]
|
||||
sweep_start_pos = [0.0, 0.0]
|
||||
for i in range(len(ox) - 1):
|
||||
@@ -97,8 +120,8 @@ def find_sweep_direction_and_start_posi(ox, oy):
|
||||
dy = oy[i + 1] - oy[i]
|
||||
d = np.sqrt(dx ** 2 + dy ** 2)
|
||||
|
||||
if d > maxd:
|
||||
maxd = d
|
||||
if d > max_dist:
|
||||
max_dist = d
|
||||
vec = [dx, dy]
|
||||
sweep_start_pos = [ox[i], oy[i]]
|
||||
|
||||
@@ -156,9 +179,7 @@ def search_free_grid_index_at_edge_y(grid_map, from_upper=False):
|
||||
return xinds, yind
|
||||
|
||||
|
||||
def setup_grid_map(ox, oy, reso):
|
||||
offset_grid = 10
|
||||
|
||||
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)
|
||||
@@ -170,27 +191,28 @@ def setup_grid_map(ox, oy, reso):
|
||||
|
||||
grid_map.expand_grid()
|
||||
|
||||
xinds, miny = search_free_grid_index_at_edge_y(grid_map)
|
||||
xinds_goaly = [], goaly = 0
|
||||
if sweep_direction == SweepSearcher.SweepDirection.UP:
|
||||
xinds_goaly, goaly = 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)
|
||||
|
||||
return grid_map, xinds, miny
|
||||
return grid_map, xinds_goaly, goaly
|
||||
|
||||
|
||||
def search_start_grid(grid_map):
|
||||
xinds, y_ind = search_free_grid_index_at_edge_y(grid_map, from_upper=True)
|
||||
|
||||
return min(xinds), y_ind
|
||||
|
||||
|
||||
def sweep_path_search(sweep_searcher, gmap):
|
||||
px, py = [], []
|
||||
|
||||
def sweep_path_search(sweep_searcher, gmap, grid_search_animation=False):
|
||||
# search start grid
|
||||
cxind, cyind = search_start_grid(gmap)
|
||||
cxind, cyind = sweep_searcher.search_start_grid(gmap)
|
||||
if not gmap.set_value_from_xy_index(cxind, cyind, 0.5):
|
||||
print("Cannot find start grid")
|
||||
return px, py
|
||||
return [], []
|
||||
|
||||
x, y = gmap.calc_grid_central_xy_position_from_xy_index(cxind, cyind)
|
||||
px, py = [x], [y]
|
||||
|
||||
if grid_search_animation:
|
||||
fig, ax = plt.subplots()
|
||||
|
||||
# fig, ax = plt.subplots()
|
||||
while True:
|
||||
cxind, cyind = sweep_searcher.move_target_grid(cxind, cyind, gmap)
|
||||
|
||||
@@ -206,44 +228,39 @@ def sweep_path_search(sweep_searcher, gmap):
|
||||
|
||||
gmap.set_value_from_xy_index(cxind, cyind, 0.5)
|
||||
|
||||
# gmap.plot_grid_map(ax=ax)
|
||||
# plt.pause(1.0)
|
||||
if grid_search_animation:
|
||||
gmap.plot_grid_map(ax=ax)
|
||||
plt.pause(1.0)
|
||||
|
||||
gmap.plot_grid_map()
|
||||
|
||||
return px, py
|
||||
|
||||
|
||||
def planning(ox, oy, reso):
|
||||
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)
|
||||
|
||||
rox, roy = convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_posi)
|
||||
|
||||
moving_direction = 1
|
||||
sweeping_direction = -1
|
||||
gmap, xinds_goaly, goaly = setup_grid_map(rox, roy, reso, sweeping_direction)
|
||||
|
||||
gmap, xinds_miny, miny = setup_grid_map(rox, roy, reso)
|
||||
|
||||
sweep_searcher = SweepSearcher(moving_direction, sweeping_direction, xinds_miny, miny)
|
||||
sweep_searcher = SweepSearcher(moving_direction, sweeping_direction, xinds_goaly, goaly)
|
||||
|
||||
px, py = sweep_path_search(sweep_searcher, gmap)
|
||||
|
||||
rx, ry = convert_global_coordinate(px, py, sweep_vec, sweep_start_posi)
|
||||
|
||||
print("Path length:", len(rx))
|
||||
|
||||
return rx, ry
|
||||
|
||||
|
||||
def main():
|
||||
print("start!!")
|
||||
|
||||
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
|
||||
|
||||
def planning_animation(ox, oy, reso):
|
||||
px, py = planning(ox, oy, reso)
|
||||
|
||||
plt.subplots()
|
||||
|
||||
# animation
|
||||
if do_animation:
|
||||
for ipx, ipy in zip(px, py):
|
||||
@@ -260,6 +277,27 @@ def main():
|
||||
plt.plot(px, py, "-r")
|
||||
plt.axis("equal")
|
||||
plt.grid(True)
|
||||
plt.pause(0.1)
|
||||
|
||||
|
||||
def main():
|
||||
print("start!!")
|
||||
|
||||
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
|
||||
planning_animation(ox, oy, reso)
|
||||
|
||||
ox = [0.0, 50.0, 50.0, 0.0, 0.0]
|
||||
oy = [0.0, 0.0, 30.0, 30.0, 0.0]
|
||||
reso = 1.3
|
||||
planning_animation(ox, oy, reso)
|
||||
|
||||
ox = [0.0, 20.0, 50.0, 200.0, 130.0, 40.0, 0.0]
|
||||
oy = [0.0, -80.0, 0.0, 30.0, 60.0, 80.0, 0.0]
|
||||
reso = 5.0
|
||||
planning_animation(ox, oy, reso)
|
||||
|
||||
plt.show()
|
||||
|
||||
print("done!!")
|
||||
@@ -0,0 +1,92 @@
|
||||
from unittest import TestCase
|
||||
|
||||
import grid_based_sweep_coverage_path_planner
|
||||
|
||||
|
||||
class TestPlanning(TestCase):
|
||||
|
||||
def test_planning1(self):
|
||||
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 = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
|
||||
moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
|
||||
sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
|
||||
)
|
||||
self.assertTrue(len(px) >= 5)
|
||||
|
||||
px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
|
||||
moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.LEFT,
|
||||
sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
|
||||
)
|
||||
self.assertTrue(len(px) >= 5)
|
||||
|
||||
px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
|
||||
moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
|
||||
sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.UP,
|
||||
)
|
||||
self.assertTrue(len(px) >= 5)
|
||||
|
||||
px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
|
||||
moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
|
||||
sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
|
||||
)
|
||||
self.assertTrue(len(px) >= 5)
|
||||
|
||||
def test_planning2(self):
|
||||
ox = [0.0, 50.0, 50.0, 0.0, 0.0]
|
||||
oy = [0.0, 0.0, 30.0, 30.0, 0.0]
|
||||
reso = 1.3
|
||||
|
||||
px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
|
||||
moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
|
||||
sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
|
||||
)
|
||||
self.assertTrue(len(px) >= 5)
|
||||
|
||||
px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
|
||||
moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.LEFT,
|
||||
sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
|
||||
)
|
||||
self.assertTrue(len(px) >= 5)
|
||||
|
||||
px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
|
||||
moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
|
||||
sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.UP,
|
||||
)
|
||||
self.assertTrue(len(px) >= 5)
|
||||
|
||||
px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
|
||||
moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
|
||||
sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
|
||||
)
|
||||
self.assertTrue(len(px) >= 5)
|
||||
|
||||
def test_planning3(self):
|
||||
ox = [0.0, 20.0, 50.0, 200.0, 130.0, 40.0, 0.0]
|
||||
oy = [0.0, -80.0, 0.0, 30.0, 60.0, 80.0, 0.0]
|
||||
reso = 5.1
|
||||
px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
|
||||
moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
|
||||
sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
|
||||
)
|
||||
self.assertTrue(len(px) >= 5)
|
||||
|
||||
px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
|
||||
moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.LEFT,
|
||||
sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
|
||||
)
|
||||
self.assertTrue(len(px) >= 5)
|
||||
|
||||
px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
|
||||
moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
|
||||
sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.UP,
|
||||
)
|
||||
self.assertTrue(len(px) >= 5)
|
||||
|
||||
px, py = grid_based_sweep_coverage_path_planner.planning(ox, oy, reso,
|
||||
moving_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.MovingDirection.RIGHT,
|
||||
sweeping_direction=grid_based_sweep_coverage_path_planner.SweepSearcher.SweepDirection.DOWN,
|
||||
)
|
||||
self.assertTrue(len(px) >= 5)
|
||||
Reference in New Issue
Block a user