ConsensusDecisionMaking/Draw_Only.py at main · shushukunkunkun/ConsensusDecisionMaking · 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
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
import math
import numpy as np
from matplotlib import pyplot as plt
from shapely import LineString, MultiLineString
from sympy import Ellipse
from matplotlib.patches import Ellipse, Circle
from Class_Def.AssignmentManager import AssignmentManager
from Class_Def.GroundRobot import GroundRobot
from Class_Def.uav_control import UAV
from other_function import discretize_line
def main():
    # 初始化参数
    dt = 1
    total_time = 400
    sim_time = 0.0
    ShowMe = True
    '''
    初始化地图
    '''
    # TODO Map I 为L形状地图
    line1_points = [(-50, -150), (250, -150), (375, -100), (475, -100), (600, -150), (600, -500),(1300,-500)]
    line2_points = [(-50, 150), (250, 150), (375, 100), (475, 100), (600, 150), (900, 150), (900, -200), (1050, -200), (1200, -300), (1300, -300)]
    line3_points = [(100, 25), (750, 25), (750, -350), (1000, -350)]
    line4_points = [(800, -50), (800, -300), (1000, -300)]
    line5_points = [(850, -50), (850, -250), (1000, -250)]
    line6_points = [(650, -150),(650, -350)]
    line7_points = [(200,-25),(700, -25),(700, -400), (1000, -400)]
    # 离散化折线
    discretized_line1 = discretize_line(line1_points)
    discretized_line2 = discretize_line(line2_points)
    discretized_line3 = discretize_line(line3_points)
    discretized_line4 = discretize_line(line4_points)
    discretized_line5 = discretize_line(line5_points)
    discretized_line6 = discretize_line(line6_points)
    discretized_line7 = discretize_line(line7_points)
    obstacle_coor = np.vstack((discretized_line1, discretized_line2, discretized_line3, discretized_line4, discretized_line5, discretized_line6, discretized_line7))
    # 将点列表转换为 LineString
    line1 = LineString(line1_points)
    line2 = LineString(line2_points)
    line3 = LineString(line3_points)
    line4 = LineString(line4_points)
    line5 = LineString(line5_points)
    line6 = LineString(line6_points)
    line7 = LineString(line7_points)
    # 将多个 LineString 合并为一个 MultiLineString
    multi_line = MultiLineString([line1, line2, line3, line4, line5, line6, line7])
    # # TODO Map Ⅱ 为Z形状地图
    # line1_points = [(-50, 150), (250, 150), (375, 100), (475, 100), (600, 150), (900, 150), (900, 0), (650, -250), (900, -250)]
    # line2_points = [(-50, -150), (350, -150),  (-50, -550), (900, -550)]
    # line3_points = [(100, 25), (750, 25), (400, -350), (650, -350)]
    # line4_points = [(600, 75), (825, 75), (800, 0), (525, -300), (650, -300)]
    # line5_points = [(200, -50), (625, -50), (250, -425), (650, -425)]
    # # 离散化折线
    # discretized_line1 = discretize_line(line1_points)
    # discretized_line2 = discretize_line(line2_points)
    # discretized_line3 = discretize_line(line3_points)
    # discretized_line4 = discretize_line(line4_points)
    # discretized_line5 = discretize_line(line5_points)
    # obstacle_coor = np.vstack((discretized_line1, discretized_line2, discretized_line3, discretized_line4,discretized_line5))
    # # 将点列表转换为 LineString
    # line1 = LineString(line1_points)
    # line2 = LineString(line2_points)
    # line3 = LineString(line3_points)
    # line4 = LineString(line4_points)
    # line5 = LineString(line5_points)
    # # 将多个 LineString 合并为一个 MultiLineString
    # multi_line = MultiLineString([line1, line2, line3, line4, line5])
    # 初始化无人机
    uavs = [
        UAV(0, [0.0, 100.0], [1.0, 0.0], [1300.0, -400.0], obstacle_coor,dt),
        UAV(1, [0.0, 50.0],  [1.0, 0.0], [1300.0, -400.0], obstacle_coor,dt),
        UAV(2, [0.0, 0.0],[1.0, 0.0], [1300.0, -400.0], obstacle_coor,dt),
        UAV(3, [0.0, -50.0], [1.0, 0.0], [650.0, -500.0], obstacle_coor, dt),
        UAV(4, [0.0, -100.0], [1.0, 0.0], [1300.0, -400.0], obstacle_coor, dt)
    ]
    # 初始化地面机器人
    ground_robots = []
    total_N = 30
    np.random.seed(1)
    for i in range(total_N):
        position = np.random.uniform(low=-20.0, high=20.0, size=2)
        robot = GroundRobot(obstacle_coor,robot_id=i, position=position)
        ground_robots.append(robot)

    # 创建 AssignmentManager
    assignment_manager = AssignmentManager(uavs, ground_robots,obstacle_coor,multi_line)
    if ShowMe == True:
        # 设置绘图
        plt.ion()
        fig, ax = plt.subplots()
        ax.set_aspect('equal', adjustable='datalim')
        ax.set_xlim(-100, 1000)
        ax.set_ylim(-500, 200)
    # 主循环
    while sim_time < total_time:
        '''
        更新
        1.无人机领导的地面机器人成员
        2.地面机器人的归属无人机编号
        3.无人机之间的合作拓扑网络
        '''
        if sim_time > 270:
            a = 1
        assignment_manager.update_assignments()
        # 更新无人机状态
        for uav in uavs:
            uav.compute_control_input(uavs)
            uav.update_state()
        # 更新地面机器人状态
        for robot in ground_robots:
            assigned_set_ids = robot.assigned_uav  # 这是一个集合，存储分配的 set_id
            if assigned_set_ids:
                # 假设每个机器人只分配给一个集合，如果有多个，选择第一个
                set_id = next(iter(assigned_set_ids))
                # 从 AssignmentManager 的 sets 中找到对应的集合
                assigned_set = next((s for s in assignment_manager.sets if s['set_id'] == set_id), None)
                if assigned_set:
                    # 获取集合中的无人机索引列表
                    uav_indices = assigned_set['uav_indices']
                    # 这里可以选择如何处理多个无人机，例如选取最近的一个
                    # 简单起见，取集合中的第一个无人机
                    uav_idx = next(iter(uav_indices))
                    assigned_uav = uavs[uav_idx]
                    group_robots = assigned_uav.assigned_robots
                    x_c = assigned_set['virtual_region_params']['center']
                    x_axis = assigned_set['virtual_region_params']['a']
                    y_axis = assigned_set['virtual_region_params']['b']
                    v_c = assigned_set['virtual_region_params']['velocity']
                    theta = assigned_set['virtual_region_params']['angle']
                    robot.update_control_input(group_robots, x_c, v_c, x_axis, y_axis, theta, dt)
                    robot.update_state(dt)
            else:
                # 如果未分配无人机，机器人保持原地或执行其他默认行为
                pass
        if ShowMe == True:
            # 清除当前绘图
            ax.clear()

            # 绘制障碍物
            ax.scatter(obstacle_coor[:, 0], obstacle_coor[:, 1], color='red', s=10, label='Obstacle')

            # 定义颜色列表，确保颜色列表足够长
            colors = ['b', 'g', 'm', 'c', 'y', 'orange', 'purple', 'brown', 'pink', 'gray']

            # 绘制集合及其虚拟区域
            for idx, s in enumerate(assignment_manager.sets):
                color = colors[idx % len(colors)]  # 为每个集合分配一个颜色
                # 绘制集合的虚拟区域椭圆
                params = s.get('virtual_region_params', None)
                if params:
                    center = params['center']
                    a = params['a']
                    b = params['b']
                    angle = params['angle']
                    ellipse = Ellipse(
                        xy=center,
                        width=2 * a,
                        height=2 * b,
                        angle=angle * 180 / math.pi,  # 确保角度为度数
                        edgecolor=color,
                        facecolor='none',
                        linewidth=2
                    )
                    ax.add_patch(ellipse)

                # 绘制该集合中的 UAV 和机器人
                for uav_idx in s['uav_indices']:
                    uav = uavs[uav_idx]
                    # 绘制 UAV
                    ax.scatter(uav.position[0], uav.position[1], color=color, marker='^', s=100)
                    # 绘制感知范围
                    perception_circle = Circle(
                        (uav.position[0], uav.position[1]),
                        uav.perception_radius,
                        edgecolor=color,
                        facecolor=color,
                        alpha=0.1,
                        linestyle='--',
                        linewidth=1
                    )
                    ax.add_patch(perception_circle)
                    # 绘制分配给该 UAV 的机器人
                    for robot in uav.assigned_robots:
                        ax.plot(robot.position[0], robot.position[1], 'o', color=color)
            # 设置标题
            ax.set_title(f'Time: {sim_time:.2f}s')

            # 显示绘图
            plt.draw()
            plt.pause(0.01)

        # 时间推进
        sim_time += dt
        assignment_manager.time4debug = sim_time
        # time.sleep(1)
    if ShowMe == True:
        plt.ioff()
        plt.show()

if __name__ == '__main__':
    main()