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))