def test_execute_nested_parallel_drone_commands(self):
nested_parallel_commands = [
ParallelDroneCommands(
[[
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(nested_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_parallel_with_different_drones_should_give_correct_commands(self):
drone_config_map = {
"DRONE1": DefaultDroneConfig(),
"DRONE2": DefaultDroneConfig(),
"DRONE3": DefaultDroneConfig()
}
actual = generate_commands("""
main() {
{DRONE1.takeoff();} || {DRONE2.takeoff();} || {DRONE3.takeoff();};
{
DRONE1.land();
} || {
{DRONE2.land();} || {DRONE3.land();};
};
}
""",
drone_config_map=drone_config_map)
expected = [
ParallelDroneCommands(
[[SingleDroneCommand("DRONE1", Command.takeoff())],
[SingleDroneCommand("DRONE2", Command.takeoff())],
[SingleDroneCommand("DRONE3", Command.takeoff())]]),
ParallelDroneCommands(
[[SingleDroneCommand("DRONE1", Command.land())],
[
ParallelDroneCommands(
[[SingleDroneCommand("DRONE2", Command.land())],
[SingleDroneCommand("DRONE3", Command.land())]])
]])
]
self.assertEqual(expected, actual)
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 visitParallel(self, ctx: xDroneParser.ParallelContext) -> None:
parallel_commands = ParallelDroneCommands()
for commands in ctx.commands():
self.commands.append([])
# scope - discard updates on existing variables, discard new variables
new_symbol_table = copy.deepcopy(self._get_latest_symbol_table())
self.symbol_table.append(new_symbol_table)
self.returned.append(False)
self.returned_value.append(None)
self.visit(commands)
returned_value = self.returned_value.pop(-1)
self.returned.pop(-1)
self.symbol_table.pop(-1)
if returned_value is not None:
raise CompileError(
"Parallel branch should not return anything, but {} is returned"
.format(returned_value))
branch = self.commands.pop(-1)
try:
parallel_commands.add(branch)
except RepeatDroneNameException as e:
raise CompileError(
"Parallel branches should have exclusive drone names, "
"but {} appeared in more than one branches".format(
e.repeated_names))
self._get_latest_commands().append(parallel_commands)
def test_properties(self):
parallel_commands = ParallelDroneCommands()
self.assertEqual([], parallel_commands.branches)
self.assertEqual([], parallel_commands.drones_involved_each_branch)
parallel_commands = ParallelDroneCommands([[], []])
self.assertEqual([[], []], parallel_commands.branches)
self.assertEqual([set(), set()],
parallel_commands.drones_involved_each_branch)
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 _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_check_parallel_drone_commands_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 = [ParallelDroneCommands([[], []])]
with self.assertRaises(SafetyCheckError) as context:
BoundaryChecker(self.boundary_config).check(
drone_commands, self.state_updater_map)
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_check_parallel_drone_command_should_detect_collision_and_give_error(self):
drone_commands = [SingleDroneCommand("DRONE1", Command.takeoff()),
SingleDroneCommand("DRONE2", Command.takeoff()),
ParallelDroneCommands([
[SingleDroneCommand("DRONE1", Command.right(1))],
[SingleDroneCommand("DRONE2", Command.left(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_repr(self):
parallel_commands = ParallelDroneCommands()
parallel_commands.add([])
parallel_commands.add([SingleDroneCommand("abc", Command.takeoff())])
self.assertEqual(
"ParallelDroneCommands: { [], " +
"[SingleDroneCommand: { drone_name: abc, " +
"command: Command: { opcode: takeoff, operands: [] } }] }",
repr(parallel_commands))
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_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_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_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 _update_states_and_check_parallel_drone_commands(
self, parallel_drone_commands: ParallelDroneCommands,
state_updaters: Dict[str, StateUpdater],
drone_state_map: Dict[str,
State], drones_involved: Set[str]) -> float:
assert len(parallel_drone_commands.branches) > 0
time_used_in_branches = self._update_states_and_check_for_each_branch(
parallel_drone_commands, state_updaters, drone_state_map)
longest_time_used = self._update_states_to_wait_for_slowest_branch(
parallel_drone_commands, drone_state_map, time_used_in_branches,
drones_involved)
for name, state in drone_state_map.items():
try:
self.boundary_config.check_state(name, state)
except SafetyCheckError as e:
raise SafetyCheckError(
"When running command '{}', boundary limits are violated: {}"
.format(parallel_drone_commands.to_command_str(), str(e)))
return longest_time_used
def test_parallel_return_in_branch_should_return_early(self):
drone_config_map = {
"DRONE1": DefaultDroneConfig(),
"DRONE2": DefaultDroneConfig(),
"DRONE3": DefaultDroneConfig()
}
actual = generate_commands("""
main() {
{return; DRONE1.takeoff();} || {DRONE2.takeoff(); DRONE2.land();};
}
""",
drone_config_map=drone_config_map)
expected = [
ParallelDroneCommands(
[[],
[
SingleDroneCommand("DRONE2", Command.takeoff()),
SingleDroneCommand("DRONE2", Command.land())
]])
]
self.assertEqual(expected, actual)
def _update_states_to_wait_for_slowest_branch(
self, parallel_drone_commands: ParallelDroneCommands,
drone_state_map: Dict[str,
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]:
old_state = drone_state_map[name]
drone_state_map[
name] = old_state.copy_and_set_time_used_seconds(
old_state.time_used_seconds + longest_time_used -
time_used)
# let drones not involved in any branch wait for longest_time_used
for name in drones_involved.difference(
parallel_drone_commands.get_drones_involved()):
old_state = drone_state_map[name]
drone_state_map[name] = old_state.copy_and_set_time_used_seconds(
old_state.time_used_seconds + longest_time_used)
return longest_time_used
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_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 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_init_with_repeated_drones_should_give_error(self):
with self.assertRaises(RepeatDroneNameException) as context:
ParallelDroneCommands(
[[SingleDroneCommand("DRONE1", Command.takeoff())],
[SingleDroneCommand("DRONE1", Command.takeoff())]])
self.assertTrue({"DRONE1"}, context.exception.repeated_names)