Multi-Agent-Coverage-Path-Planning-Algorithm/main.py at main · windycn/Multi-Agent-Coverage-Path-Planning-Algorithm · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
from getPath import plan_coverage_path
from mapTools import (
    gen_base_map, randomStartPoint, random_obstacle_map, map2np,
    basic_region_partition, advanced_region_partition, visualize_multi_agent_path
)
import numpy as np

# 多机覆盖路径规划示例
def multi_agent_coverage_example():
    print("\n=== 多机覆盖路径规划示例 ===")

    # 生成测试地图
    test_map = gen_base_map(16, 19, 2)
    print(f"测试地图尺寸: {test_map.shape}")

    # 选择区域划分算法
    print("\n1. 选择区域划分算法：")
    print("   1. 初阶区域划分算法")
    print("   2. 高阶区域划分算法")
    algorithm_choice = input("   请选择 (1/2): ")

    # 输入智能体数量
    num_agents = int(input("\n2. 请输入智能体数量 (2-4): "))

    # 执行区域划分
    if algorithm_choice == '1':
        print("\n使用初阶区域划分算法...")
        regions = basic_region_partition(test_map, num_agents)
    else:
        print("\n使用高阶区域划分算法...")
        regions = advanced_region_partition(test_map, num_agents)

    # 保存子地图
    submap_names = []
    for i, region in enumerate(regions):
        submap = region['map']
        submap_name = f"submap_{i+1}"
        submap_names.append(submap_name)

        # 保存为npy文件
        import os
        if not os.path.exists("maps"):
            os.makedirs("maps")
        np.save(f"maps/{submap_name}.npy", submap)

    # 为每个子地图规划路径
    print("\n3. 为每个子地图规划路径...")
    paths = plan_coverage_path(submap_names, isprint=False, isconsole=False)

    # 提取路径点
    path_points = []
    for path in paths:
        points = path['Path_point_list']
        path_points.append(points)

    # 可视化多机路径
    print("\n4. 可视化多机覆盖路径...")
    visualize_multi_agent_path(test_map, regions, path_points, f"多机覆盖路径规划_{num_agents}智能体")

    # 打印各区域信息
    print("\n5. 各区域信息:")
    for i, region in enumerate(regions):
        start_row, start_col, end_row, end_col = region['bounds']
        area = region['area']
        print(f"   区域 {i+1}: 边界=({start_row},{start_col})-({end_row},{end_col}), 面积={area}")

    print("\n多机覆盖路径规划示例完成！")

if __name__ == "__main__":
    print("=== 多机覆盖路径规划算法 ===")
    print("1. 单机路径规划示例")
    print("2. 多机覆盖路径规划示例")
    choice = input("请选择 (1/2): ")

    if choice == '1':
        # 预设地图
        Test_map = [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
                   [0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1],
                   [0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1],
                   [0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 0, 0, 0],
                   [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1],
                   [0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1],
                   [0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1],
                   [0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1],
                   [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
                   [0, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1],
                   [0, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1],
                   [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2]]
        Cover1 = [
            [2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1, 0],
            [1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1, 0],
            [1, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0],
            [1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
        ]
        Cover2 = [
            [0, 0, 0, 0],
            [0, 1, 1, 1],
            [0, 1, 1, 1],
            [0, 0, 0, 0],
            [0, 1, 1, 0],
            [0, 1, 1, 0],
            [0, 0, 0, 0],
            [2, 1, 1, 0],
            [0, 1, 1, 0],
            [0, 0, 0, 0],
            [0, 1, 1, 0],
            [0, 1, 1, 0],
            [0, 1, 1, 0],
            [0, 1, 1, 0],
            [0, 1, 1, 0],
            [0, 0, 0, 0]
        ]
        Cover3 = [
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0],
            [1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1, 0],
            [1, 1, 0, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0],
            [1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 0]
        ]
        maps = [Test_map, Cover1, Cover2, Cover3]
        map_name_list = ["Test_map", "Cover1", "Cover2", "Cover3"]

        # 生成基础地图
        Base_map = gen_base_map()
        # 生成随机地图
        Random_obstacle_map = random_obstacle_map()
        # 随机添加一个起始点
        Base_map = randomStartPoint(Base_map, 1)
        Random_obstacle_map = randomStartPoint(Random_obstacle_map, 1)

        maps.append(Base_map)
        map_name_list.append("Base_map")

        maps.append(Random_obstacle_map)
        map_name_list.append("Random_obstacle_map")

        # 将所有数组地图转换成npy地图，储存在/map/XX.npy
        map2np(maps, map_name_list)

        # 载入地图，生成规划
        best_trajectory_list = plan_coverage_path(map_name_list, True, True, False)
        print(best_trajectory_list)

    elif choice == '2':
        # 运行多机覆盖路径规划示例
        multi_agent_coverage_example()

    else:
        print("无效选择！")