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_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_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_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') ])
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()