-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmission_control.py
488 lines (363 loc) · 15.3 KB
/
mission_control.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
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
"Control mission for each agents"
import logging
import time
from typing import List
from defer import return_value
import numpy as np
import settings
from agent import Agent
class MissionControl:
"""Handles the agent operations depending on the mission on hand.
"""
__agents: List[Agent]
__swarm_heading: np.ndarray
__swarm_desired_heading: np.ndarray
__rotation_angle: float
def __init__(self, agents: List[Agent]):
"""Initialize the MissionControl.
Args:
agents (List[Agent]): Agents to be operated.
"""
self.__agents = agents
self.__rotation_angle = 0.0
self.__swarm_heading = np.array([0.001, 0.001, 0.0])
self.__swarm_desired_heading = np.array([0.0, 1.0, 0.0])
def __update(self) -> bool:
"""Update the agents
Returns:
bool: Specifies whether the operation was successfull or not.
"""
# Synchronize swarm info
# Update swarm info
swarm_center = np.array([0.0, 0.0, 0.0])
for agent in self.__agents:
swarm_center += agent.get_pos()
swarm_center /= len(self.__agents)
front_agent = self.__agents[0]
dist_diff = front_agent.get_pos() - swarm_center
self.__swarm_heading = dist_diff / np.linalg.norm(dist_diff)
angle_diff = settings.angle_between(self.__swarm_heading, self.__swarm_desired_heading)
# Determine rotation direction
# 1.0 for counter-clockwise -1.0 for counterwise
if (0.5 <= angle_diff):
# print(angle_diff)
rot_dir = 1.0
x_1 = self.__swarm_heading[0]
y_1 = self.__swarm_heading[1]
x_2 = self.__swarm_desired_heading[0]
y_2 = self.__swarm_desired_heading[1]
temp_val = x_1 * y_2 - x_2 * y_1
if temp_val <= 0.0:
rot_dir = -1.0
self.__rotation_angle += rot_dir * 0.05
for agent in self.__agents:
agent.set_rotation_angle(self.__rotation_angle)
return True
def __validate_formation_matrix(self, formation_matrix: np.ndarray) -> bool:
rows_count = len(formation_matrix)
is_valid = True
ret_value = True
for i in range(rows_count):
columns_count = len(formation_matrix[i])
if rows_count != columns_count:
is_valid = False
if not is_valid:
logging.info("Formatin matrix is invalid. Rows and columns count do not match")
ret_value = False
if len(self.__agents) != rows_count:
logging.info(f"Agent count and formation_matrix does not match. Agents: {len(self.__agents)}, formation_matrix: {rows_count}x{rows_count}")
ret_value = False
return ret_value
def take_formation(self, formation_matrix: np.ndarray, duration: float) -> bool:
"""Takes the Agents into the specified formation.
Returns:
bool: Specifies whether the mission was successfull or not.
"""
ret_value = False
logging.info("Starting mission take_formation.")
# Check if formation_matrix matches the agent count
if not self.__validate_formation_matrix(formation_matrix):
logging.info("Aborting!")
return False
# Activate and give the formation parameters
for agent in self.__agents:
agent.set_formation_matrix(formation_matrix)
# Activate the formationControl and swarming
for agent in self.__agents:
agent.set_trajectory_active(False)
agent.set_avoidance_active(True)
agent.set_formation_active(True)
agent.set_swarming(True)
# Wait for the formation to happen
t1_val = time.perf_counter()
t2_val = time.perf_counter()
time.sleep(duration)
for agent in self.__agents:
agent.set_formation_const(1.25)
# Print the distances between
for agent1 in self.__agents:
for agent2 in self.__agents:
if agent1 is not agent2:
print(f"{agent1.get_name()} {agent2.get_name()} -> {round(settings.get_distance(agent1.get_pos(), agent2.get_pos()), 2)}")
logging.info(f"Ending mission take_formation with success.")
return ret_value
def swap_swarm_agents(self, target_agents: List[Agent], new_agents: List[Agent], durations: List[float]) -> bool:
"""_summary_
Args:
target_agents (List[Agent]): _description_
new_agents (List[Agent]): _description_
duration (float): _description_
Returns:
bool: _description_
"""
ret_value = False
logging.info("Starting mission swap_swarm_agents.")
if len(target_agents) != len(new_agents):
logging.info("Size of target_agents and new_agents are not the same! Aborting")
return False
if len(durations) != 4:
logging.info("Size of durations is not 4! Aborting")
return False
# Stop formation forces
for agent in self.__agents:
agent.set_formation_active(False)
agent.set_swarming(False)
pos = agent.get_pos()
agent.set_target_point(pos)
# Prepare the targetAgenys
poses = []
for agent in target_agents:
poses.append(agent.get_pos())
agent.set_trajectory_active(True)
# Move the target_agents back to their spawn point
for agent in target_agents:
cur_pos = agent.get_pos()
initial_pos = agent.getInitialPos()
initial_pos[2] = cur_pos[2]
agent.set_target_point(initial_pos)
# Wait for agents to go
t1_val = time.perf_counter()
t2_val = time.perf_counter()
while t2_val - t1_val <= durations[0]:
t2_val = time.perf_counter()
# Land the target_agents
for agent in target_agents:
agent.land()
# Wait for target_agents to land
t1_val = time.perf_counter()
t2_val = time.perf_counter()
while t2_val - t1_val <= durations[1]:
t2_val = time.perf_counter()
# Remove target_agents from the agents list
for agent in target_agents:
self.__agents.remove(agent)
# Take off the new_agents
for agent in new_agents:
agent.take_off(0.50)
agent.set_avoidance_active(True)
agent.set_trajectory_active(True)
# Wait for new_agents to take_off
t1_val = time.perf_counter()
t2_val = time.perf_counter()
while t2_val - t1_val <= durations[2]:
t2_val = time.perf_counter()
# Move new_agents to their positions in the swarm
for idx, agent in enumerate(new_agents):
agent.set_target_point(poses[idx])
# Wait for new_agents to move
t1_val = time.perf_counter()
t2_val = time.perf_counter()
while t2_val - t1_val <= durations[2]:
t2_val = time.perf_counter()
# Finalize the new_agents
for idx, agent in enumerate(new_agents):
# Add them to the list
self.__agents.append(agent)
# Give them the formation_matrix
agent.set_formation_matrix(target_agents[idx].get_formation_matrix())
# Give them the rotationAngle
agent.set_rotation_angle(target_agents[idx].get_rotation_angle())
# Give them the formationConst
agent.set_formation_const(target_agents[idx].get_formation_const())
# Give them the swarm desiredHeding
agent.set_swarm_desired_heading(target_agents[idx].get_swarm_desired_heading())
# Swap the indexes
temp = target_agents[idx].get_index()
target_agents[idx].set_index(agent.get_index())
agent.set_index(temp)
# Activate formation forces
for agent in self.__agents:
agent.set_formation_active(True)
agent.set_swarming(True)
logging.info(f"Ending mission swap_swarm_agents with success.")
ret_value = True
return ret_value
def load_obstacles(self, obstacles: List[Agent], duration) -> bool:
"""Takes off the agent.
Returns:
bool: Specifies whether the mission was successfull or not.
"""
logging.info(f"Starting mission load_obstacles. Obstacle count: {len(obstacles)}")
ret_value = False
for obstacle in obstacles:
obstacle.set_is_static(True)
for agent in self.__agents:
agent.set_obstacles(obstacles)
t1_val = time.perf_counter()
t2_val = time.perf_counter()
while t2_val - t1_val <= duration:
t2_val = time.perf_counter()
logging.info(f"Ending mission load_obstacles with success.")
agent.set_trajectory_active(False)
return ret_value
def take_off_agent(self, agent: Agent, height: float, duration: float) -> bool:
"""Takes off the agent.
Returns:
bool: Specifies whether the mission was successfull or not.
"""
logging.info("Starting mission take_off_agent.")
ret_value = False
agent.take_off(height)
agent.set_trajectory_active(True)
t1_val = time.perf_counter()
t2_val = time.perf_counter()
while t2_val - t1_val <= duration:
t2_val = time.perf_counter()
logging.info(f"Ending mission take_off_agent with success. Current height: {round(agent.get_pos()[2], 2)}")
#agent.set_trajectory_active(False)
return ret_value
def take_off_all(self, height: float, duration: float) -> bool:
"""Takes off all the agents.
Returns:
bool: Specifies whether the mission was successfull or not.
"""
ret_value = False
print("Take_off_all")
for agent in self.__agents:
ret_value = agent.take_off(height)
agent.set_trajectory_active(True)
t1_val = time.perf_counter()
t2_val = time.perf_counter()
print(t2_val)
while t2_val-t1_val<=duration:
t2_val = time.perf_counter()
pass
print(t2_val)
#agent.set_trajectory_active(False)
return ret_value
def land_agent(self, agent: Agent, duration: float) -> bool:
"""Lands off the agent.
Returns:
bool: Specifies whether the mission was successfull or not.
"""
logging.info("Starting mission land_agent.")
ret_value = False
agent.land()
agent.set_trajectory_active(True)
t1_val = time.perf_counter()
t2_val = time.perf_counter()
while t2_val - t1_val <= duration:
t2_val = time.perf_counter()
logging.info(f"Ending mission land_agent with success. Current height: {round(agent.get_pos()[2], 2)}")
agent.set_trajectory_active(False)
return ret_value
def land_all(self, duration: float):
"""Lands all the agents.
Returns:
bool: Specifies whether the mission was successfull or not.
"""
ret_value = False
logging.info("Starting mission land_all.")
for agent in self.__agents:
ret_value = agent.land()
agent.set_trajectory_active(True)
agent.set_formation_active(False)
agent.set_swarming(False)
t1_val = time.perf_counter()
t2_val = time.perf_counter()
while t2_val - t1_val <= duration:
t2_val = time.perf_counter()
print(f"Ending missionLandAll with success. Total target count: {len(self.__agents)}")
#agent.set_trajectory_active(False)
return ret_value
def goto_agent(self, target_agent: Agent, points: np.ndarray, duration: float) -> bool:
"""Moves the target agent to the specified point.
Args:
agent (Agent): Agent to be moved.
points (np.ndarray): Points to be moved to.
Returns:
bool: Specifies whether the mission was successfull or not.
"""
ret_value = False
logging.info(f"Starting mission goto_agent. Target is '{target_agent.get_name()}'")
# Itarete over the points
for i, point in enumerate(points):
target_agent.set_target_point(np.array([point[0], point[1], point[2]]))
target_agent.set_trajectory_active(True)
t1_val = time.perf_counter()
t2_val = time.perf_counter()
time.sleep(duration)
# Last point
if i == len(points) - 1:
logging.info(f"Ending mission goto_agent with success. Target was '{target_agent.get_name()}'")
ret_value = True
target_agent.set_trajectory_active(False)
return ret_value
def goto_swarm(self, points: np.ndarray, duration: float) -> bool:
"""Moves the swarm of agents to the specified point.
Args:
points (np.ndarray): Points to be moved to.
Returns:
bool: Specifies whether the mission was successfull or not.
"""
ret_value = False
logging.info(f"Starting mission goto_agent.")
# Itarete over the points
for i, point in enumerate(points):
# Set target points and activate trajectory
for agent in self.__agents:
agent.set_target_point(np.array([point[0], point[1], point[2]]))
agent.set_trajectory_active(True)
t1_val = time.perf_counter()
t2_val = time.perf_counter()
while t2_val - t1_val <= duration:
t2_val = time.perf_counter()
# Last point
if i == len(points) - 1:
logging.info(f"Ending mission goto_swarm with success. Total target count: {len(self.__agents)}'")
ret_value = True
# Set target points and activate trajectory
for agent in self.__agents:
agent.set_trajectory_active(False)
return ret_value
def rotate_swarm(self, angle: float, duration: float):
"""Rotates the swarm.
Returns:
bool: Specifies whether the operation was successfull or not.
"""
ret_value = False
logging.info(f"Starting mission rotate_swarm.")
for agent in self.__agents:
agent.set_rotation(angle)
agent.set_trajectory_active(False)
agent.set_formation_const(0.15)
agent.set_formation_active(True)
t1_val = time.perf_counter()
t2_val = time.perf_counter()
while t2_val - t1_val <= duration:
t2_val = time.perf_counter()
for agent in self.__agents:
agent.set_formation_const(1.25)
logging.info(f"Ending rotate_swarm with success. Total target count: {len(self.__agents)}")
ret_value = True
return ret_value
def kill_switch(self) -> bool:
"""Kills the agents in case of an emergency.
Returns:
bool: Specifies whether the operation was successfull or not.
"""
ret_value = False
for agent in self.__agents:
agent.kill()
ret_value = True
return ret_value