class TestPositionHlCommander(unittest.TestCase):
def setUp(self):
self.commander_mock = MagicMock(spec=HighLevelCommander)
self.param_mock = MagicMock(spec=Param)
self.cf_mock = MagicMock(spec=Crazyflie)
self.cf_mock.high_level_commander = self.commander_mock
self.cf_mock.param = self.param_mock
self.cf_mock.is_connected.return_value = True
self.sut = PositionHlCommander(self.cf_mock)
def test_that_the_estimator_is_reset_on_take_off(
self, sleep_mock):
# Fixture
sut = PositionHlCommander(self.cf_mock, 1.0, 2.0, 3.0)
# Test
sut.take_off()
# Assert
self.param_mock.set_value.assert_has_calls([
call('kalman.initialX', '{:.2f}'.format(1.0)),
call('kalman.initialY', '{:.2f}'.format(2.0)),
call('kalman.initialZ', '{:.2f}'.format(3.0)),
call('kalman.resetEstimation', '1'),
call('kalman.resetEstimation', '0')
])
def test_that_the_hi_level_commander_is_activated_on_take_off(
self, sleep_mock):
# Fixture
# Test
self.sut.take_off()
# Assert
self.param_mock.set_value.assert_has_calls([
call('commander.enHighLevel', '1')
])
def test_that_controller_is_selected_on_take_off(
self, sleep_mock):
# Fixture
self.sut.set_controller(PositionHlCommander.CONTROLLER_MELLINGER)
# Test
self.sut.take_off()
# Assert
self.param_mock.set_value.assert_has_calls([
call('stabilizer.controller', '2')
])
def test_that_take_off_raises_exception_if_not_connected(
self, sleep_mock):
# Fixture
self.cf_mock.is_connected.return_value = False
# Test
# Assert
with self.assertRaises(Exception):
self.sut.take_off()
def test_that_take_off_raises_exception_when_already_flying(
self, sleep_mock):
# Fixture
self.sut.take_off()
# Test
# Assert
with self.assertRaises(Exception):
self.sut.take_off()
def test_that_it_goes_up_on_take_off(
self, sleep_mock):
# Fixture
# Test
self.sut.take_off(height=0.4, velocity=0.6)
# Assert
duration = 0.4 / 0.6
self.commander_mock.takeoff.assert_called_with(0.4, duration)
sleep_mock.assert_called_with(duration)
def test_that_it_goes_up_to_default_height(
self, sleep_mock):
# Fixture
sut = PositionHlCommander(self.cf_mock, default_height=0.4)
# Test
sut.take_off(velocity=0.6)
# Assert
duration = 0.4 / 0.6
self.commander_mock.takeoff.assert_called_with(0.4, duration)
sleep_mock.assert_called_with(duration)
def test_that_it_goes_down_on_landing(
self, sleep_mock):
# Fixture
self.sut.take_off(height=0.4)
# Test
self.sut.land(velocity=0.6)
# Assert
duration = 0.4 / 0.6
self.commander_mock.land.assert_called_with(0.0, duration)
sleep_mock.assert_called_with(duration)
def test_that_it_takes_off_and_lands_as_context_manager(
self, sleep_mock):
# Fixture
# Test
with self.sut:
pass
# Assert
duration1 = 0.5 / 0.5
duration2 = 0.5 / 0.5
self.commander_mock.takeoff.assert_called_with(0.5, duration1)
self.commander_mock.land.assert_called_with(0.0, duration2)
sleep_mock.assert_called_with(duration1)
sleep_mock.assert_called_with(duration2)
def test_that_it_returns_current_position(
self, sleep_mock):
# Fixture
self.sut.take_off(height=0.4, velocity=0.6)
# Test
actual = self.sut.get_position()
# Assert
self.assertEqual(actual, (0.0, 0.0, 0.4))
def test_that_it_goes_to_position(
self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
# Test
self.sut.go_to(1.0, 2.0, 3.0, 4.0)
# Assert
distance = self._distance(inital_pos, (1.0, 2.0, 3.0))
duration = distance / 4.0
self.commander_mock.go_to.assert_called_with(
1.0, 2.0, 3.0, 0.0, duration)
sleep_mock.assert_called_with(duration)
def test_that_it_moves_distance(
self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
# Test
self.sut.move_distance(1.0, 2.0, 3.0, 4.0)
# Assert
distance = self._distance((0.0, 0.0, 0.0), (1.0, 2.0, 3.0))
duration = distance / 4.0
final_pos = (
inital_pos[0] + 1.0,
inital_pos[1] + 2.0,
inital_pos[2] + 3.0)
self.commander_mock.go_to.assert_called_with(
final_pos[0], final_pos[1], final_pos[2], 0.0, duration)
sleep_mock.assert_called_with(duration)
def test_that_it_goes_forward(
self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
# Test
self.sut.forward(1.0, 2.0)
# Assert
duration = 1.0 / 2.0
final_pos = (
inital_pos[0] + 1.0,
inital_pos[1],
inital_pos[2])
self.commander_mock.go_to.assert_called_with(
final_pos[0], final_pos[1], final_pos[2], 0.0, duration)
sleep_mock.assert_called_with(duration)
def test_that_it_goes_back(
self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
# Test
self.sut.back(1.0, 2.0)
# Assert
duration = 1.0 / 2.0
final_pos = (
inital_pos[0] - 1.0,
inital_pos[1],
inital_pos[2])
self.commander_mock.go_to.assert_called_with(
final_pos[0], final_pos[1], final_pos[2], 0.0, duration)
sleep_mock.assert_called_with(duration)
def test_that_it_goes_left(
self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
# Test
self.sut.left(1.0, 2.0)
# Assert
duration = 1.0 / 2.0
final_pos = (
inital_pos[0],
inital_pos[1] + 1.0,
inital_pos[2])
self.commander_mock.go_to.assert_called_with(
final_pos[0], final_pos[1], final_pos[2], 0.0, duration)
sleep_mock.assert_called_with(duration)
def test_that_it_goes_right(
self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
# Test
self.sut.right(1.0, 2.0)
# Assert
duration = 1.0 / 2.0
final_pos = (
inital_pos[0],
inital_pos[1] - 1,
inital_pos[2])
self.commander_mock.go_to.assert_called_with(
final_pos[0], final_pos[1], final_pos[2], 0, duration)
sleep_mock.assert_called_with(duration)
def test_that_it_goes_up(
self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
# Test
self.sut.up(1.0, 2.0)
# Assert
duration = 1.0 / 2.0
final_pos = (
inital_pos[0],
inital_pos[1],
inital_pos[2] + 1)
self.commander_mock.go_to.assert_called_with(
final_pos[0], final_pos[1], final_pos[2], 0, duration)
sleep_mock.assert_called_with(duration)
def test_that_it_goes_down(
self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
# Test
self.sut.down(1.0, 2.0)
# Assert
duration = 1.0 / 2.0
final_pos = (
inital_pos[0],
inital_pos[1],
inital_pos[2] - 1)
self.commander_mock.go_to.assert_called_with(
final_pos[0], final_pos[1], final_pos[2], 0, duration)
sleep_mock.assert_called_with(duration)
def test_that_default_velocity_is_used(
self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
self.sut.set_default_velocity(7)
# Test
self.sut.go_to(1.0, 2.0, 3.0)
# Assert
distance = self._distance(inital_pos, (1.0, 2.0, 3.0))
duration = distance / 7.0
self.commander_mock.go_to.assert_called_with(
1.0, 2.0, 3.0, 0.0, duration)
sleep_mock.assert_called_with(duration)
def test_that_default_height_is_used(
self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
self.sut.set_default_velocity(7.0)
self.sut.set_default_height(5.0)
# Test
self.sut.go_to(1.0, 2.0)
# Assert
distance = self._distance(inital_pos, (1.0, 2.0, 5.0))
duration = distance / 7.0
self.commander_mock.go_to.assert_called_with(
1.0, 2.0, 5.0, 0.0, duration)
sleep_mock.assert_called_with(duration)
######################################################################
def _distance(self, p1, p2):
dx = p1[0] - p2[0]
dy = p1[1] - p2[1]
dz = p1[2] - p2[2]
return math.sqrt(dx * dx + dy * dy + dz * dz)
class TestPositionHlCommander(unittest.TestCase):
def setUp(self):
self.commander_mock = MagicMock(spec=HighLevelCommander)
self.param_mock = MagicMock(spec=Param)
self.cf_mock = MagicMock(spec=Crazyflie)
self.cf_mock.high_level_commander = self.commander_mock
self.cf_mock.param = self.param_mock
self.cf_mock.is_connected.return_value = True
self.sut = PositionHlCommander(self.cf_mock)
def test_that_the_estimator_is_reset_on_take_off(self, sleep_mock):
# Fixture
sut = PositionHlCommander(self.cf_mock, 1.0, 2.0, 3.0)
# Test
sut.take_off()
# Assert
self.param_mock.set_value.assert_has_calls([
call('kalman.initialX', '{:.2f}'.format(1.0)),
call('kalman.initialY', '{:.2f}'.format(2.0)),
call('kalman.initialZ', '{:.2f}'.format(3.0)),
call('kalman.resetEstimation', '1'),
call('kalman.resetEstimation', '0')
])
def test_that_the_hi_level_commander_is_activated_on_take_off(
self, sleep_mock):
# Fixture
# Test
self.sut.take_off()
# Assert
self.param_mock.set_value.assert_has_calls(
[call('commander.enHighLevel', '1')])
def test_that_controller_is_selected_on_take_off(self, sleep_mock):
# Fixture
self.sut.set_controller(PositionHlCommander.CONTROLLER_MELLINGER)
# Test
self.sut.take_off()
# Assert
self.param_mock.set_value.assert_has_calls(
[call('stabilizer.controller', '2')])
def test_that_take_off_raises_exception_if_not_connected(self, sleep_mock):
# Fixture
self.cf_mock.is_connected.return_value = False
# Test
# Assert
with self.assertRaises(Exception):
self.sut.take_off()
def test_that_take_off_raises_exception_when_already_flying(
self, sleep_mock):
# Fixture
self.sut.take_off()
# Test
# Assert
with self.assertRaises(Exception):
self.sut.take_off()
def test_that_it_goes_up_on_take_off(self, sleep_mock):
# Fixture
# Test
self.sut.take_off(height=0.4, velocity=0.6)
# Assert
duration = 0.4 / 0.6
self.commander_mock.takeoff.assert_called_with(0.4, duration)
sleep_mock.assert_called_with(duration)
def test_that_it_goes_up_to_default_height(self, sleep_mock):
# Fixture
sut = PositionHlCommander(self.cf_mock, default_height=0.4)
# Test
sut.take_off(velocity=0.6)
# Assert
duration = 0.4 / 0.6
self.commander_mock.takeoff.assert_called_with(0.4, duration)
sleep_mock.assert_called_with(duration)
def test_that_it_goes_down_on_landing(self, sleep_mock):
# Fixture
self.sut.take_off(height=0.4)
# Test
self.sut.land(velocity=0.6)
# Assert
duration = 0.4 / 0.6
self.commander_mock.land.assert_called_with(0.0, duration)
sleep_mock.assert_called_with(duration)
def test_that_it_takes_off_and_lands_as_context_manager(self, sleep_mock):
# Fixture
# Test
with self.sut:
pass
# Assert
duration1 = 0.5 / 0.5
duration2 = 0.5 / 0.5
self.commander_mock.takeoff.assert_called_with(0.5, duration1)
self.commander_mock.land.assert_called_with(0.0, duration2)
sleep_mock.assert_called_with(duration1)
sleep_mock.assert_called_with(duration2)
def test_that_it_returns_current_position(self, sleep_mock):
# Fixture
self.sut.take_off(height=0.4, velocity=0.6)
# Test
actual = self.sut.get_position()
# Assert
self.assertEqual(actual, (0.0, 0.0, 0.4))
def test_that_it_goes_to_position(self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
# Test
self.sut.go_to(1.0, 2.0, 3.0, 4.0)
# Assert
distance = self._distance(inital_pos, (1.0, 2.0, 3.0))
duration = distance / 4.0
self.commander_mock.go_to.assert_called_with(1.0, 2.0, 3.0, 0.0,
duration)
sleep_mock.assert_called_with(duration)
def test_that_it_moves_distance(self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
# Test
self.sut.move_distance(1.0, 2.0, 3.0, 4.0)
# Assert
distance = self._distance((0.0, 0.0, 0.0), (1.0, 2.0, 3.0))
duration = distance / 4.0
final_pos = (inital_pos[0] + 1.0, inital_pos[1] + 2.0,
inital_pos[2] + 3.0)
self.commander_mock.go_to.assert_called_with(final_pos[0],
final_pos[1],
final_pos[2], 0.0,
duration)
sleep_mock.assert_called_with(duration)
def test_that_it_goes_forward(self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
# Test
self.sut.forward(1.0, 2.0)
# Assert
duration = 1.0 / 2.0
final_pos = (inital_pos[0] + 1.0, inital_pos[1], inital_pos[2])
self.commander_mock.go_to.assert_called_with(final_pos[0],
final_pos[1],
final_pos[2], 0.0,
duration)
sleep_mock.assert_called_with(duration)
def test_that_it_goes_back(self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
# Test
self.sut.back(1.0, 2.0)
# Assert
duration = 1.0 / 2.0
final_pos = (inital_pos[0] - 1.0, inital_pos[1], inital_pos[2])
self.commander_mock.go_to.assert_called_with(final_pos[0],
final_pos[1],
final_pos[2], 0.0,
duration)
sleep_mock.assert_called_with(duration)
def test_that_it_goes_left(self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
# Test
self.sut.left(1.0, 2.0)
# Assert
duration = 1.0 / 2.0
final_pos = (inital_pos[0], inital_pos[1] + 1.0, inital_pos[2])
self.commander_mock.go_to.assert_called_with(final_pos[0],
final_pos[1],
final_pos[2], 0.0,
duration)
sleep_mock.assert_called_with(duration)
def test_that_it_goes_right(self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
# Test
self.sut.right(1.0, 2.0)
# Assert
duration = 1.0 / 2.0
final_pos = (inital_pos[0], inital_pos[1] - 1, inital_pos[2])
self.commander_mock.go_to.assert_called_with(final_pos[0],
final_pos[1],
final_pos[2], 0, duration)
sleep_mock.assert_called_with(duration)
def test_that_it_goes_up(self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
# Test
self.sut.up(1.0, 2.0)
# Assert
duration = 1.0 / 2.0
final_pos = (inital_pos[0], inital_pos[1], inital_pos[2] + 1)
self.commander_mock.go_to.assert_called_with(final_pos[0],
final_pos[1],
final_pos[2], 0, duration)
sleep_mock.assert_called_with(duration)
def test_that_it_goes_down(self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
# Test
self.sut.down(1.0, 2.0)
# Assert
duration = 1.0 / 2.0
final_pos = (inital_pos[0], inital_pos[1], inital_pos[2] - 1)
self.commander_mock.go_to.assert_called_with(final_pos[0],
final_pos[1],
final_pos[2], 0, duration)
sleep_mock.assert_called_with(duration)
def test_that_default_velocity_is_used(self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
self.sut.set_default_velocity(7)
# Test
self.sut.go_to(1.0, 2.0, 3.0)
# Assert
distance = self._distance(inital_pos, (1.0, 2.0, 3.0))
duration = distance / 7.0
self.commander_mock.go_to.assert_called_with(1.0, 2.0, 3.0, 0.0,
duration)
sleep_mock.assert_called_with(duration)
def test_that_default_height_is_used(self, sleep_mock):
# Fixture
self.sut.take_off()
inital_pos = self.sut.get_position()
self.sut.set_default_velocity(7.0)
self.sut.set_default_height(5.0)
# Test
self.sut.go_to(1.0, 2.0)
# Assert
distance = self._distance(inital_pos, (1.0, 2.0, 5.0))
duration = distance / 7.0
self.commander_mock.go_to.assert_called_with(1.0, 2.0, 5.0, 0.0,
duration)
sleep_mock.assert_called_with(duration)
######################################################################
def _distance(self, p1, p2):
dx = p1[0] - p2[0]
dy = p1[1] - p2[1]
dz = p1[2] - p2[2]
return math.sqrt(dx * dx + dy * dy + dz * dz)
pc.go_to(0.0, 0.0)
# Go slowly to a coordinate
pc.go_to(1.0, 1.0, velocity=0.2)
# Set new default velocity and height
pc.set_default_velocity(0.3)
pc.set_default_height(1.0)
pc.go_to(0.0, 0.0)
def simple_sequence():
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
with PositionHlCommander(scf) as pc:
pc.forward(1.0)
pc.left(1.0)
pc.back(1.0)
pc.go_to(0.0, 0.0, 1.0)
if __name__ == '__main__':
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
pc = PositionHlCommander(scf)
pc.take_off()
time.sleep(1)
pc.land()
# simple_sequence()
# slightly_more_complex_usage()