def test_check_parallel_drone_commands_nested_should_update_states_correctly(
self):
drone_commands = \
[
ParallelDroneCommands([
[SingleDroneCommand("DRONE1", Command.wait(5))],
[SingleDroneCommand("DRONE2", Command.wait(3)),
ParallelDroneCommands([
[SingleDroneCommand("DRONE3", Command.wait(4))],
[SingleDroneCommand("DRONE4", Command.wait(1))]
])]
]),
SingleDroneCommand("DRONE1", Command.wait(1))
]
drones_involved = {"DRONE1", "DRONE2", "DRONE3", "DRONE4", "DRONE5"}
state_updaters = {
name: StateUpdater(DroneConfig((1, 0, 0), 2, 180, 2))
for name in drones_involved
}
drone_state_map = {name: State() for name in drones_involved}
BoundaryChecker(
self.boundary_config)._update_states_and_check_drone_commands(
drone_commands, state_updaters, drone_state_map,
drones_involved)
expected = {
name: State(time_used_seconds=8)
for name in drones_involved
}
self.assertEqual(expected, drone_state_map)
def _update_states_to_wait_for_slowest_branch(
self, parallel_drone_commands: ParallelDroneCommands,
state_updaters: Dict[str, StateUpdater],
drone_trajectory_map: Dict[str, List[State]],
time_used_in_branches: List[float],
drones_involved: Set[str]) -> float:
longest_time_used = max(time_used_in_branches)
# for each branch, let drones involved in the branch wait until longest_time_used
for i, time_used in enumerate(time_used_in_branches):
for name in parallel_drone_commands.drones_involved_each_branch[i]:
wait_command = Command.wait(longest_time_used - time_used)
self._update_states_for_single_drone_command(
SingleDroneCommand(name, wait_command),
state_updaters,
drone_trajectory_map,
drones_involved={name})
# let drones not involved in any branch wait for longest_time_used
for name in drones_involved.difference(
parallel_drone_commands.get_drones_involved()):
wait_command = Command.wait(longest_time_used)
self._update_states_for_single_drone_command(
SingleDroneCommand(name, wait_command),
state_updaters,
drone_trajectory_map,
drones_involved={name})
return longest_time_used
def test_eq(self):
commands1 = [
None,
Command.takeoff(),
Command.land(),
Command.up(1),
Command.down(1),
Command.left(1),
Command.right(1),
Command.forward(1),
Command.backward(1),
Command.rotate_left(1),
Command.rotate_right(1),
Command.wait(1),
Command.up(2),
Command.down(2),
Command.left(2),
Command.right(2),
Command.forward(2),
Command.backward(2),
Command.rotate_left(2),
Command.rotate_right(2),
Command.wait(2)
]
commands2 = [
None,
Command.takeoff(),
Command.land(),
Command.up(1),
Command.down(1),
Command.left(1),
Command.right(1),
Command.forward(1),
Command.backward(1),
Command.rotate_left(1),
Command.rotate_right(1),
Command.wait(1),
Command.up(2),
Command.down(2),
Command.left(2),
Command.right(2),
Command.forward(2),
Command.backward(2),
Command.rotate_left(2),
Command.rotate_right(2),
Command.wait(2)
]
for i in range(len(commands1)):
for j in range(len(commands2)):
if i == j:
self.assertEqual(commands1[i], commands2[j])
else:
self.assertNotEqual(commands1[i], commands2[j])
def test_multiple_commands_should_return_correct_command(self):
actual = generate_commands("""
main() {
takeoff();
up(1);
down(2);
left(3);
right(4);
forward(5);
backward(6);
rotate_left(7);
rotate_right(8);
wait(9);
land();
}
""")
expected = [
SingleDroneCommand("DEFAULT", Command.takeoff()),
SingleDroneCommand("DEFAULT", Command.up(1)),
SingleDroneCommand("DEFAULT", Command.down(2)),
SingleDroneCommand("DEFAULT", Command.left(3)),
SingleDroneCommand("DEFAULT", Command.right(4)),
SingleDroneCommand("DEFAULT", Command.forward(5)),
SingleDroneCommand("DEFAULT", Command.backward(6)),
SingleDroneCommand("DEFAULT", Command.rotate_left(7)),
SingleDroneCommand("DEFAULT", Command.rotate_right(8)),
SingleDroneCommand("DEFAULT", Command.wait(9)),
SingleDroneCommand("DEFAULT", Command.land())
]
self.assertEqual(expected, actual)
def test_wait_with_drone_name_should_return_correct_command(self):
actual = generate_commands(
"""
main() { DRONE1.wait(1); }
""",
drone_config_map={"DRONE1": DefaultDroneConfig()})
expected = [SingleDroneCommand("DRONE1", Command.wait(1))]
self.assertEqual(expected, actual)
def test_check_single_drone_command_wait_when_taken_off_should_not_give_error(
self):
drone_commands = [
SingleDroneCommand("DRONE1", Command.takeoff()),
SingleDroneCommand("DRONE1", Command.wait(1)),
SingleDroneCommand("DRONE1", Command.land())
]
BoundaryChecker(self.boundary_config).check(drone_commands,
self.state_updater_map)
def visitWait(self, ctx: xDroneParser.WaitContext) -> None:
exprs = [self.visit(expr) for expr in ctx.expr()]
if ctx.DOT():
drone_expr, expr = exprs
else:
drone_expr, expr = None, exprs[0]
if expr.type != Type.int() and expr.type != Type.decimal():
raise CompileError(
"Expression {} should have type int or decimal, but is {}".
format(expr, expr.type))
drone_name = self._get_drone_name(drone_expr)
self._get_latest_commands().append(
SingleDroneCommand(drone_name, Command.wait(expr.value)))
def test_update_wait_should_update_state(self):
state = State(has_taken_off=True,
x_meters=1,
y_meters=2,
z_meters=3,
orientation_degrees=60)
actual = self.state_updater.update(Command.wait(5), state)
expected = State(has_taken_off=True,
time_used_seconds=5,
x_meters=1,
y_meters=2,
z_meters=3,
orientation_degrees=60)
self.assertEqual(expected, actual)
def test_check_single_drone_command_should_detect_collision_and_give_error(self):
drone_commands = [SingleDroneCommand("DRONE1", Command.takeoff()),
SingleDroneCommand("DRONE2", Command.takeoff()),
SingleDroneCommand("DRONE1", Command.right(1)),
SingleDroneCommand("DRONE2", Command.wait(1)),
SingleDroneCommand("DRONE2", Command.rotate_left(90)),
SingleDroneCommand("DRONE2", Command.forward(1)),
SingleDroneCommand("DRONE1", Command.land()),
SingleDroneCommand("DRONE2", Command.land())]
with self.assertRaises(SafetyCheckError) as context:
self.collision_checker.check(drone_commands, self.state_updater_map)
self.assertTrue("Collisions might happen!\nCollision might happen between DRONE1 and DRONE2"
in str(context.exception))
def test_check_parallel_drone_commands_should_let_drone_not_involved_wait(
self):
drone_commands = [
ParallelDroneCommands(
[[], [SingleDroneCommand("DRONE2", Command.wait(5))]])
]
drone_state_map = {"DRONE1": State(), "DRONE2": State(x_meters=1)}
drones_involved = {"DRONE1", "DRONE2"}
BoundaryChecker(
self.boundary_config)._update_states_and_check_drone_commands(
drone_commands, self.state_updater_map, drone_state_map,
drones_involved)
expected = {
"DRONE1": State(time_used_seconds=5),
"DRONE2": State(time_used_seconds=5, x_meters=1)
}
self.assertEqual(expected, drone_state_map)
def test_execute_single_drone_command(self):
single_commands = [
SingleDroneCommand("name1", Command.takeoff()),
SingleDroneCommand("name1", Command.land()),
SingleDroneCommand("name2", Command.up(1.1)),
SingleDroneCommand("name2", Command.down(1.2)),
SingleDroneCommand("name2", Command.left(1.3)),
SingleDroneCommand("name2", Command.right(1.4)),
SingleDroneCommand("name2", Command.forward(1.5)),
SingleDroneCommand("name2", Command.backward(1.6)),
SingleDroneCommand("name2", Command.rotate_left(91)),
SingleDroneCommand("name2", Command.rotate_right(92)),
SingleDroneCommand("name2", Command.wait(1))
]
with patch(
'xdrone.command_converters.dji_tello_edu_drone_executor.FlyTello',
return_value=self.fly_tello):
executor = DJITelloEduExecutor(self.name_id_map)
executor.execute_drone_commands(single_commands)
calls = [
call.wait_sync(),
call.takeoff(tello=1),
call.wait_sync(),
call.land(tello=1),
call.wait_sync(),
call.up(110, tello=2),
call.wait_sync(),
call.down(120, tello=2),
call.wait_sync(),
call.left(130, tello=2),
call.wait_sync(),
call.right(140, tello=2),
call.wait_sync(),
call.forward(150, tello=2),
call.wait_sync(),
call.back(160, tello=2),
call.wait_sync(),
call.rotate_ccw(91, tello=2),
call.wait_sync(),
call.rotate_cw(92, tello=2),
call.wait_sync(),
call.pause(1)
]
self.fly.assert_has_calls(calls)
def test_check_parallel_drone_commands_should_wait_for_slower_branch(self):
drone_commands = [
ParallelDroneCommands(
[[SingleDroneCommand("DRONE1", Command.takeoff())],
[SingleDroneCommand("DRONE2", Command.wait(5))]])
]
drone_state_map = {"DRONE1": State(), "DRONE2": State(x_meters=1)}
drones_involved = {"DRONE1", "DRONE2"}
BoundaryChecker(
self.boundary_config)._update_states_and_check_drone_commands(
drone_commands, self.state_updater_map, drone_state_map,
drones_involved)
expected = {
"DRONE1": State(has_taken_off=True,
time_used_seconds=5,
z_meters=1),
"DRONE2": State(time_used_seconds=5, x_meters=1)
}
self.assertEqual(expected, drone_state_map)
def test_convert_command(self):
commands = [
SingleDroneCommand("DRONE1", Command.takeoff()),
SingleDroneCommand("DRONE1", Command.up(1)),
SingleDroneCommand("DRONE1", Command.down(1)),
SingleDroneCommand("DRONE1", Command.left(1)),
SingleDroneCommand("DRONE1", Command.right(1)),
SingleDroneCommand("DRONE1", Command.forward(1)),
SingleDroneCommand("DRONE1", Command.backward(1)),
SingleDroneCommand("DRONE1", Command.rotate_left(1)),
SingleDroneCommand("DRONE1", Command.rotate_right(1)),
ParallelDroneCommands(
[[
ParallelDroneCommands(
[[], [SingleDroneCommand("DRONE1", Command.up(1))]])
],
[
SingleDroneCommand("DRONE2", Command.takeoff()),
SingleDroneCommand("DRONE2", Command.land())
]]),
SingleDroneCommand("DRONE1", Command.land()),
SingleDroneCommand("DRONE1", Command.wait(1))
]
drone_config_map = {
"DRONE1": DroneConfig((1, 2, 3), 1, 90, 2),
"DRONE2": DroneConfig((0, 0, 0), 1, 90, 1)
}
expected = {
"config": [{
"name": "DRONE1",
"init_pos": [1, 2, 3],
"speed": 1,
"rotate_speed": 90,
"takeoff_height": 2
}, {
"name": "DRONE2",
"init_pos": [0, 0, 0],
"speed": 1,
"rotate_speed": 90,
"takeoff_height": 1
}],
"commands": [{
"type": "single",
"name": "DRONE1",
"action": "takeoff",
"values": []
}, {
"type": "single",
"name": "DRONE1",
"action": "up",
"values": [1]
}, {
"type": "single",
"name": "DRONE1",
"action": "down",
"values": [1]
}, {
"type": "single",
"name": "DRONE1",
"action": "left",
"values": [1]
}, {
"type": "single",
"name": "DRONE1",
"action": "right",
"values": [1]
}, {
"type": "single",
"name": "DRONE1",
"action": "forward",
"values": [1]
}, {
"type": "single",
"name": "DRONE1",
"action": "backward",
"values": [1]
}, {
"type": "single",
"name": "DRONE1",
"action": "rotate_left",
"values": [1]
}, {
"type": "single",
"name": "DRONE1",
"action": "rotate_right",
"values": [1]
}, {
"type":
"parallel",
"branches": [[{
"type":
"parallel",
"branches": [[],
[{
"type": "single",
"name": "DRONE1",
"action": "up",
"values": [1]
}]]
}],
[{
"type": "single",
"name": "DRONE2",
"action": "takeoff",
"values": []
}, {
"type": "single",
"name": "DRONE2",
"action": "land",
"values": []
}]]
}, {
"type": "single",
"name": "DRONE1",
"action": "land",
"values": []
}, {
"type": "single",
"name": "DRONE1",
"action": "wait",
"values": [1]
}]
}
actual = SimulationConverter().convert(commands, drone_config_map)
self.assertEqual(expected,
json.loads(zlib.decompress(b64decode(actual))))
def _update_states(self, single_drone_command: SingleDroneCommand,
state_updaters: Dict[str, StateUpdater],
drone_trajectory_map: Dict[str, List[State]],
drones_involved: Set[str]) -> float:
time_interval = self.collision_config.time_interval_seconds
drone_name = single_drone_command.drone_name
command = single_drone_command.command
if command.opcode == "takeoff":
takeoff_distance = self.drone_config_map[
drone_name].takeoff_height_meters
new_drone_command = SingleDroneCommand(
drone_name, Command.up(takeoff_distance))
return self._update_states(new_drone_command, state_updaters,
drone_trajectory_map, drones_involved)
if command.opcode == "land":
old_state = drone_trajectory_map[drone_name][-1]
land_distance = old_state.z_meters
new_drone_command = SingleDroneCommand(drone_name,
Command.down(land_distance))
return self._update_states(new_drone_command, state_updaters,
drone_trajectory_map, drones_involved)
if command.opcode == "wait":
seconds, = command.operands
old_state = drone_trajectory_map[drone_name][-1]
if seconds <= time_interval:
new_state = state_updaters[drone_name].update(
Command.wait(seconds), old_state)
drone_trajectory_map[drone_name].append(new_state)
for name in drones_involved.difference({drone_name}):
state = drone_trajectory_map[name][-1]
drone_trajectory_map[name].append(
state.copy_and_set_time_used_seconds(
state.time_used_seconds + seconds))
return seconds
else:
new_state = state_updaters[drone_name].update(
Command.wait(time_interval), old_state)
drone_trajectory_map[drone_name].append(new_state)
for name in drones_involved.difference({drone_name}):
state = drone_trajectory_map[name][-1]
drone_trajectory_map[name].append(
state.copy_and_set_time_used_seconds(
state.time_used_seconds + time_interval))
new_drone_command = SingleDroneCommand(
drone_name, Command.wait(seconds - time_interval))
return time_interval + self._update_states(
new_drone_command, state_updaters, drone_trajectory_map,
drones_involved)
if command.opcode in ["rotate_left", "rotate_right"]:
degrees, = command.operands
old_state = drone_trajectory_map[drone_name][-1]
rotate_speed = self.drone_config_map[drone_name].rotate_speed_dps
seconds = degrees / rotate_speed
if seconds <= time_interval:
new_state = state_updaters[drone_name].update(
Command(command.opcode, [rotate_speed * seconds]),
old_state)
drone_trajectory_map[drone_name].append(new_state)
for name in drones_involved.difference({drone_name}):
state = drone_trajectory_map[name][-1]
drone_trajectory_map[name].append(
state.copy_and_set_time_used_seconds(
state.time_used_seconds + seconds))
return seconds
else:
new_state = state_updaters[drone_name].update(
Command(command.opcode, [rotate_speed * time_interval]),
old_state)
drone_trajectory_map[drone_name].append(new_state)
for name in drones_involved.difference({drone_name}):
state = drone_trajectory_map[name][-1]
drone_trajectory_map[name].append(
state.copy_and_set_time_used_seconds(
state.time_used_seconds + time_interval))
new_drone_command = SingleDroneCommand(
drone_name,
Command(command.opcode,
[degrees - rotate_speed * time_interval]))
return time_interval + self._update_states(
new_drone_command, state_updaters, drone_trajectory_map,
drones_involved)
if command.opcode in [
"up", "down", "left", "right", "forward", "backward"
]:
meters, = command.operands
old_state = drone_trajectory_map[drone_name][-1]
speed = self.drone_config_map[drone_name].speed_mps
seconds = meters / speed
if seconds <= time_interval:
new_state = state_updaters[drone_name].update(
Command(command.opcode, [speed * seconds]), old_state)
drone_trajectory_map[drone_name].append(new_state)
for name in drones_involved.difference({drone_name}):
state = drone_trajectory_map[name][-1]
drone_trajectory_map[name].append(
state.copy_and_set_time_used_seconds(
state.time_used_seconds + seconds))
return seconds
else:
new_state = state_updaters[drone_name].update(
Command(command.opcode, [speed * time_interval]),
old_state)
drone_trajectory_map[drone_name].append(new_state)
for name in drones_involved.difference({drone_name}):
state = drone_trajectory_map[name][-1]
drone_trajectory_map[name].append(
state.copy_and_set_time_used_seconds(
state.time_used_seconds + time_interval))
new_drone_command = SingleDroneCommand(
drone_name,
Command(command.opcode, [meters - speed * time_interval]))
return time_interval + self._update_states(
new_drone_command, state_updaters, drone_trajectory_map,
drones_involved)
def test_wait(self):
self.assertEqual(Command.wait(1), Command.wait(1))
self.assertEqual("wait", Command.wait(1).opcode)
self.assertEqual([1], Command.wait(1).operands)
self.assertEqual("Command: { opcode: wait, operands: [1] }",
str(Command.wait(1)))
def test_dji_tello_executor_should_send_correct_message(self) -> None:
mocked_socket = Mock()
mocked_socket.recvfrom = Mock(return_value=(b'ok', ('192.168.10.1',
8889)))
mocked_socket.sendto = Mock()
commands = [
Command.takeoff(),
Command.up(1),
Command.down(1),
Command.left(1),
Command.right(1),
Command.forward(1),
Command.backward(1),
Command.rotate_left(90),
Command.rotate_right(90),
Command.wait(0),
Command.land()
]
executor = DJITelloExecutor()
executor.sock.close()
executor.sock = mocked_socket
with self.assertLogs(logging.getLogger()) as log:
executor.execute_commands(commands)
calls = [
call(b"command", ('192.168.10.1', 8889)),
call(b"takeoff", ('192.168.10.1', 8889)),
call(b"up 100", ('192.168.10.1', 8889)),
call(b"down 100", ('192.168.10.1', 8889)),
call(b"left 100", ('192.168.10.1', 8889)),
call(b"right 100", ('192.168.10.1', 8889)),
call(b"forward 100", ('192.168.10.1', 8889)),
call(b"back 100", ('192.168.10.1', 8889)),
call(b"ccw 90", ('192.168.10.1', 8889)),
call(b"cw 90", ('192.168.10.1', 8889)),
call(b"land", ('192.168.10.1', 8889))
]
mocked_socket.sendto.assert_has_calls(calls)
expected_log = [
"INFO:root:sent message: b'command'",
"INFO:root:received response: ok",
"INFO:root:sent message: b'takeoff'",
"INFO:root:received response: ok",
"INFO:root:sent message: b'up 100'",
"INFO:root:received response: ok",
"INFO:root:sent message: b'down 100'",
"INFO:root:received response: ok",
"INFO:root:sent message: b'left 100'",
"INFO:root:received response: ok",
"INFO:root:sent message: b'right 100'",
"INFO:root:received response: ok",
"INFO:root:sent message: b'forward 100'",
"INFO:root:received response: ok",
"INFO:root:sent message: b'back 100'",
"INFO:root:received response: ok",
"INFO:root:sent message: b'ccw 90'",
"INFO:root:received response: ok",
"INFO:root:sent message: b'cw 90'",
"INFO:root:received response: ok",
"INFO:root:sent message: b'land'",
"INFO:root:received response: ok"
]
self.assertEqual(expected_log, log.output)
def test_wait_with_decimal_parameter_should_return_correct_command(self):
actual = generate_commands("""
main() { wait(1.0); }
""")
expected = [SingleDroneCommand("DEFAULT", Command.wait(1.0))]
self.assertEqual(expected, actual)
