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 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 test_position_set(self):
grid_map = GridMap(100, 120, 0.5, 10.0, -0.5)
grid_map.set_value_from_xy_pos(10.1, -1.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, -0.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, 1.1, 1.0)
grid_map.set_value_from_xy_pos(11.1, 0.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, 0.1, 1.0)
grid_map.set_value_from_xy_pos(9.1, 0.1, 1.0)
self.assertEqual(True, True)
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, resolution, sweep_direction, offset_grid=10):
width = math.ceil((max(ox) - min(ox)) / resolution) + offset_grid
height = math.ceil((max(oy) - min(oy)) / resolution) + offset_grid
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, resolution, 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()
x_inds_goal_y = []
goal_y = 0
if sweep_direction == SweepSearcher.SweepDirection.UP:
x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y(
grid_map, from_upper=True)
elif sweep_direction == SweepSearcher.SweepDirection.DOWN:
x_inds_goal_y, goal_y = search_free_grid_index_at_edge_y(
grid_map, from_upper=False)
return grid_map, x_inds_goal_y, goal_y
def setup_grid_map(ox, oy, reso, sweep_direction, offset_grid=20):
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, 2.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
def __initGridMap(self, polyon, resolution, offset_grid=10):
# 计算栅格的中心点
polyon_x, polyon_y = Tools.departurePoints(polyon)
center_px = np.mean(polyon_x)
center_py = np.mean(polyon_y)
# 计算栅格的x轴和y轴宽度
width_nx = math.ceil((max(polyon_x) - min(polyon_x)) / resolution) + offset_grid
width_ny = math.ceil((max(polyon_y) - min(polyon_y)) / resolution) + offset_grid
# 得到栅格地图
grid_map = GridMap(center_px, center_py, width_nx, width_ny, resolution)
# 对栅格地图进行赋值
grid_map.setGridMapbyPolyon(polyon_x, polyon_y, 1.0, False)
# 进行膨胀
grid_map.expandGrid()
# 判断扫描是从下到上还是从上到下
if self.sweep_direction_ == Motion.SweepDirection.UP:
goal_nxes, goal_ny = Tools.searchBoundary(grid_map, True)
else:
goal_nxes, goal_ny = Tools.searchBoundary(grid_map, False)
# 记录信息
self.grid_map_ = grid_map
self.goal_nxes_ = goal_nxes
self.goal_ny_ = goal_ny
