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 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
