def test_update_right_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.right(1), state)
expected = State(has_taken_off=True,
time_used_seconds=1 / 0.5,
x_meters=1 + 0.5,
y_meters=2 - sqrt(3) / 2,
z_meters=3,
orientation_degrees=60)
self.assertEqual(expected, actual)
state = State(has_taken_off=True,
x_meters=1,
y_meters=2,
z_meters=3,
orientation_degrees=240)
actual = self.state_updater.update(Command.right(1), state)
expected = State(has_taken_off=True,
time_used_seconds=1 / 0.5,
x_meters=1 - 0.5,
y_meters=2 + sqrt(3) / 2,
z_meters=3,
orientation_degrees=240)
self.assertEqual(expected, actual)
def test_dji_tello_executor_if_receive_error_should_print(self) -> None:
mocked_socket = Mock()
mocked_socket.recvfrom = Mock(side_effect=Exception('timed out'))
mocked_socket.sendto = Mock()
commands = [Command.takeoff(), 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"land", ('192.168.10.1', 8889))
]
mocked_socket.sendto.assert_has_calls(calls)
expected_log = [
"INFO:root:sent message: b'command'",
"ERROR:root:Error met when receiving response: timed out",
"INFO:root:sent message: b'takeoff'",
"ERROR:root:Error met when receiving response: timed out",
"INFO:root:sent message: b'land'",
"ERROR:root:Error met when receiving response: timed out"
]
self.assertEqual(expected_log, log.output)
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_dji_tello_executor_if_interrupt_error_should_stop_and_emergency(
self) -> None:
mocked_socket = Mock()
mocked_socket.recvfrom = Mock(return_value=(b'ok', ('192.168.10.1',
8889)))
mocked_socket.sendto = Mock()
def mocked_sendto(msg, _):
if msg == b"takeoff":
raise KeyboardInterrupt
mocked_socket.sendto.side_effect = mocked_sendto
commands = [Command.takeoff(), 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"emergency", ('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:KeyboardInterrupt received. Forced stop."
]
self.assertEqual(expected_log, log.output)
def test_update_rotate_left_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.rotate_left(90), state)
expected = State(has_taken_off=True,
time_used_seconds=1 / 0.5,
x_meters=1,
y_meters=2,
z_meters=3,
orientation_degrees=330)
self.assertEqual(expected, actual)
state = State(has_taken_off=True,
x_meters=1,
y_meters=2,
z_meters=3,
orientation_degrees=240)
actual = self.state_updater.update(Command.rotate_left(90), state)
expected = State(has_taken_off=True,
time_used_seconds=1 / 0.5,
x_meters=1,
y_meters=2,
z_meters=3,
orientation_degrees=150)
self.assertEqual(expected, actual)
def test_execute_parallel_drone_commands(self):
parallel_commands = [
ParallelDroneCommands(
[[
SingleDroneCommand("name1", Command.takeoff()),
SingleDroneCommand("name1", Command.land())
],
[
SingleDroneCommand("name2", Command.takeoff()),
SingleDroneCommand("name2", Command.land())
]])
]
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(parallel_commands)
calls = [
call.wait_sync(),
call.takeoff(tello=1),
call.land(tello=1),
call.takeoff(tello=2),
call.land(tello=2)
]
self.fly.assert_has_calls(calls)
def test_function_procedure_in_function(self):
commands = generate_commands("""
function func(int i) return int {
while i < 10 {
i <- i + 1;
}
return i;
}
procedure proc(int i) {
forward(i * i);
}
function func2(int i) return int {
proc(func(i));
return func(i) * func(i);
}
main () {
takeoff();
forward(func2(1));
land();
}
""")
expected_commands = [SingleDroneCommand("DEFAULT", Command.takeoff()),
SingleDroneCommand("DEFAULT", Command.forward(100)),
SingleDroneCommand("DEFAULT", Command.forward(100)),
SingleDroneCommand("DEFAULT", Command.land())]
self.assertEqual(expected_commands, commands)
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 test_basic_program(self):
commands = "main() {takeoff(); land();}"
expected = [
SingleDroneCommand("DEFAULT", Command.takeoff()),
SingleDroneCommand("DEFAULT", Command.land())
]
self.assertEqual(expected, generate_commands(commands))
def test_add_already_involved_drones_should_give_error(self):
parallel_commands = ParallelDroneCommands()
parallel_commands.add(
[SingleDroneCommand("DRONE1", Command.takeoff())])
with self.assertRaises(RepeatDroneNameException) as context:
parallel_commands.add(
[SingleDroneCommand("DRONE1", Command.takeoff())])
self.assertTrue({"DRONE1"}, context.exception.repeated_names)
def test_immutable(self):
drone_command = SingleDroneCommand("abc", Command.takeoff())
drone_name = drone_command.drone_name
command = drone_command.command
drone_name += "corrupted"
command._opcode = "corrupted"
self.assertEqual("abc", drone_command.drone_name)
self.assertEqual(Command.takeoff(), drone_command.command)
def test_check_bad_collision_config_should_not_give_error(self):
collision_config = CollisionConfig(collision_meters=0.3, time_interval_seconds=0.001)
collision_checker = CollisionChecker(self.drone_config_map, collision_config)
drone_commands = [SingleDroneCommand("DRONE1", Command.takeoff()),
SingleDroneCommand("DRONE1", Command.land())]
with self.assertRaises(SafetyCheckError) as context:
collision_checker.check(drone_commands, self.state_updater_map)
self.assertTrue("Error occurred during collision check, please retry with better the collision_config."
in str(context.exception))
def test_land_should_return_correct_command(self):
actual = generate_commands("""
main() { takeoff(); land(); }
""")
expected = [
SingleDroneCommand("DEFAULT", Command.takeoff()),
SingleDroneCommand("DEFAULT", Command.land())
]
self.assertEqual(expected, actual)
def test_corrupted_command_should_not_affect_correct_type(self):
command = Command.forward(1)
corrupted_command = Command.forward(1)
corrupted_command._opcode = "corrupted"
self.assertNotEqual(command, corrupted_command)
self.assertEqual("forward", Command.forward(1).opcode)
self.assertEqual([1], Command.forward(1).operands)
self.assertEqual("corrupted", corrupted_command.opcode)
self.assertEqual([1], corrupted_command.operands)
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 test_up_with_decimal_parameter_should_return_correct_command(self):
actual = generate_commands("""
main() { takeoff(); up(1.0); land(); }
""")
expected = [
SingleDroneCommand("DEFAULT", Command.takeoff()),
SingleDroneCommand("DEFAULT", Command.up(1.0)),
SingleDroneCommand("DEFAULT", Command.land())
]
self.assertEqual(expected, actual)
def test_backward_with_int_parameter_should_return_correct_command(self):
actual = generate_commands("""
main() { takeoff(); backward(1); land(); }
""")
expected = [
SingleDroneCommand("DEFAULT", Command.takeoff()),
SingleDroneCommand("DEFAULT", Command.backward(1)),
SingleDroneCommand("DEFAULT", Command.land())
]
self.assertEqual(expected, actual)
def test_add(self):
parallel_commands = ParallelDroneCommands()
parallel_commands.add([])
self.assertEqual([[]], parallel_commands.branches)
self.assertEqual([set()],
parallel_commands.drones_involved_each_branch)
parallel_commands.add([SingleDroneCommand("abc", Command.takeoff())])
self.assertEqual([[], [SingleDroneCommand("abc", Command.takeoff())]],
parallel_commands.branches)
self.assertEqual([set(), {"abc"}],
parallel_commands.drones_involved_each_branch)
def test_land_with_drone_name_should_return_correct_command(self):
actual = generate_commands(
"""
main() { DRONE1.takeoff(); DRONE1.land(); }
""",
drone_config_map={"DRONE1": DefaultDroneConfig()})
expected = [
SingleDroneCommand("DRONE1", Command.takeoff()),
SingleDroneCommand("DRONE1", Command.land())
]
self.assertEqual(expected, actual)
def test_check_single_drone_command_should_check_state_and_catch_error(
self):
boundary_checker = BoundaryChecker(self.boundary_config)
boundary_checker.boundary_config.check_state = Mock(
side_effect=SafetyCheckError)
drone_commands = [
SingleDroneCommand("DRONE1", Command.takeoff()),
SingleDroneCommand("DRONE1", Command.land())
]
with self.assertRaises(SafetyCheckError) as context:
BoundaryChecker(self.boundary_config).check(
drone_commands, self.state_updater_map)
def test_movement_with_no_name_specified_if_only_one_drone_should_use_that_drone(
self):
drone_config_map = {"THE_ONE": DefaultDroneConfig()}
actual = generate_commands("""
main() { takeoff(); land(); }
""",
drone_config_map=drone_config_map)
expected = [
SingleDroneCommand("THE_ONE", Command.takeoff()),
SingleDroneCommand("THE_ONE", Command.land())
]
self.assertEqual(expected, actual)
def test_check_single_drone_command_takeoff_when_taken_off_should_give_error(
self):
with self.assertRaises(SafetyCheckError) as context:
drone_commands = [
SingleDroneCommand("DRONE1", Command.takeoff()),
SingleDroneCommand("DRONE1", Command.takeoff())
]
BoundaryChecker(self.boundary_config).check(
drone_commands, self.state_updater_map)
self.assertTrue(
"'takeoff' command used when drone 'DRONE1' has already been taken off"
in str(context.exception))
def test_eq(self):
parallel_commands1 = ParallelDroneCommands()
parallel_commands1.add([])
parallel_commands1.add([SingleDroneCommand("abc", Command.takeoff())])
parallel_commands2 = ParallelDroneCommands(
[[], [SingleDroneCommand("abc", Command.takeoff())]])
parallel_commands3 = ParallelDroneCommands(
[[SingleDroneCommand("abc", Command.takeoff())], []])
self.assertEqual(ParallelDroneCommands(), ParallelDroneCommands())
self.assertEqual(parallel_commands1, parallel_commands2)
self.assertNotEqual(parallel_commands1, parallel_commands3)
self.assertNotEqual(None, parallel_commands1)
def test_get_drones_involved(self):
parallel_commands = ParallelDroneCommands()
self.assertEqual(set(), parallel_commands.get_drones_involved())
parallel_commands = ParallelDroneCommands(
[[SingleDroneCommand("DRONE1", Command.takeoff())],
[SingleDroneCommand("DRONE2", Command.takeoff())],
[
ParallelDroneCommands(
[[SingleDroneCommand("DRONE3", Command.takeoff())],
[SingleDroneCommand("DRONE4", Command.takeoff())]])
]])
self.assertEqual({"DRONE1", "DRONE2", "DRONE3", "DRONE4"},
parallel_commands.get_drones_involved())
def test_if_with_error_commands_not_entering_should_not_give_error(self):
actual_commands = generate_commands("""
main () {
takeoff();
if false {
int a <- "error";
forward(1);
}
land();
}
""")
expected_commands = [SingleDroneCommand("DEFAULT", Command.takeoff()),
SingleDroneCommand("DEFAULT", Command.land())]
self.assertEqual(expected_commands, actual_commands)
def test_to_command_str(self):
parallel_commands = ParallelDroneCommands()
parallel_commands.add([])
parallel_commands.add([
SingleDroneCommand("abc", Command.takeoff()),
SingleDroneCommand("abc", Command.up(1))
])
self.assertEqual("{ } || { abc.takeoff(); abc.up(1); };",
parallel_commands.to_command_str())
outer_parallel_commands = ParallelDroneCommands()
outer_parallel_commands.add([])
outer_parallel_commands.add([parallel_commands])
self.assertEqual("{ } || { { } || { abc.takeoff(); abc.up(1); }; };",
outer_parallel_commands.to_command_str())
def test_repeat_should_run_correct_commands(self):
actual_commands = generate_commands("""
main () {
takeoff();
repeat 4 times {
forward(1);
}
land();
}
""")
expected_commands = [SingleDroneCommand("DEFAULT", Command.takeoff())] + \
[SingleDroneCommand("DEFAULT", Command.forward(1)) for _ in range(4)] + \
[SingleDroneCommand("DEFAULT", Command.land())]
self.assertEqual(expected_commands, actual_commands)
def test_call_procedure_with_parameter_should_shadow_constant(self):
commands = generate_commands("""
procedure proc(drone DRONE2) {
DRONE2.up(1);
}
main () {
DRONE1.takeoff();
proc(DRONE1);
DRONE1.land();
}
""", drone_config_map={"DRONE1": DefaultDroneConfig(), "DRONE2": DefaultDroneConfig()})
expected_commands = [SingleDroneCommand("DRONE1", Command.takeoff()),
SingleDroneCommand("DRONE1", Command.up(1)),
SingleDroneCommand("DRONE1", Command.land())]
self.assertEqual(expected_commands, commands)
def test_update_down_should_update_state(self):
state = State(has_taken_off=True, z_meters=3)
actual = self.state_updater.update(Command.down(1), state)
expected = State(has_taken_off=True,
time_used_seconds=1 / 0.5,
z_meters=3 - 1)
self.assertEqual(expected, actual)
def test_check_in_the_end_not_land_should_give_error(self):
with self.assertRaises(SafetyCheckError) as context:
drone_commands = [SingleDroneCommand("DRONE1", Command.takeoff())]
BoundaryChecker(self.boundary_config).check(
drone_commands, self.state_updater_map)
self.assertTrue(
"Drone 'DRONE1' did not land in the end" in str(context.exception))
