-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathanimator.py
182 lines (158 loc) · 6.75 KB
/
animator.py
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
#!/usr/bin/env python3
# Modified based on USC's MAPF class project
from matplotlib.patches import Circle, Rectangle
from matplotlib.lines import Line2D
import matplotlib.pyplot as plt
import matplotlib.colors as mcolors
import numpy as np
from matplotlib import animation
# Colors = ['green', 'blue', 'orange']
Colors = list(mcolors.CSS4_COLORS)
FPS = 60
class Animation:
def __init__(self, agents, my_map, starts, goals, history):
self.radius = len(agents['p1']) // 2
self.my_map = np.flip(np.transpose(my_map), 1)
# self.my_map = np.flip(np.transpose(my_map), 0)
self.starts = []
for start in starts:
self.starts.append((start[1], len(self.my_map[0]) - 1 - start[0]))
self.goals = []
for goal in goals:
self.goals.append((goal[1], len(self.my_map[0]) - 1 - goal[0]))
self.paths = []
if history:
for i in range(len(starts)):
self.paths.append([])
for state in history:
self.paths[-1].append(
(state[i][1],
len(self.my_map[0]) - 1 - state[i][0])
)
aspect = len(self.my_map) / len(self.my_map[0])
self.fig = plt.figure(frameon=False, figsize=(4 * aspect, 4))
self.ax = self.fig.add_subplot(111, aspect='equal')
self.fig.subplots_adjust(left=0, right=1, bottom=0, top=1,
wspace=None, hspace=None)
# self.ax.set_frame_on(False)
self.patches = []
self.artists = []
self.agents = dict()
self.agent_names = dict()
# create boundary patch
x_min = -0.5
y_min = -0.5
x_max = len(self.my_map) - 0.5
y_max = len(self.my_map[0]) - 0.5
plt.xlim(x_min, x_max)
plt.ylim(y_min, y_max)
self.patches.append(
Rectangle((x_min, y_min),
x_max - x_min, y_max - y_min,
facecolor='none', edgecolor='gray')
)
for i in range(len(self.my_map)):
for j in range(len(self.my_map[0])):
if self.my_map[i][j]:
self.patches.append(
Rectangle((i - 0.5, j - 0.5),
1, 1,
facecolor='gray', edgecolor='gray')
)
# create agents:
self.T = 0
# draw goals first
for i, goal in enumerate(self.goals):
self.patches.append(
Rectangle((goal[0] - 0.25, goal[1] - 0.25),
0.5, 0.5,
facecolor=Colors[i % len(Colors)],
edgecolor='black',
alpha=0.5)
)
self.sensors = dict()
for i in range(len(self.paths)):
name = f'p{i + 1}'
self.agents[i] = Circle((starts[i][0], starts[i][1]),
0.3,
facecolor=Colors[i % len(Colors)],
edgecolor='black')
self.agents[i].original_face_color = Colors[i % len(Colors)]
self.patches.append(self.agents[i])
self.T = max(self.T, len(self.paths[i]) - 1)
self.agent_names[i] = self.ax.text(self.starts[i][0],
self.starts[i][1],
name)
self.agent_names[i].set_horizontalalignment('center')
self.agent_names[i].set_verticalalignment('center')
self.artists.append(self.agent_names[i])
for j in range(i + 1, len(self.paths)):
line = Line2D((starts[i][0], starts[j][0]),
(starts[i][1], starts[j][1]),
color='black',
linestyle='dotted',
alpha=0)
self.sensors[f'{i}-{j}'] = line
self.artists.append(line)
self.animation = animation.FuncAnimation(self.fig, self.animate_func,
init_func=self.init_func,
frames=int(self.T + 1) * FPS,
interval=1000 // FPS,
blit=True)
def save(self, file_name, speed):
self.animation.save(
file_name,
# fps=FPS * speed,
dpi=200,
savefig_kwargs={"pad_inches": 0}) # "bbox_inches": "tight"
@staticmethod
def show():
plt.show()
def init_func(self):
for p in self.patches:
self.ax.add_patch(p)
for a in self.artists:
self.ax.add_artist(a)
return self.patches + self.artists
def animate_func(self, t):
for k in range(len(self.paths)):
pos = self.get_state(t / FPS, self.paths[k])
self.agents[k].center = (pos[0], pos[1])
self.agent_names[k].set_position((pos[0], pos[1]))
# reset all colors
for _, agent in self.agents.items():
agent.set_facecolor(agent.original_face_color)
# check drive-drive collisions
agents_array = [agent for _, agent in self.agents.items()]
# sensors = []
for i in range(0, len(agents_array)):
for j in range(i + 1, len(agents_array)):
d1 = agents_array[i]
d2 = agents_array[j]
pos1 = np.array(d1.center)
pos2 = np.array(d2.center)
if np.linalg.norm(pos1 - pos2) < 0.7:
d1.set_facecolor('red')
d2.set_facecolor('red')
print(f"COLLISION! (agent-agent)"
"({i}, {j}) at time {t / FPS}")
self.sensors[f'{i}-{j}'].set_xdata((pos1[0], pos2[0]))
self.sensors[f'{i}-{j}'].set_ydata((pos1[1], pos2[1]))
self.sensors[f'{i}-{j}'].set(alpha=0)
# if np.linalg.norm(pos1 - pos2) <= np.sqrt(2) * self.radius:
# self.sensors[f'{i}-{j}'].set(alpha=0.5)
if abs(pos1[0] - pos2[0]) <= self.radius and \
abs(pos1[1] - pos2[1]) <= self.radius:
self.sensors[f'{i}-{j}'].set(alpha=0.5)
return self.patches + self.artists # + sensors
@staticmethod
def get_state(t, path):
if int(t) <= 0:
return np.array(path[0])
elif int(t) >= len(path):
return np.array(path[-1])
else:
pos_last = np.array(path[int(t) - 1])
pos_next = np.array(path[int(t)])
pos = (pos_next - pos_last) * (t - int(t)) + pos_last
return pos