前序教程请参照专栏,如您是从0开始阅读,可以直接跳到Python运动规划库教程(Python Motion Planning)-0-简介与安装。
创建路径规划器并规划路径。混合搜索规划器Voronoi Planner会创建Voronoi地图,然后依靠另一个路径规划算法(默认A*)来规划路径。你可以通过设置参数base_planner来更改基础规划器,并通过参数base_planner_kwargs向基础规划器传递参数。
planner=VoronoiPlanner(map_=map_,start=start,goal=goal,base_planner=AStar)path,path_info=planner.plan()print(path)print(path_info)打印结果:
[(25, 5, 5), (25, 6, 5), (25, 6, 6), (24, 6, 7), (23, 7, 8), (23, 7, 9), (22, 7, 10), (21, 8, 10), (20, 8, 10), (19, 8, 11), (18, 8, 12), (17, 8, 12), (16, 9, 13), (15, 9, 14), (14, 9, 14), (13, 9, 14), (12, 8, 14), (11, 8, 14), (10, 8, 14), (9, 8, 15), (8, 9, 16), (8, 10, 16), (7, 11, 16), (7, 12, 17), (7, 13, 18), (7, 14, 18), (6, 15, 19), (6, 16, 19), (5, 17, 20), (5, 18, 21), (5, 19, 21), (5, 20, 22), (5, 21, 22), (5, 22, 22), (6, 23, 23), (5, 24, 24), (5, 25, 25), (5, 25, 25)] {'success': True, 'start': (25, 5, 5), 'goal': (5, 25, 25), 'length': 46.92334552620546, 'cost': 46.92334552620546, 'expand': {(25, 6, 5): Node((25, 6, 5), None, 0, 34.07345007480164), ...}, 'voronoi_candidates': array([[[False, False, False, ..., False, False, False], ...]]), 'voronoi_start': (25, 6, 5), 'voronoi_goal': (5, 25, 25), 'voronoi_path': [(25, 6, 5), (25, 6, 6), (24, 6, 7), (23, 7, 8), (23, 7, 9), (22, 7, 10), (21, 8, 10), (20, 8, 10), (19, 8, 11), (18, 8, 12), (17, 8, 12), (16, 9, 13), (15, 9, 14), (14, 9, 14), (13, 9, 14), (12, 8, 14), (11, 8, 14), (10, 8, 14), (9, 8, 15), (8, 9, 16), (8, 10, 16), (7, 11, 16), (7, 12, 17), (7, 13, 18), (7, 14, 18), (6, 15, 19), (6, 16, 19), (5, 17, 20), (5, 18, 21), (5, 19, 21), (5, 20, 22), (5, 21, 22), (5, 22, 22), (6, 23, 23), (5, 24, 24), (5, 25, 25)]}可视化。如果你想可视化Voronoi候选点,将开头两行代码取消注释。
Visualize. If you want to visualize the Voronoi candidates, uncomment the first two line code.
# if "voronoi_candidates" in path_info:# map_.type_map[path_info["voronoi_candidates"]] = TYPES.CUSTOMvis=Visualizer3D()vis.plot_grid_map(map_)vis.plot_path(path)vis.show()vis.close()完整的可运行代码:
importrandom random.seed(0)importnumpyasnp np.random.seed(0)frompython_motion_planning.commonimport*frompython_motion_planning.path_plannerimport*frompython_motion_planning.controllerimport*map_=Grid(bounds=[[0,31],[0,31],[0,31]],resolution=1.0)foriinrange(75):rd_p=tuple(np.random.randint(0,30,size=3))map_.type_map[rd_p[0],rd_p[1],:rd_p[2]]=TYPES.OBSTACLE map_.inflate_obstacles(radius=3)start=(25,5,5)goal=(5,25,25)map_.type_map[start]=TYPES.START map_.type_map[goal]=TYPES.GOAL planner=VoronoiPlanner(map_=map_,start=start,goal=goal,base_planner=AStar)path,path_info=planner.plan()print(path)print(path_info)# if "voronoi_candidates" in path_info:# map_.type_map[path_info["voronoi_candidates"]] = TYPES.CUSTOMvis=Visualizer3D()vis.plot_grid_map(map_)vis.plot_path(path)vis.show()vis.close()如需了解更多规划器及其参数,请参阅官方教程文档中的API Reference部分。