def setup_grid_map(ox, oy, reso, sweep_direction, offset_grid=5):
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.average(ox[0:len(ox)-1])
# center_y = np.average(oy[0:len(oy)-1])
# print("oxs ", ox[0:len(ox)-1])
# print("oys ", oy[0:len(ox)-1])
print("width, height, center x, center y:", width, height, center_x,
center_y)
#polygon_boundary mk
grid_map = GridMap(width, height, reso, center_x, center_y)
#set zero inside polygon/ one outside polygon in gridmap
grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
#This expand_grid function makes offset from the polygon boundaries
#grid_map.expand_grid()
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)
print("sweep up - xinds_goaly, goaly:", xinds_goaly, goaly)
elif sweep_direction == SweepSearcher.SweepDirection.DOWN:
xinds_goaly, goaly = search_free_grid_index_at_edge_y(grid_map,
from_upper=False)
print("sweep down - xinds_goaly, goaly:", xinds_goaly, goaly)
return grid_map, xinds_goaly, goaly
def calc_obstacle_map(self, ox, oy, ox_in, oy_in):
self.minx = round(min(ox))
self.miny = round(min(oy))
self.maxx = round(max(ox))
self.maxy = round(max(oy))
# self.xwidth = round((self.maxx - self.minx) / self.reso)
# self.ywidth = round((self.maxy - self.miny) / self.reso)
self.xwidth = math.ceil(
(max(ox) - min(ox)) / self.reso) # 创建一个栅格地图，可包括整个区域
self.ywidth = math.ceil((max(oy) - min(oy)) / self.reso)
center_x = np.mean(ox)
center_y = np.mean(oy)
grid_map = GridMap(self.xwidth, self.ywidth, self.reso, center_x,
center_y) #创建
grid_map.set_value_from_polygon(ox, oy, 1.0,
inside=False) #将多边形外均设为1，内部默认为0
for i in range(len(ox_in)):
grid_map.set_value_from_polygon(ox_in[i],
oy_in[i],
1.0,
inside=True) #将多边形内均设为1，外部默认为0
grid_map.expand_grid() # 膨胀一个栅格大小
# obstacle map generation
self.obmap = grid_map
def test_polygon_set(self):
ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0]
oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0]
grid_map = GridMap(600, 290, 0.7, 60.0, 30.5)
grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
self.assertEqual(True, True)
def setup_grid_map(ox, oy, ox_in, oy_in, reso, 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)
grid_map = GridMap(width, height, reso, center_x, center_y) #创建
grid_map.set_value_from_polygon(ox, oy, 1.0,
inside=False) #将多边形外均设为1，内部默认为0
for i in range(len(ox_in)):
grid_map.set_value_from_polygon(ox_in[i], oy_in[i], 1.0,
inside=True) #将多边形内均设为1，外部默认为0
grid_map.expand_grid() # 膨胀一个栅格大小
return grid_map
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)
grid_map = GridMap(width, height, reso, center_x, center_y)
grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
grid_map.expand_grid()
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_goaly, goaly