├── requirements.txt ├── images ├── alg.png ├── Cover1.png ├── Cover2.png ├── Cover3.png ├── Base_map.png ├── Test_map.png ├── framework.png └── Random_obstacle_map.png ├── main.py ├── mapTools.py ├── README.md ├── getPath.py └── PathPlanningCore.py /requirements.txt: -------------------------------------------------------------------------------- 1 | matplotlib==3.1.2 2 | numpy==1.17.4 3 | tabulate==0.8.9 -------------------------------------------------------------------------------- /images/alg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/windycn/Multi-Agent-Coverage-Path-Planning-Algorithm/HEAD/images/alg.png -------------------------------------------------------------------------------- /images/Cover1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/windycn/Multi-Agent-Coverage-Path-Planning-Algorithm/HEAD/images/Cover1.png -------------------------------------------------------------------------------- /images/Cover2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/windycn/Multi-Agent-Coverage-Path-Planning-Algorithm/HEAD/images/Cover2.png -------------------------------------------------------------------------------- /images/Cover3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/windycn/Multi-Agent-Coverage-Path-Planning-Algorithm/HEAD/images/Cover3.png -------------------------------------------------------------------------------- /images/Base_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/windycn/Multi-Agent-Coverage-Path-Planning-Algorithm/HEAD/images/Base_map.png -------------------------------------------------------------------------------- /images/Test_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/windycn/Multi-Agent-Coverage-Path-Planning-Algorithm/HEAD/images/Test_map.png -------------------------------------------------------------------------------- /images/framework.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/windycn/Multi-Agent-Coverage-Path-Planning-Algorithm/HEAD/images/framework.png -------------------------------------------------------------------------------- /images/Random_obstacle_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/windycn/Multi-Agent-Coverage-Path-Planning-Algorithm/HEAD/images/Random_obstacle_map.png -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | from getPath import plan_coverage_path 2 | from mapTools import gen_base_map, randomStartPoint, random_obstacle_map, map2np 3 | 4 | if __name__ == "__main__": 5 | # 预设地图 6 | Test_map = [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 7 | [0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1], 8 | [0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1], 9 | [0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 0, 0, 0], 10 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1], 11 | [0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1], 12 | [0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1], 13 | [0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1], 14 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 15 | [0, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1], 16 | [0, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1], 17 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2]] 18 | Cover1 = [ 19 | [2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 20 | [1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0], 21 | [1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0], 22 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 23 | [1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1, 0], 24 | [1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1, 0], 25 | [1, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0], 26 | [1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0], 27 | [1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0], 28 | ] 29 | Cover2 = [ 30 | [0, 0, 0, 0], 31 | [0, 1, 1, 1], 32 | [0, 1, 1, 1], 33 | [0, 0, 0, 0], 34 | [0, 1, 1, 0], 35 | [0, 1, 1, 0], 36 | [0, 0, 0, 0], 37 | [2, 1, 1, 0], 38 | [0, 1, 1, 0], 39 | [0, 0, 0, 0], 40 | [0, 1, 1, 0], 41 | [0, 1, 1, 0], 42 | [0, 1, 1, 0], 43 | [0, 1, 1, 0], 44 | [0, 1, 1, 0], 45 | [0, 0, 0, 0] 46 | ] 47 | Cover3 = [ 48 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0], 49 | [1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1, 0], 50 | [1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1, 0], 51 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 52 | [1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0], 53 | [1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0], 54 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 0] 55 | ] 56 | maps = [Test_map, Cover1, Cover2, Cover3] 57 | map_name_list = ["Test_map", "Cover1", "Cover2", "Cover3"] 58 | 59 | # 生成基础地图 60 | Base_map = gen_base_map() 61 | # 生成随机地图 62 | Random_obstacle_map = random_obstacle_map() 63 | # 随机添加一个起始点 64 | Base_map = randomStartPoint(Base_map, 1) 65 | Random_obstacle_map = randomStartPoint(Random_obstacle_map, 1) 66 | 67 | maps.append(Base_map) 68 | map_name_list.append("Base_map") 69 | 70 | maps.append(Random_obstacle_map) 71 | map_name_list.append("Random_obstacle_map") 72 | 73 | # 将所有数组地图转换成npy地图，储存在/map/XX.npy 74 | map2np(maps, map_name_list) 75 | 76 | # 载入地图，生成规划 77 | best_trajectory_list = plan_coverage_path(map_name_list, True, True, False) 78 | print(best_trajectory_list) -------------------------------------------------------------------------------- /mapTools.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import random 3 | import copy 4 | import os 5 | 6 | def gen_base_map(rows=16, cols=19, obstacle_size=2): 7 | ''' 8 | 生成基础的数组地图，可以手动调整障碍后调用map2np生成持久储存使用的npy地图 9 | 10 | :param rows: 行数 (空格为0) 11 | :param cols: 列数 (空格为0) 12 | :param obstacle_size: 障碍尺寸 (障碍默认2X2大小，为1) 13 | :return: grid: 生成的map数组 14 | ''' 15 | # 创建一个二维数组，初始值为0 16 | grid = np.zeros((rows, cols), dtype=int) 17 | 18 | # 在边界周围的一圈设置为0 19 | grid[0, :] = grid[-1, :] = grid[:, 0] = grid[:, -1] = 0 20 | 21 | # 随机放置大障碍物，确保周围一圈都是0 22 | for i in range(1, rows - 1, obstacle_size + 1): 23 | for j in range(1, cols - 1, obstacle_size + 1): 24 | if grid[i, j] == 0: 25 | for x in range(obstacle_size): 26 | for y in range(obstacle_size): 27 | if i + x < rows and j + y < cols: 28 | grid[i + x, j + y] = 1 29 | 30 | return grid 31 | 32 | 33 | def random_obstacle_map(rows=10, cols=12): 34 | ''' 35 | 生成随机障碍地图 36 | 37 | :param rows: 行数 (空格为0) 38 | :param cols: 列数 (空格为0) 39 | :return: grid: 生成的map数组 40 | ''' 41 | # 创建一个二维数组，初始值为0 42 | grid = np.zeros((rows, cols), dtype=int) 43 | 44 | # 在边界周围的一圈设置为0 45 | grid[0, :] = grid[-1, :] = grid[:, 0] = grid[:, -1] = 0 46 | 47 | for i in range(2, rows - 2, 2): 48 | for j in range(2, cols - 2, 2): 49 | if grid[i, j] == 0: 50 | if random.choice([True, False]): 51 | # 横向障碍 52 | grid[i, j] = grid[i, j + 1] = 1 53 | else: 54 | # 纵向障碍 55 | grid[i, j] = grid[i + 1, j] = 1 56 | 57 | return grid 58 | 59 | def randomStartPoint(input_map: list, startpoint=1): 60 | ''' 61 | 随机生成起始点 (置为2) 加入到地图中(随机四周放点，四周所有的[0][0]和首行[0]与尾行[-1]) 62 | 63 | :param input_map: 输入的地图 64 | :param startpoint: 起始点数量，默认为1（一次规划只能规划最开始的起始点），划分后可以进行多个 65 | :return: new_map: 含随机起始点的新地图 66 | ''' 67 | # 使用深拷贝创建副本 68 | print(input_map) 69 | input_map = copy.deepcopy(input_map) 70 | # 定义地图的行数和列数 71 | map_row = len(input_map) 72 | map_columns = len(input_map[0]) 73 | 74 | # 随机四周放点，四周所有的[0][0]和首行[0]与尾行[-1] 75 | possible_start_positions = [(0, 0)] + [(0, i) for i in range(map_columns)] + [(map_row - 1, 0)] + [(map_row - 1, i) for i in range(map_columns)] 76 | 77 | # 随机选择不重复的起始位置 78 | selected_start_positions = random.sample(possible_start_positions, startpoint) 79 | 80 | # 在地图上标记起始点（起始点为2） 81 | for pos in selected_start_positions: 82 | input_map[pos[0]][pos[1]] = 2 83 | 84 | print("含起始点({})的地图：".format(selected_start_positions)) 85 | # 打印包含起始点的地图 86 | for row in input_map: 87 | print(row) 88 | 89 | return input_map 90 | 91 | 92 | def map2np(maps: list, map_name_list: list): 93 | ''' 94 | 将数组地图转换成持久储存使用的npy地图 95 | 96 | :param maps: 地图数据列表 (嵌套数组) 97 | :param map_name_list: 地图名字列表 (也是保存的文件名maps/map_name.npy) 98 | :return: None 99 | ''' 100 | # 检查目标文件夹是否存在，如果不存在则创建 101 | if not os.path.exists("maps"): 102 | os.makedirs("maps") 103 | 104 | for i in range(0, len(maps)): 105 | print("地图名称：" + map_name_list[i]) 106 | m = np.array(maps[i]) 107 | with open("maps/{}.npy".format(map_name_list[i]), 'wb') as f: 108 | np.save(f, m) 109 | 110 | m = None 111 | with open("maps/{}.npy".format(map_name_list[i]), 'rb') as f: 112 | m = np.load(f) 113 | print(m) 114 | 115 | print("所有地图数据转换完毕") -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## 多机覆盖路径规划算法 Multi-Agent Coverage Path Planning Algorithm 2 | 3 | ### 整体思路 Overall 4 | ![整体流程框架图](https://github.com/windycn/Multi-Agent-Coverage-Path-Planning-Algorithm/blob/main/images/framework.png) 5 | 6 | 将多机问题转化为单机问题，每个个体（无人机/UAV）覆盖式扫描每个划分好的地图，最后合并每个地图下的路径规划，完成多机协同覆盖搜索。 7 | 8 | 每个划分地图都包含一个个体在内，因此有多种划分方式，这里的区域划分算法效果可能有初阶、中阶和高阶之分。理想情况下，覆盖所有的地图划分需要尽可能地均匀分配地图，以确保总时长最少。更高阶的区域划分需要考虑到空间的连通性和每个区间的步数，划分出更加合理的区间。 9 | 10 | Transforming the multi-agent problem into a single-agent problem, where each agent (UAV) conducts coverage scanning over individually partitioned maps. Finally, the path planning for each map is merged to achieve collaborative multi-agent coverage search. 11 | 12 | Each partitioned map includes one agent, giving rise to multiple partitioning approaches. The quality of the area partitioning algorithm can be categorized as basic, intermediate, or advanced. Ideally, achieving an even distribution of maps for coverage is essential to minimize the overall time. Advanced area partitioning methods consider spatial connectivity and the number of steps in each partition to create more rational divisions. 13 | 14 | 单机路径规划算法（启发式）Single-Agent Path Planning Algorithm (Heuristic): 15 | 16 | ![算法原理](https://github.com/windycn/Multi-Agent-Coverage-Path-Planning-Algorithm/blob/main/images/alg.png) 17 | 18 | ### 待加算法 Todo List 19 | 20 | - [x] 单机路径规划算法（启发式）Single-Agent Path Planning Algorithm (Heuristic) 21 | 22 | - [ ] 初阶区域划分算法 Basic Region Partitioning Algorithm 23 | 24 | - [ ] 高阶区域划分算法 Advanced Region Partitioning Algorithm 25 | 26 | - [ ] 转译层（①普通地图与01地图互转；②划分后01地图对应至总图位置坐标转译）Translation layer ( ①Common map and 01 map intertransfer; ② After division 01 map corresponds to the general map position coordinate translation ) 27 | 28 | ### 安装和使用 Installation & Usage 29 | ```bash 30 | conda create -n CoPath python=3.8 31 | conda activate CoPath 32 | pip install -r requirements.txt 33 | ``` 34 | 35 | ```bash 36 | python main.py 37 | ``` 38 | 39 | ### 现有函数说明 Existing function specification 40 | - PathPlanningCore 算法核心层 41 | - getPath 算法解算层 42 | ```python 43 | def plan_coverage_path(maps: list, isprint=True, isconsole=True ,test_show_each_result=False) -> list: 44 | """ 45 | 覆盖路径规划算法生成函数 46 | 47 | :param maps: (map_name_list) 输入已有的map(npy)格式数据文件名； 48 | :param isprint: (默认为True) 是否输出图示； 49 | :param isconsole: (默认为True) 是否控制台打印信息； 50 | :param test_show_each_result: (默认为False) 是否显示每个结果的测试标志； 51 | :return: best_trajectory_list: 最好的路径列表 52 | 53 | { 54 | "map_name": 地图文件名, 55 | 56 | "start_pos": 起始位置, 57 | 58 | "end_pos": 结束位置, 59 | 60 | "start_orientation": 初始方向 ['^', '<', 'v', '>'], 61 | 62 | "start_orientation_code": 初始方向代码 [0, 1, 2, 3], 63 | 64 | "coverage_path_Heuristic": 启发式算法名称（MANHATTAN曼哈顿距离；CHEBYSHEV切比雪夫距离；VERTICAL垂直启发式；HORIZONTAL水平启发式, 65 | 66 | "Path_point_list": 路径点列表 ([row_id, column_id]), 67 | 68 | "Cost": 总代价, 69 | 70 | "Steps": 总步长, 71 | 72 | "policy_map": 策略地图 73 | } 74 | """ 75 | ``` 76 | - mapTools 地图工具 77 | ```python 78 | def gen_base_map(rows=16, cols=19, obstacle_size=2): 79 | ''' 80 | 生成基础的数组地图，可以手动调整障碍后调用map2np生成持久储存使用的npy地图 81 | 82 | :param rows: 行数 (空格为0) 83 | :param cols: 列数 (空格为0) 84 | :param obstacle_size: 障碍尺寸 (障碍默认2X2大小，为1) 85 | :return: grid: 生成的map数组 86 | ''' 87 | 88 | def random_obstacle_map(rows=10, cols=12): 89 | ''' 90 | 生成随机障碍地图 91 | 92 | :param rows: 行数 (空格为0) 93 | :param cols: 列数 (空格为0) 94 | :return: grid: 生成的map数组 95 | ''' 96 | 97 | def randomStartPoint(input_map: list, startpoint=1): 98 | ''' 99 | 随机生成起始点 (置为2) 加入到地图中(随机四周放点，四周所有的[0][0]和首行[0]与尾行[-1]) 100 | 101 | :param input_map: 输入的地图 102 | :param startpoint: 起始点数量，默认为1（一次规划只能规划最开始的起始点），划分后可以进行多个 103 | :return: new_map: 含随机起始点的新地图 104 | ''' 105 | 106 | def map2np(maps: list, map_name_list: list): 107 | ''' 108 | 将数组地图转换成持久储存使用的npy地图 109 | 110 | :param maps: 地图数据列表 (嵌套数组) 111 | :param map_name_list: 地图名字列表 (也是保存的文件名maps/map_name.npy) 112 | :return: None 113 | ''' 114 | ``` 115 | 116 | ### 效果展示 Results 117 | 基础地图[Base_map] 118 | ![Base_map](https://github.com/windycn/Multi-Agent-Coverage-Path-Planning-Algorithm/blob/main/images/Base_map.png) 119 | 120 | 修改后基础地图[Test_map] 121 | ![Test_map](https://github.com/windycn/Multi-Agent-Coverage-Path-Planning-Algorithm/blob/main/images/Test_map.png) 122 | 123 | 随机障碍地图[Random_obstacle_map] 124 | ![Random_obstacle_map](https://github.com/windycn/Multi-Agent-Coverage-Path-Planning-Algorithm/blob/main/images/Random_obstacle_map.png) 125 | 126 | 划分区域1[Cover1] 127 | ![Cover1](https://github.com/windycn/Multi-Agent-Coverage-Path-Planning-Algorithm/blob/main/images/Cover1.png) 128 | 129 | 划分区域2[Cover2] 130 | ![Cover2](https://github.com/windycn/Multi-Agent-Coverage-Path-Planning-Algorithm/blob/main/images/Cover2.png) 131 | 132 | 划分区域3[Cover3] 133 | ![Cover3](https://github.com/windycn/Multi-Agent-Coverage-Path-Planning-Algorithm/blob/main/images/Cover3.png) 134 | -------------------------------------------------------------------------------- /getPath.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from PathPlanningCore import CoveragePlanner, HeuristicType, PlannerStatus 3 | from tabulate import tabulate 4 | import os 5 | 6 | import matplotlib.pyplot as plt 7 | import matplotlib as mpl 8 | 9 | # 指定中文字体的路径 10 | font_path = "C:\\Windows\\Fonts\\simhei.ttf" # 根据实际路径进行修改 11 | plt.rcParams['font.family'] = 'SimHei' 12 | 13 | from matplotlib.patches import Patch 14 | from matplotlib.lines import Line2D 15 | 16 | # 覆盖规划器的调试级别 17 | cp_debug_level = 0 18 | # 是否显示每个结果的测试标志 19 | test_show_each_result = False 20 | 21 | # 载入地图 22 | def load_map(map_name): 23 | with open("maps/{}.npy".format(map_name), 'rb') as f: 24 | return np.load(f) 25 | 26 | # 使用matplotlib绘制结果 27 | def plot_map(target_map, trajectory, map_name="map", params_str=""): 28 | # 从CoveragePlanner到转换动作为定向移动的参考 29 | movement = [[-1, 0], # 上 30 | [0, -1], # 左 31 | [1, 0], # 下 32 | [0, 1]] # 右 33 | action = [-1, 0, 1, 2] 34 | 35 | # 创建一个图形 36 | fig, ax = plt.subplots() 37 | 38 | # 定义颜色 39 | start_position_color = 'gold' # 起始位置颜色 40 | start_orientation_color = 'deeppink' # 起始方向颜色 41 | status_color_ref = { 42 | PlannerStatus.STANDBY: 'black', 43 | PlannerStatus.COVERAGE_SEARCH: 'royalblue', 44 | PlannerStatus.NEARST_UNVISITED_SEARCH: 'darkturquoise', 45 | PlannerStatus.FOUND: 'mediumseagreen', 46 | PlannerStatus.NOT_FOUND: 'red' 47 | } 48 | 49 | cmap = mpl.colors.ListedColormap( 50 | ['w', 'k', start_position_color, status_color_ref[PlannerStatus.FOUND], status_color_ref[PlannerStatus.NOT_FOUND]]) 51 | norm = mpl.colors.BoundaryNorm([0, 1, 2, 3, 4, 5], cmap.N) 52 | 53 | # 定义状态到cmap引用idx的转换 54 | status_to_cmap_pos = { 55 | PlannerStatus.FOUND: 3, 56 | PlannerStatus.NOT_FOUND: 4 57 | } 58 | 59 | # 复制原始地图以避免更改 60 | target_map_ref = np.copy(target_map) 61 | 62 | # 将最后访问的位置的参考添加到地图中，以反映其在地图上的颜色 63 | target_map_ref[ 64 | trajectory[-1][1]][trajectory[-1][2]] = status_to_cmap_pos[trajectory[-1][6]] 65 | 66 | # 绘制带颜色的地图 67 | ax.imshow(target_map_ref, interpolation='none', cmap=cmap, norm=norm) 68 | 69 | # 在轨迹的每个动作上绘制箭头 70 | for i in range(len(trajectory)-1): 71 | 72 | x = trajectory[i][2] 73 | y = trajectory[i][1] 74 | 75 | # 将动作值添加到当前方向将导致的移动索引 76 | mov_idx = (trajectory[i][3]+action[trajectory[i][5]]) % len(movement) 77 | mov = movement[mov_idx] 78 | 79 | # 从参考列表中获取对应的状态颜色 80 | arrow_color = status_color_ref[trajectory[i][6]] 81 | 82 | # 仅为了改善可视化，将A*箭头略微右移/下移 83 | if trajectory[i][6] == PlannerStatus.NEARST_UNVISITED_SEARCH: 84 | # 检查是否为垂直或水平移动 85 | if mov_idx % 2: 86 | y -= 0.25 87 | else: 88 | x += 0.25 89 | 90 | # 从当前位置到下一个位置添加箭头点 91 | ax.arrow(x, y, mov[1], mov[0], width=0.1, 92 | color=arrow_color, length_includes_head=True) 93 | 94 | # 绘制初始方向 95 | init_direction = np.array(movement[trajectory[0][3]])/2 96 | ax.arrow(trajectory[0][2]-init_direction[1]/2, trajectory[0][1]-init_direction[0]/2, init_direction[1], init_direction[0], width=0.1, 97 | color=start_orientation_color, length_includes_head=True, head_length=0.2) 98 | 99 | # 添加图例 100 | legend_elements = [ 101 | Line2D([0], [0], color=status_color_ref[PlannerStatus.COVERAGE_SEARCH], lw=1, marker='>', 102 | markerfacecolor=status_color_ref[PlannerStatus.COVERAGE_SEARCH], label='前进(覆盖搜索)'), 103 | Line2D([0], [0], color=status_color_ref[PlannerStatus.NEARST_UNVISITED_SEARCH], lw=1, marker='>', 104 | markerfacecolor=status_color_ref[PlannerStatus.NEARST_UNVISITED_SEARCH], label='迂回(A*搜索)'), 105 | Line2D([0], [0], color='w', lw=1, marker='>', 106 | markerfacecolor=start_orientation_color, label='初始方向'), 107 | Line2D([0], [0], marker='s', color='w', label='起始位置', 108 | markerfacecolor=start_position_color, markersize=15), 109 | Line2D([0], [0], marker='s', color='w', label='结束位置', 110 | markerfacecolor=status_color_ref[trajectory[-1][6]], markersize=15), 111 | Line2D([0], [0], marker='s', color='w', 112 | label='障碍物', markerfacecolor='k', markersize=15), 113 | ] 114 | ax.legend(handles=legend_elements, bbox_to_anchor=( 115 | 1.025, 1.0), loc='upper left') 116 | plt.title("覆盖路径规划[{}]\n{}".format(map_name, params_str)) 117 | plt.tight_layout() 118 | plt.show() 119 | # 检查目标文件夹是否存在，如果不存在则创建 120 | if not os.path.exists("output_images"): 121 | os.makedirs("output_images") 122 | fig.savefig("output_images/{}.png".format(map_name), bbox_inches='tight') 123 | 124 | 125 | def plan_coverage_path(maps: list, isprint=True, isconsole=True ,test_show_each_result=False) -> list: 126 | """ 127 | 覆盖路径规划算法生成函数 128 | 129 | :param maps: (map_name_list) 输入已有的map(npy)格式数据文件名； 130 | :param isprint: (默认为True) 是否输出图示； 131 | :param isconsole: (默认为True) 是否控制台打印信息； 132 | :param test_show_each_result: (默认为False) 是否显示每个结果的测试标志； 133 | :return: best_trajectory_list: 最好的路径列表 134 | 135 | { 136 | "map_name": 地图文件名, 137 | 138 | "start_pos": 起始位置, 139 | 140 | "end_pos": 结束位置, 141 | 142 | "start_orientation": 初始方向 ['^', '<', 'v', '>'], 143 | 144 | "start_orientation_code": 初始方向代码 [0, 1, 2, 3], 145 | 146 | "coverage_path_Heuristic": 启发式算法名称（MANHATTAN曼哈顿距离；CHEBYSHEV切比雪夫距离；VERTICAL垂直启发式；HORIZONTAL水平启发式, 147 | 148 | "Path_point_list": 路径点列表 ([row_id, column_id]), 149 | 150 | "Cost": 总代价, 151 | 152 | "Steps": 总步长, 153 | 154 | "policy_map": 策略地图 155 | } 156 | """ 157 | # 为每个地图动态计算最佳覆盖启发式的列表 158 | cp_heuristics = [HeuristicType.VERTICAL, 159 | HeuristicType.HORIZONTAL, HeuristicType.CHEBYSHEV, HeuristicType.MANHATTAN] 160 | orientations = [0, 1, 2, 3] 161 | best_trajectory_list = [] 162 | for map_name in maps: 163 | compare_tb = [] 164 | 165 | target_map = load_map(map_name) 166 | cp = CoveragePlanner(target_map) 167 | cp.set_debug_level(cp_debug_level) 168 | 169 | # 对每个方向和每个启发式进行迭代 170 | for heuristic in cp_heuristics: 171 | for orientation in orientations: 172 | if test_show_each_result: 173 | print("\n\n迭代[地图：{}，cp：{}，初始方向：{}]".format( 174 | map_name, heuristic.name, orientation)) 175 | 176 | cp.start(initial_orientation=orientation, cp_heuristic=heuristic) 177 | cp.compute() 178 | 179 | if test_show_each_result: 180 | cp.show_results() 181 | 182 | res = [heuristic.name, orientation] 183 | res.extend(cp.result()) 184 | compare_tb.append(res) 185 | 186 | # 按步数排序 187 | compare_tb.sort(key=lambda x: (x[3], x[4])) 188 | 189 | # 显示结果 190 | if isconsole: 191 | print("测试的地图：{}".format(map_name)) 192 | 193 | # 打印给定地图的结果摘要 194 | summary = [row[0:5] for row in compare_tb] 195 | for row in summary: 196 | # 格式化成2位小数的成本 197 | row[4] = "{:.2f}".format(row[4]) 198 | # 将移动索引转换为移动名称 199 | row[1] = cp.movement_name[row[1]] 200 | 201 | compare_tb_headers = ["启发式", 202 | "初始方向", "找到?", "步数", "成本"] 203 | summary_tb = tabulate(summary, compare_tb_headers, 204 | tablefmt="pretty", floatfmt=".2f") 205 | if isconsole: 206 | print(summary_tb) 207 | 208 | # 打印最佳覆盖规划器的策略地图 209 | if isconsole: 210 | cp.print_policy_map(trajectory=compare_tb[0][5], trajectory_annotations=[]) 211 | 212 | # 绘制完整的轨迹地图 213 | if isprint: 214 | plot_map(target_map, compare_tb[0][5], map_name=map_name, 215 | params_str="启发式:{}, 初始方向: {}".format(compare_tb[0][0], cp.movement_name[compare_tb[0][1]])) 216 | 217 | # 打印最佳路径 218 | if isconsole: 219 | print("\n最佳路径的坐标列表：[地图：{}，初始方向：{} ({})，覆盖路径启发式：{}]".format( 220 | map_name, cp.movement_name[compare_tb[0][1]], compare_tb[0][1], compare_tb[0][0])) 221 | print(compare_tb[0][6]) 222 | print("\n\n") 223 | 224 | # 返回信息 225 | best_trajectory_list.append({ 226 | "map_name": map_name, 227 | "start_pos": compare_tb[0][6][0], 228 | "end_pos": compare_tb[0][6][-1], 229 | "start_orientation": cp.movement_name[compare_tb[0][1]], 230 | "start_orientation_code": compare_tb[0][1], 231 | "coverage_path_Heuristic": compare_tb[0][0], 232 | "Path_point_list": compare_tb[0][6], 233 | "Cost": summary[0][-1], 234 | "Steps": summary[0][-2], 235 | "policy_map": compare_tb[0][5] 236 | }) 237 | 238 | return best_trajectory_list 239 | 240 | # 测试 241 | if __name__ == "__main__": 242 | # 载入地图 243 | maps = ["map1", "map2", "map3", "map4"] 244 | best_trajectory_list = plan_coverage_path(maps, False, False, False) 245 | print(best_trajectory_list) -------------------------------------------------------------------------------- /PathPlanningCore.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import copy 3 | from enum import Enum, IntEnum, auto 4 | 5 | # 定义PlannerStatus枚举类型 6 | class PlannerStatus(Enum): 7 | STANDBY = auto() # 待机 8 | COVERAGE_SEARCH = auto() # 覆盖搜索 9 | NEARST_UNVISITED_SEARCH = auto() # 最近未访问搜索 10 | FOUND = auto() # 找到目标 11 | NOT_FOUND = auto() # 未找到目标 12 | 13 | # 定义HeuristicType枚举类型 14 | class HeuristicType(Enum): 15 | MANHATTAN = auto() # 曼哈顿距离启发式 16 | CHEBYSHEV = auto() # 切比雪夫距离启发式 17 | VERTICAL = auto() # 垂直启发式 18 | HORIZONTAL = auto() # 水平启发式 19 | 20 | # 定义CoveragePlanner类 21 | class CoveragePlanner(): 22 | 23 | def __init__(self, map_open): 24 | self.map_grid = map_open # 地图网格 25 | 26 | # 在x和y轴上的可能移动方式 27 | self.movement = [[-1, 0], # 上 28 | [0, -1], # 左 29 | [1, 0], # 下 30 | [0, 1]] # 右 31 | 32 | # 可读的移动描述['上', '左', '下', '右'] 33 | self.movement_name = ['^', '<', 'v', '>'] 34 | 35 | # 机器人可能执行的动作 36 | self.action = [-1, 0, 1, 2] 37 | self.action_name = ['R', '#', 'L', 'B'] # 右、前进、左、后退 38 | self.action_cost = [.2, .1, .2, .4] 39 | 40 | # A*算法的移动成本 41 | self.a_star_movement_cost = [1, 1, 1, 1] 42 | 43 | # 当前位置 [x, y, 方向 (默认 = 0)] 44 | self.current_pos = self.get_start_position() 45 | 46 | # 轨迹点的列表 47 | self.current_trajectory = [] 48 | self.current_trajectory_annotations = [] 49 | 50 | # 累积访问过的地图位置的网格 51 | self.coverage_grid = np.copy(map_open) 52 | 53 | # 有限状态机变量 54 | self.state_ = PlannerStatus.STANDBY # 初始状态为待机 55 | 56 | # 各种搜索算法的启发式类型 57 | self.a_star_heuristic = HeuristicType.MANHATTAN 58 | self.cp_heuristic = HeuristicType.VERTICAL 59 | 60 | self.debug_level = -1 # 调试级别，默认为-1（不显示调试信息） 61 | 62 | # 设置调试级别 63 | # 决定终端中要显示多少信息 64 | def set_debug_level(self, level): 65 | self.debug_level = level 66 | 67 | # 执行路径规划 68 | def compute(self): 69 | self.printd("compute", "{}".format(self.state_.name), 1) 70 | while self.compute_non_blocking(): 71 | pass 72 | return self.state_ 73 | 74 | # 处理路径规划的有限状态机 75 | def compute_non_blocking(self): 76 | self.printd("compute_non_blocking", "{}".format(self.state_.name), 1) 77 | searching = False 78 | 79 | # 根据self.state_属性开始FSM状态机 80 | if self.state_ == PlannerStatus.COVERAGE_SEARCH: 81 | 82 | # 使用coverage_search算法进行搜索 83 | heuristic = self.create_heuristic( 84 | self.current_pos, self.cp_heuristic) 85 | res = self.coverage_search(self.current_pos, heuristic) 86 | 87 | # 更新当前位置到最终搜索位置 88 | self.current_pos = [res[1][-1][1], res[1][-1][2], res[1][-1][3]] 89 | 90 | self.append_trajectory(res[1], "CS") 91 | 92 | # 更新当前coverage_grid 93 | self.coverage_grid = res[2] 94 | 95 | # 检查路径是否成功找到。如果没有，则尝试找到最近的未访问位置 96 | if res[0]: 97 | self.state_ = PlannerStatus.FOUND 98 | self.current_trajectory[-1][6] = PlannerStatus.FOUND 99 | else: 100 | self.state_ = PlannerStatus.NEARST_UNVISITED_SEARCH 101 | searching = True 102 | 103 | elif self.state_ == PlannerStatus.NEARST_UNVISITED_SEARCH: 104 | 105 | # 使用a_star_search_closest_unvisited算法进行搜索 106 | heuristic = self.create_heuristic( 107 | self.current_pos, self.a_star_heuristic) 108 | res = self.a_star_search_closest_unvisited( 109 | self.current_pos, heuristic) 110 | 111 | # 如果找到路径 112 | if res[0]: 113 | # 更新当前位置到最终搜索位置 114 | self.current_pos = [res[1][-1][1], 115 | res[1][-1][2], res[1][-1][3]] 116 | 117 | self.append_trajectory(res[1], "A*") 118 | 119 | # 设置FSM以再次进行覆盖搜索 120 | self.state_ = PlannerStatus.COVERAGE_SEARCH 121 | searching = True 122 | 123 | # 如果找不到路径，就结束搜索 124 | else: 125 | self.state_ = PlannerStatus.NOT_FOUND 126 | if len(self.current_trajectory) > 0: 127 | self.current_trajectory[-1][6] = PlannerStatus.NOT_FOUND 128 | 129 | else: 130 | self.printd("compute_non_blocking", 131 | "给定的状态无效，停止FSM", 0) 132 | 133 | return searching 134 | 135 | # 重新开始初始位置，覆盖网格和轨迹列表，并准备开始搜索 136 | def start(self, initial_orientation=0, a_star_heuristic=None, cp_heuristic=None): 137 | 138 | # 将当前位置设置为给定地图的起始位置 139 | self.current_pos = self.get_start_position( 140 | orientation=initial_orientation) 141 | 142 | self.coverage_grid = np.copy(self.map_grid) 143 | self.current_trajectory = [] 144 | self.current_trajectory_annotations = [] 145 | 146 | if cp_heuristic is not None: 147 | self.cp_heuristic = cp_heuristic 148 | if a_star_heuristic is not None: 149 | self.a_star_heuristic = a_star_heuristic 150 | 151 | self.state_ = PlannerStatus.COVERAGE_SEARCH 152 | self.printd("start", "搜索设置为从{}开始，轨迹和覆盖网格已清除".format( 153 | self.current_pos), debug_level=1) 154 | 155 | # 使用coverage_search算法查找路径 156 | def coverage_search(self, initial_pos, heuristic): 157 | # 创建已访问坐标的参考网格 158 | closed = np.copy(self.coverage_grid) 159 | closed[initial_pos[0]][initial_pos[1]] = 1 160 | 161 | if self.debug_level > 1: 162 | self.printd("coverage_search", 163 | "初始已关闭网格:", 2) 164 | print(closed) 165 | 166 | x = initial_pos[0] 167 | y = initial_pos[1] 168 | o = initial_pos[2] 169 | v = 0 170 | 171 | # 将初始坐标填充到迭代列表中 172 | trajectory = [[v, x, y, o, None, None, self.state_]] 173 | 174 | complete_coverage = False 175 | resign = False 176 | 177 | while not complete_coverage and not resign: 178 | 179 | if self.check_full_coverage(self.map_grid, closed): 180 | self.printd("coverage_search", "完全覆盖", 2) 181 | complete_coverage = True 182 | 183 | else: 184 | # 获取上一个访问的坐标信息 185 | v = trajectory[-1][0] 186 | x = trajectory[-1][1] 187 | y = trajectory[-1][2] 188 | o = trajectory[-1][3] 189 | 190 | # [累积成本, x坐标, y坐标, 方向, 执行的动作, 下一个动作] 191 | possible_next_coords = [] 192 | 193 | # 计算可能的下一个坐标 194 | for a in range(len(self.action)): 195 | o2 = (self.action[a]+o) % len(self.movement) 196 | x2 = x + self.movement[o2][0] 197 | y2 = y + self.movement[o2][1] 198 | 199 | # 检查是否超出地图边界 200 | if x2 >= 0 and x2 < len(self.map_grid) and y2 >= 0 and y2 < len(self.map_grid[0]): 201 | # 检查此位置是否已访问或是否为可访问的位置 202 | if closed[x2][y2] == 0 and self.map_grid[x2][y2] == 0: 203 | # 计算累积成本：当前累积成本 + 动作成本 + 给定位置的启发成本 204 | v2 = v + self.action_cost[a] + heuristic[x2][y2] 205 | possible_next_coords.append( 206 | [v2, x2, y2, o2, a, None, self.state_]) 207 | 208 | # 如果没有可能的下一个位置，停止搜索 209 | if len(possible_next_coords) == 0: 210 | resign = True 211 | self.printd("coverage_search", 212 | "找不到下一个未访问的坐标", 2) 213 | 214 | # 否则使用具有最低成本的下一个位置更新轨迹列表 215 | else: 216 | # 按总成本排序 217 | possible_next_coords.sort(key=lambda x: x[0]) 218 | 219 | # 更新最后轨迹的下一个动作 220 | trajectory[-1][5] = possible_next_coords[0][4] 221 | 222 | # 将具有最低成本的possible_next_coords添加到轨迹列表 223 | trajectory.append(possible_next_coords[0]) 224 | 225 | # 将已选择的possible_next_coords位置标记为已访问 226 | closed[possible_next_coords[0][1] 227 | ][possible_next_coords[0][2]] = 1 228 | 229 | if self.debug_level > 1: 230 | self.printd("coverage_search", "启发式：", 2) 231 | print(heuristic) 232 | 233 | self.printd("coverage_search", "已关闭：", 2) 234 | print(closed) 235 | 236 | self.printd("coverage_search", "策略：", 2) 237 | self.print_policy_map(trajectory, []) 238 | 239 | self.printd("coverage_search", "轨迹：", 2) 240 | self.print_trajectory(trajectory) 241 | 242 | total_cost = self.calculate_trajectory_cost(trajectory) 243 | total_steps = len(trajectory)-1 244 | 245 | self.printd("coverage_search", "找到: {}, 总步数: {}, 总成本: {}".format( 246 | not resign, total_steps, total_cost), 1) 247 | 248 | # 打包标准响应 249 | # 轨迹：[值, x, y, 方向, 执行的动作, 下一个动作, 当前状态_] 250 | # res: [成功？, 轨迹, 最终覆盖网格, 总动作成本, 总步数] 251 | res = [not resign, trajectory, closed, total_cost, total_steps] 252 | 253 | return res 254 | 255 | # 使用A*搜索算法找到初始坐标和目标坐标之间的最短路径 256 | def a_star_search_closest_unvisited(self, initial_pos, heuristic): 257 | 258 | # 创建一个已访问位置的参考网格 259 | closed = np.zeros_like(self.map_grid) 260 | closed[initial_pos[0]][initial_pos[1]] = 1 261 | 262 | if self.debug_level > 1: 263 | self.printd("a_star_search_closest_unvisited", 264 | "初始已关闭网格：", 2) 265 | print(closed) 266 | 267 | # A*访问位置的移动方向 268 | orientation = np.full( 269 | (np.size(self.map_grid, 0), np.size(self.map_grid, 1)), -1) 270 | 271 | # 将给定的A*初始位置与其关联的成本添加到“open”列表中 272 | # “open”是要扩展的有效位置列表：[[f, g, x, y]] 273 | # g：累积a*移动成本（以0成本开始） 274 | # f：总成本= a*移动成本+给定位置的启发成本 275 | # x，y：给定位置 276 | x = initial_pos[0] 277 | y = initial_pos[1] 278 | g = 0 279 | f = g + heuristic[x][y] 280 | open = [[f, g, x, y]] 281 | 282 | found = False # 是否找到了未访问的位置 283 | resign = False # 如果我们找不到扩展，则设置标志 284 | 285 | while not found and not resign: 286 | self.printd("a_star_search_closest_unvisited", 287 | " open：{}".format(open), 2) 288 | 289 | # 如果没有更多要扩展的位置，则未找到未访问的位置，然后放弃 290 | if len(open) == 0: 291 | resign = True 292 | self.printd("a_star_search_closest_unvisited", 293 | " 未找到路径", 2) 294 | 295 | # 否则再次扩展搜索 296 | else: 297 | 298 | # 按总成本的降序对位置列表进行排序并反转它，以弹出具有最低总成本的元素 299 | open.sort(key=lambda x: x[0]) # +heuristic[x[1]][x[2]]) 300 | open.reverse() 301 | next = open.pop() 302 | 303 | # 更新当前搜索的x，y，g 304 | x = next[2] 305 | y = next[3] 306 | g = next[1] 307 | 308 | # 检查是否找到了未访问的位置 309 | if self.coverage_grid[x][y] == 0: 310 | found = True 311 | else: 312 | # 计算可能的下一个坐标 313 | for i in range(len(self.movement)): 314 | x_next = x + self.movement[i][0] 315 | y_next = y + self.movement[i][1] 316 | 317 | # 检查是否超出地图边界 318 | if x_next >= 0 and x_next < len(self.map_grid) and y_next >= 0 and y_next < len(self.map_grid[0]): 319 | # 检查此位置是否已访问或是否为可访问的位置 320 | if closed[x_next][y_next] == 0 and self.map_grid[x_next][y_next] == 0: 321 | g2 = g + self.a_star_movement_cost[i] 322 | f = g2 + heuristic[x_next][y_next] 323 | open.append([f, g2, x_next, y_next]) 324 | closed[x_next][y_next] = 1 325 | orientation[x_next][y_next] = i 326 | 327 | # 初始化轨迹 328 | trajectory = [] 329 | 330 | # 如果找到路径，则构建轨迹。 331 | # 此时x和y代表搜索的最后一个位置，即未访问的位置 332 | if found: 333 | 334 | # 将最后一个位置添加到轨迹列表中，两个操作均为None（稍后将设置） 335 | trajectory = [ 336 | [0, x, y, orientation[x][y], None, None, self.state_]] 337 | 338 | # 将初始方向添加到方向矩阵中 339 | orientation[initial_pos[0]][initial_pos[1]] = initial_pos[2] 340 | 341 | # 从找到的未访问的位置向后移动，直到到达A*的初始位置 342 | # 此下标0表示该变量是前身位置的，因为 343 | # 这个过程从A*的最终位置开始，到A*的初始位置 344 | while (x != initial_pos[0] or y != initial_pos[1]): 345 | # 计算前身位置 346 | x0 = x - self.movement[orientation[x][y]][0] 347 | y0 = y - self.movement[orientation[x][y]][1] 348 | # 前身方向是在方向矩阵上的方向 349 | o0 = orientation[x0][y0] 350 | # 前身操作将在下一次迭代中设置（它是它之前的下一个操作） 351 | a0 = None 352 | 353 | # 计算从前身位置到当前迭代位置所需的操作索引 354 | a = (trajectory[-1][3]-o0 + 1) % len(self.action) 355 | 356 | # 更新后继位置的“action_performed_to_get_here”以当前的下一个动作 357 | trajectory[-1][4] = a 358 | 359 | # 将前身位置和操作添加到轨迹列表中 360 | trajectory.append([0, x0, y0, o0, a0, a, self.state_]) 361 | 362 | # 更新x和y以进行下一次迭代 363 | x = x0 364 | y = y0 365 | 366 | trajectory.reverse() 367 | 368 | if self.debug_level > 1: 369 | self.printd("a_star_search_closest_unvisited", "启发式：", 2) 370 | print(heuristic) 371 | 372 | self.printd("a_star_search_closest_unvisited", "已关闭：", 2) 373 | print(closed) 374 | 375 | self.printd("a_star_search_closest_unvisited", "策略：", 2) 376 | self.print_policy_map(trajectory, orientation) 377 | 378 | self.printd("a_star_search_closest_unvisited", "轨迹：", 2) 379 | self.print_trajectory(trajectory) 380 | 381 | total_cost = self.calculate_trajectory_cost(trajectory) 382 | total_steps = len(trajectory)-1 383 | 384 | self.printd("a_star_search_closest_unvisited", "找到: {}, 总步数: {}, 总成本: {}".format( 385 | found, total_steps, total_cost), 1) 386 | 387 | # 打包标准响应 388 | # 轨迹：[值, x, y, 方向, 执行的动作, 下一个动作, 当前状态_] 389 | # res: [成功？, 轨迹, 最终覆盖网格, 总动作成本, 总步数] 390 | res = [found, trajectory, closed, total_cost, total_steps] 391 | 392 | return res 393 | 394 | # 合并给定的两个网格，并返回True，如果所有可访问的位置都已访问 395 | def check_full_coverage(self, grid, closed): 396 | return np.all(np.copy(grid)+np.copy(closed)) 397 | 398 | # 返回给定目标点的曼哈顿启发式 399 | def create_manhattan_heuristic(self, target_point): 400 | heuristic = np.zeros_like(self.map_grid) 401 | for x in range(len(heuristic)): 402 | for y in range(len(heuristic[0])): 403 | heuristic[x][y] = abs(x-target_point[0])+abs(y-target_point[1]) 404 | return heuristic 405 | 406 | # 返回给定目标点的切比雪夫启发式 407 | def create_chebyshev_heuristic(self, target_point): 408 | heuristic = np.zeros_like(self.map_grid) 409 | for x in range(len(heuristic)): 410 | for y in range(len(heuristic[0])): 411 | heuristic[x][y] = max( 412 | abs(x-target_point[0]), abs(y-target_point[1])) 413 | return heuristic 414 | 415 | # 返回给定目标点的水平启发式 416 | def create_horizontal_heuristic(self, target_point): 417 | heuristic = np.zeros_like(self.map_grid) 418 | for x in range(len(heuristic)): 419 | for y in range(len(heuristic[0])): 420 | heuristic[x][y] = abs(x-target_point[0]) 421 | return heuristic 422 | 423 | # 返回给定目标点的垂直启发式 424 | def create_vertical_heuristic(self, target_point): 425 | heuristic = np.zeros_like(self.map_grid) 426 | for x in range(len(heuristic)): 427 | for y in range(len(heuristic[0])): 428 | heuristic[x][y] = abs(y-target_point[1]) 429 | return heuristic 430 | 431 | # 返回给定目标点和启发式类型的启发式 432 | def create_heuristic(self, target_point, heuristic_type): 433 | heuristic = np.zeros_like(self.map_grid) 434 | for x in range(len(heuristic)): 435 | for y in range(len(heuristic[0])): 436 | if heuristic_type == HeuristicType.MANHATTAN: 437 | heuristic[x][y] = abs( 438 | x-target_point[0]) + abs(y-target_point[1]) 439 | elif heuristic_type == HeuristicType.CHEBYSHEV: 440 | heuristic[x][y] = max( 441 | abs(x-target_point[0]), abs(y-target_point[1])) 442 | elif heuristic_type == HeuristicType.HORIZONTAL: 443 | heuristic[x][y] = abs(x-target_point[0]) 444 | elif heuristic_type == HeuristicType.VERTICAL: 445 | heuristic[x][y] = abs(y-target_point[1]) 446 | return heuristic 447 | 448 | # 返回当前地图网格的初始x、y和方向 449 | def get_start_position(self, orientation=0): 450 | for x in range(len(self.map_grid)): 451 | for y in range(len(self.map_grid[0])): 452 | if(self.map_grid[x][y] == 2): 453 | return [x, y, orientation] 454 | 455 | # 将给定轨迹附加到主轨迹 456 | def append_trajectory(self, new_trajectory, algorithm_ref): 457 | # 如果已经有轨迹位置，则通过连接动作来删除重复位置，并在新轨迹的第一个位置列表末尾添加一个特殊的观测 458 | if len(self.current_trajectory) > 0 and len(new_trajectory) > 0: 459 | # 由于每次搜索只依赖于当前位置，因此必须将累积轨迹的最后一个元素的“action_performed_to_get_here”复制到新轨迹的第一个元素中。 460 | new_trajectory[0][4] = self.current_trajectory[-1][4] 461 | 462 | # 添加一个特殊标注以显示在策略地图上 463 | self.current_trajectory_annotations.append( 464 | [new_trajectory[0][1], new_trajectory[0][2], algorithm_ref]) 465 | 466 | # 移除重复的位置 467 | self.current_trajectory.pop() 468 | 469 | # 将计算得到的路径添加到轨迹列表中 470 | for t in new_trajectory: 471 | self.current_trajectory.append(t) 472 | 473 | # 计算轨迹的总成本 474 | def calculate_trajectory_cost(self, trajectory): 475 | cost = 0 476 | 477 | # 将每个步骤的动作成本相加 478 | for t in trajectory: 479 | if t[5] is not None: 480 | cost += self.action_cost[t[5]] 481 | return cost 482 | 483 | # 返回仅包含轨迹xy的numpy数组 484 | def get_xy_trajectory(self, trajectory): 485 | if type(trajectory) == list: 486 | if type(trajectory[0]) == list: 487 | return [t[1:3] for t in trajectory] 488 | return trajectory[1:3] 489 | return [] 490 | 491 | # 返回搜索结果：[found?, total_steps, total_cost, trajectory, xy_trajectory] 492 | def result(self): 493 | found = self.state_ == PlannerStatus.FOUND 494 | total_steps = len(self.current_trajectory)-1 495 | total_cost = self.calculate_trajectory_cost(self.current_trajectory) 496 | xy_trajectory = self.get_xy_trajectory(self.current_trajectory) 497 | 498 | res = [found, total_steps, total_cost, 499 | self.current_trajectory, xy_trajectory] 500 | return res 501 | 502 | # 打印搜索结果的摘要 503 | def show_results(self): 504 | self.printd("show_results", 505 | "展示当前的搜索结果：\n") 506 | self.printd("show_results", 507 | "最终状态: {}".format(self.state_.name)) 508 | # 最后一个元素只指向最后一个轨迹位置，它不是已完成的步骤。 509 | self.printd("show_results", "总步数: {}".format( 510 | len(self.current_trajectory)-1)) 511 | self.printd("show_results", "总成本: {:.2f}".format( 512 | self.calculate_trajectory_cost(self.current_trajectory))) 513 | if self.debug_level > 0: 514 | self.print_trajectory(self.current_trajectory) 515 | self.print_policy_map() 516 | 517 | # 打印轨迹数据 518 | def print_trajectory(self, trajectory): 519 | self.printd("print_trajectory", "轨迹数据:\n") 520 | print("{}\t{}\t{}\t{}\t{}\t{}".format( 521 | "l_cost", "x", "y", "orient.", "act_in", "act_next")) 522 | for t in trajectory: 523 | print("{:.2f}\t{}\t{}\t{}\t{}\t{}\t{}".format( 524 | t[0], t[1], t[2], t[3], t[4], t[5], t[6].name)) 525 | 526 | # 打印具有标准列宽的给定地图网格 527 | def print_map(self, m): 528 | for row in m: 529 | s = "[" 530 | for i in range(len(m[0])): 531 | if type(row[i]) is str: 532 | s += row[i] 533 | else: 534 | s += "{:.1f}".format(row[i]) 535 | if i is not (len(m[0])-1): 536 | s += "\t," 537 | else: 538 | s += "\t]" 539 | print(s) 540 | 541 | # 计算并打印基于轨迹列表的当前策略地图 542 | def print_policy_map(self, trajectory=None, trajectory_annotations=None): 543 | policy = [[" " for row in range(len(self.map_grid[0]))] 544 | for col in range(len(self.map_grid))] 545 | 546 | # 放置障碍物的参考 547 | for col in range(len(self.map_grid[0])): 548 | for row in range(len(self.map_grid)): 549 | if self.map_grid[row][col] == 1: 550 | policy[row][col] = "XXXXXX" 551 | 552 | if trajectory == None: 553 | trajectory = self.current_trajectory 554 | 555 | if trajectory_annotations == None: 556 | trajectory_annotations = self.current_trajectory_annotations 557 | 558 | # 在每个位置放置下一个动作名称 559 | for t in trajectory: 560 | if t[5] is not None: 561 | policy[t[1]][t[2]] += self.action_name[t[5]] 562 | 563 | # 在地图上放置注释 564 | trajectory_annotations.append( 565 | [trajectory[0][1], trajectory[0][2], "STA"]) 566 | trajectory_annotations.append( 567 | [trajectory[-1][1], trajectory[-1][2], "END"]) 568 | 569 | for t in trajectory_annotations: 570 | policy[t[0]][t[1]] += "@"+t[2] 571 | 572 | self.printd("print_policy_map", "策略地图:\n") 573 | self.print_map(policy) 574 | 575 | # 带有标准化打印结构的打印辅助函数 576 | # [function_name] message 577 | def printd(self, f, m, debug_level=0): 578 | if debug_level <= self.debug_level: 579 | print("["+f+"] "+m) 580 | --------------------------------------------------------------------------------