def _update_takeoff(self, command: Command, state: State) -> State:
assert command.opcode == "takeoff"
state = state.copy_and_set_has_taken_off(True)
state = state.copy_and_set_time_used_seconds(
state.time_used_seconds + self.drone_config.takeoff_height_meters / self.drone_config.speed_mps)
state = state.copy_and_set_z_meters(self.drone_config.takeoff_height_meters)
return state
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_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 _update_rotate_right(self, command: Command, state: State) -> State:
assert command.opcode == "rotate_right"
rotate_right_degrees, = command.operands
state = state.copy_and_set_time_used_seconds(
state.time_used_seconds + rotate_right_degrees / self.drone_config.rotate_speed_dps)
state = state.copy_and_set_orientation_degrees((state.orientation_degrees + rotate_right_degrees) % 360)
return state
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_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_update_land_should_update_state(self):
state = State(has_taken_off=True, z_meters=3)
actual = self.state_updater.update(Command.land(), state)
expected = State(has_taken_off=False,
time_used_seconds=3 / 0.5,
z_meters=0)
self.assertEqual(expected, actual)
def _update_down(self, command: Command, state: State) -> State:
assert command.opcode == "down"
down_meters, = command.operands
state = state.copy_and_set_time_used_seconds(
state.time_used_seconds + down_meters / self.drone_config.speed_mps)
state = state.copy_and_set_z_meters(state.z_meters - down_meters)
return state
def _update_land(self, command: Command, state: State) -> State:
assert command.opcode == "land"
state = state.copy_and_set_has_taken_off(False)
state = state.copy_and_set_time_used_seconds(
state.time_used_seconds + state.z_meters / self.drone_config.speed_mps)
state = state.copy_and_set_z_meters(0)
return state
def test_update_takeoff_should_update_state(self):
state = State()
actual = self.state_updater.update(Command.takeoff(), state)
expected = State(
has_taken_off=True,
time_used_seconds=self.drone_config.takeoff_height_meters / 0.5,
z_meters=self.drone_config.takeoff_height_meters)
self.assertEqual(expected, actual)
def _update_backward(self, command: Command, state: State) -> State:
assert command.opcode == "backward"
backward_meters, = command.operands
move_direction = state.orientation_degrees + 180
state = state.copy_and_set_time_used_seconds(
state.time_used_seconds + backward_meters / self.drone_config.speed_mps)
state = state.copy_and_set_x_meters(state.x_meters + backward_meters * sin(radians(move_direction)))
state = state.copy_and_set_y_meters(state.y_meters + backward_meters * cos(radians(move_direction)))
return state
def test_check_should_call_update_states_and_check_drone_commands_with_correct_parameters(
self):
boundary_checker = BoundaryChecker(self.boundary_config)
boundary_checker._update_states_and_check_drone_commands = Mock()
drone_state_map = {"DRONE1": State(), "DRONE2": State(x_meters=1)}
drones_involved = {"DRONE1", "DRONE2"}
boundary_checker.check([], self.state_updater_map)
boundary_checker._update_states_and_check_drone_commands.assert_called_once_with(
[], self.state_updater_map, drone_state_map, drones_involved)
def test_get_init_state(self):
self.assertEqual(State(x_meters=0, y_meters=0, z_meters=0),
self.state_updater.get_init_state())
drone_config = DroneConfig(init_position=(1, 2, 3),
speed_mps=0.5,
rotate_speed_dps=45,
takeoff_height_meters=1)
state_updater = StateUpdater(drone_config=drone_config)
self.assertEqual(State(x_meters=1, y_meters=2, z_meters=3),
state_updater.get_init_state())
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_update_state_correctly(self):
drone_commands = [SingleDroneCommand("DRONE1", Command.takeoff())]
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=1,
z_meters=1),
"DRONE2": State(time_used_seconds=1, x_meters=1)
}
self.assertEqual(expected, drone_state_map)
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_check_single_drone_command_should_check_state(self):
boundary_checker = BoundaryChecker(self.boundary_config)
boundary_checker.boundary_config.check_state = Mock()
drone_commands = [
SingleDroneCommand("DRONE1", Command.takeoff()),
SingleDroneCommand("DRONE1", Command.land())
]
BoundaryChecker(self.boundary_config).check(drone_commands,
self.state_updater_map)
calls = [
call("DRONE1",
State(has_taken_off=True, time_used_seconds=1, z_meters=1)),
call("DRONE2", State(time_used_seconds=1, x_meters=1)),
call("DRONE1", State(time_used_seconds=2)),
call("DRONE2", State(time_used_seconds=2, x_meters=1))
]
boundary_checker.boundary_config.check_state.assert_has_calls(calls)
def test_init_state(self):
state = State()
self.assertEqual(False, state.has_taken_off)
self.assertEqual(0, state.time_used_seconds)
self.assertEqual(0, state.x_meters)
self.assertEqual(0, state.y_meters)
self.assertEqual(0, state.z_meters)
self.assertEqual(0, state.orientation_degrees)
def test_property(self):
state = State(has_taken_off=True, time_used_seconds=1,
x_meters=1, y_meters=1, z_meters=1, orientation_degrees=1)
self.assertEqual(True, state.has_taken_off)
self.assertEqual(1, state.time_used_seconds)
self.assertEqual(1, state.x_meters)
self.assertEqual(1, state.y_meters)
self.assertEqual(1, state.z_meters)
self.assertEqual(1, state.orientation_degrees)
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_check_parallel_drone_commands_should_check_state(self):
boundary_checker = BoundaryChecker(self.boundary_config)
boundary_checker.boundary_config.check_state = Mock()
drone_commands = [
ParallelDroneCommands(
[[
SingleDroneCommand("DRONE1", Command.takeoff()),
SingleDroneCommand("DRONE1", Command.land())
],
[
SingleDroneCommand("DRONE2", Command.takeoff()),
SingleDroneCommand("DRONE2", Command.land())
]])
]
BoundaryChecker(self.boundary_config).check(drone_commands,
self.state_updater_map)
last_two_calls = [
call("DRONE1", State(time_used_seconds=2)),
call("DRONE2", State(time_used_seconds=2, x_meters=1))
]
boundary_checker.boundary_config.check_state.assert_has_calls(
last_two_calls)
def test_check_state_if_no_limit_should_not_give_error(self):
boundary_config = BoundaryConfig()
for number in [1e-5, 1e-3, 1e0, 1e3, 1e5, 1e10, float('inf')]:
boundary_config.check_state('default', State(x_meters=number))
boundary_config.check_state('default', State(y_meters=number))
boundary_config.check_state('default', State(z_meters=number))
boundary_config.check_state('default', State(x_meters=-number))
boundary_config.check_state('default', State(y_meters=-number))
boundary_config.check_state('default', State(z_meters=-number))
boundary_config.check_state('default',
State(time_used_seconds=number))
def test_check_state_if_beyond_limit_should_give_error(self):
boundary_config = BoundaryConfig(max_seconds=0,
max_x_meters=0,
max_y_meters=0,
max_z_meters=0,
min_x_meters=0,
min_y_meters=0,
min_z_meters=0)
with self.assertRaises(SafetyCheckError) as context:
boundary_config.check_state('default', State(x_meters=10))
self.assertTrue(
"Drone 'default': the x coordinate 10 will go beyond its upper limit 0"
in str(context.exception))
with self.assertRaises(SafetyCheckError) as context:
boundary_config.check_state('default', State(y_meters=10))
self.assertTrue(
"Drone 'default': the y coordinate 10 will go beyond its upper limit 0"
in str(context.exception))
with self.assertRaises(SafetyCheckError) as context:
boundary_config.check_state('default', State(z_meters=10))
self.assertTrue(
"Drone 'default': the z coordinate 10 will go beyond its upper limit 0"
in str(context.exception))
with self.assertRaises(SafetyCheckError) as context:
boundary_config.check_state('default', State(x_meters=-10))
self.assertTrue(
"Drone 'default': the x coordinate -10 will go beyond its lower limit 0"
in str(context.exception))
with self.assertRaises(SafetyCheckError) as context:
boundary_config.check_state('default', State(y_meters=-10))
self.assertTrue(
"Drone 'default': the y coordinate -10 will go beyond its lower limit 0"
in str(context.exception))
with self.assertRaises(SafetyCheckError) as context:
boundary_config.check_state('default', State(z_meters=-10))
self.assertTrue(
"Drone 'default': the z coordinate -10 will go beyond its lower limit 0"
in str(context.exception))
with self.assertRaises(SafetyCheckError) as context:
boundary_config.check_state('default', State(time_used_seconds=10))
self.assertTrue(
"Drone 'default': the time used 10 seconds will go beyond the time limit 0 seconds"
in str(context.exception))
def test_eq(self):
self.assertEqual(State(), State())
self.assertNotEqual(State(time_used_seconds=False), State(has_taken_off=True))
self.assertNotEqual(State(time_used_seconds=0), State(time_used_seconds=1))
self.assertNotEqual(State(x_meters=0), State(x_meters=1))
self.assertNotEqual(State(y_meters=0), State(y_meters=1))
self.assertNotEqual(State(z_meters=0), State(z_meters=1))
self.assertNotEqual(State(orientation_degrees=0), State(orientation_degrees=1))
self.assertNotEqual(None, State())
def test_str(self):
state = State()
self.assertEqual("State: { has_taken_off: False, time_used_seconds: 0, " +
"x_meters: 0, y_meters: 0, z_meters: 0, orientation_degrees: 0 }",
str(state))
def test_copy_and_set_orientation_degrees(self):
state = State()
actual = state.copy_and_set_orientation_degrees(1)
expected = State(has_taken_off=False, time_used_seconds=0,
x_meters=0, y_meters=0, z_meters=0, orientation_degrees=1)
self.assertEqual(expected, actual)
def _update_wait(self, command: Command, state: State) -> State:
assert command.opcode == "wait"
wait_seconds, = command.operands
state = state.copy_and_set_time_used_seconds(state.time_used_seconds + wait_seconds)
return state
def get_init_state(self) -> State:
(x, y, z) = self.drone_config.init_position
return State(x_meters=x, y_meters=y, z_meters=z)