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_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 square(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander( scf, x=0, y=0, z=0, default_velocity=0.3, default_height=0.2, controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc: print(pc.get_position()) pc.up(.5) print(pc.get_position()) pc.right(.5) print(pc.get_position()) pc.forward(.5) print(pc.get_position()) pc.left(1) print(pc.get_position()) pc.back(1) print(pc.get_position()) pc.right(1) print(pc.get_position()) pc.forward(.5) print(pc.get_position()) pc.left(.5) print(pc.get_position()) pc.down(.5) print(pc.get_position())
def line_sequence(velocity): global pos_fixed #先记录固定点的坐标用于计算距离 with SyncCrazyflie(uri_fixed, cf=Crazyflie(rw_cache='./cache')) as scf: lpos = LogConfig(name='position', period_in_ms=100) lpos.add_variable('stateEstimate.x') lpos.add_variable('stateEstimate.y') lpos.add_variable('stateEstimate.z') scf.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(pos_data_fixed) lpos.start() time.sleep(2) lpos.stop() pos_fixed = np.mean(fixed_dt, axis=0).tolist() print(pos_fixed) with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: lpos = LogConfig(name='position', period_in_ms=100) lpos.add_variable('stateEstimate.x') lpos.add_variable('stateEstimate.y') lpos.add_variable('stateEstimate.z') lpos.add_variable(distance_mapper[uri_fixed]) scf.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(pos_data) with PositionHlCommander(scf, default_velocity=velocity) as pc: time.sleep(2) pc.go_to(2.0, 0.0, 0.5) lpos.start() pc.go_to(-0.5, 0, 0.5) lpos.stop() df = pd.DataFrame( moving_dt, columns=['x', 'y', 'z', 'distance_UWB', 'distance_lighthouse']) df.to_csv('./antenna_' + str(velocity) + '_154.65_8' + '.csv', index=False)
def move_baduanjin_hl_p1(scf, event2): with PositionHlCommander( scf, x=1.4, y=2.2, z=0.0, default_velocity=0.5, default_height=0.7, # default = 0.7 controller=PositionHlCommander.CONTROLLER_PID) as pc: print('Setting position [1.4, 2.2, {}]'.format(DEFAULT_HEIGHT)) t_init = time.time() time.sleep(1) ## Go up: d_fly meter/6 sec print('Setting position [1.4, 2.2, {}]'.format(d_abs)) pc.up(d_fly, velocity=d_fly / 6) t1 = time.time() - t_init print("t1: ", t1) ## Delay 4 sec time.sleep(4) t2 = time.time() - t_init print("t2: ", t2) ## Go down: d_fly meter/6 sec print('Setting position [1.4, 2.2, {}]'.format(DEFAULT_HEIGHT)) pc.down(d_fly, velocity=d_fly / 6) t3 = time.time() - t_init print("t3: ", t3) time.sleep(1) # setting the event for turning off the sound feedback process event2.set()
def simple_sequence(): with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: scf.cf.param.add_update_callback(group="deck", name="bcLighthouse4", cb=param_deck_flow) time.sleep(1) logconf = LogConfig(name='Parameters', period_in_ms=SAMPLE_PERIOD_MS) for param in log_parameters: logconf.add_variable(param[0], param[1]) scf.cf.log.add_config(logconf) logconf.data_received_cb.add_callback(logconf_callback) if not is_deck_attached: return logconf.start() with PositionHlCommander( scf, x=0.0, y=0.0, z=0.0, default_velocity=VELOCITY, default_height=DEFAULT_HEIGHT, controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc: for position in SEQUENCE: pc.go_to(position[0], position[1]) time.sleep(5) logconf.stop() write_log_history()
def move_baduanjin_hl_p1(scf): with PositionHlCommander( scf, x=1.2, y=1.7, z=0.0, default_velocity=0.2, default_height=DEFAULT_HEIGHT, controller=PositionHlCommander.CONTROLLER_PID) as pc: print('Setting position [2.2, 1.7, {}]'.format(DEFAULT_HEIGHT)) t_init = time.time() time.sleep(1) ## Go up: d_fly meter/6 sec print('Setting position [2.2, 1.7, {}]'.format(d_abs)) pc.up(d_fly, velocity=d_fly/6) t1 = time.time() - t_init print("t1: ", t1) ## Delay 4 sec time.sleep(4) t2 = time.time() - t_init print("t2: ", t2) ## Go down: d_fly meter/6 sec print('Setting position [2.2, 1.7, {}]'.format(DEFAULT_HEIGHT)) pc.down(d_fly, velocity=d_fly/6) t3 = time.time() - t_init print("t3: ", t3) time.sleep(1)
def slightly_more_complex_usage(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander( scf, x=0.0, y=0.0, z=0.0, default_velocity=0.3, default_height=0.5, controller=PositionHlCommander.CONTROLLER_PID) as pc: # Go to a coordinate pc.go_to(1.0, 1.0, 1.0) # Move relative to the current position pc.right(1.0) # Go to a coordinate and use default height 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)
def connected(self, linkURI): # MOTOR & THRUST lg = LogConfig("Motors", Config().get("ui_update_period")) lg.add_variable("stabilizer.thrust", "uint16_t") lg.add_variable("motor.m1") lg.add_variable("motor.m2") lg.add_variable("motor.m3") lg.add_variable("motor.m4") lg.add_variable("sys.canfly") self._hlCommander = PositionHlCommander( self.helper.cf, x=0.0, y=0.0, z=0.0, default_velocity=0.3, default_height=0.5, controller=PositionHlCommander.CONTROLLER_PID ) try: self.helper.cf.log.add_config(lg) lg.data_received_cb.add_callback(self._log_data_signal.emit) lg.error_cb.add_callback(self._log_error_signal.emit) lg.start() except KeyError as e: logger.warning(str(e)) except AttributeError as e: logger.warning(str(e))
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 simple_sequence(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander(scf) as pc: with Multiranger(scf) as multiranger: for i in xrange(4): pc.forward(1.0) pc.left(1.0) pc.back(1.0) pc.right(1.0)
def land_on_elevated_surface(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander(scf, default_height=0.5, default_velocity=0.2, default_landing_height=0.35) as pc: # fly onto a landing platform at non-zero height (ex: from floor to desk, etc) pc.forward(1.0) pc.left(1.0)
def test(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander( scf, x=0, y=0, z=0, default_velocity=0.3, default_height=0.2, controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc: pc.go_to(-1.3,-1.8,0.3)
def test_that_controller_is_selected_on_creation(self, sleep_mock): # Fixture # Test PositionHlCommander( self.cf_mock, controller=PositionHlCommander.CONTROLLER_MELLINGER) # Assert self.param_mock.set_value.assert_has_calls( [call('stabilizer.controller', '2')])
def simple_sequence(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander(scf) as pc: # pc.forward(1.0) # pc.left(0.5) # pc.back(1.0) pc.go_to(-0.5, 0.0, 0.5) time.sleep(5) pc.go_to(3.0,0.0,0.5) time.sleep(2)
def run_sequence(scf, sequence): try: cf = scf.cf lpos = LogConfig(name='position', period_in_ms=60) lpos.add_variable('stateEstimate.x') lpos.add_variable('stateEstimate.y') lpos.add_variable('stateEstimate.z') # 配置 if scf.cf.link_uri == URI: lpos.add_variable(distance_mapper[URI_fixed]) else: pass scf.cf.log.add_config(lpos) # 回调 if scf.cf.link_uri == URI: lpos.data_received_cb.add_callback(pos_data) else: lpos.data_received_cb.add_callback(pos_data_fixed) # lpos.start() # time.sleep(500) # lpos.stop() with PositionHlCommander(scf) as pc: if scf.cf.link_uri == URI: # 等待二号无人机飞到指定地点后开始做事情 pc.go_to(sequence[0][0], sequence[0][1], sequence[0][2]) time.sleep(2) for i in range(cyc): for idx, ele in enumerate(sequence): if idx == 0: pass elif idx == 1: pc.set_default_velocity(0.5) pc.go_to(ele[0], ele[1], ele[2]) else: pc.set_default_velocity(vel) lpos.start() pc.go_to(ele[0], ele[1], ele[2]) lpos.stop() else: for ele in sequence: pc.go_to(ele[0], ele[1], ele[2]) time.sleep(ele[3]) if scf.cf.link_uri == URI: df = pd.DataFrame( moving_dt, columns=['x', 'y', 'z', 'distance_UWB', 'distance_lighthouse']) df.to_csv('./VelocitySwarm_50ms_' + str(vel) + '_far.csv', index=False) except Exception as e: print(e)
def simple_sequence(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander(scf) as pc: pc.forward(1.0) # pc.left(2.0) pc.forward(1.5) # pc.back(1.0) pc.go_to(1.5, 3.0, 1.0) pc.down(0.5) time.sleep(0.1) # wait for testing
def run_sequence(scf, sequence): try: cf = scf.cf cf.param.set_value('flightmode.posSet', '1') g=PositionHlCommander(cf,cf._x,cf._y,cf._z) # g=PositionHlCommander(cf,kalman.stateX,kalman.stateY,kalman.stateZ) take_off(cf, sequence[0]) for position in sequence: # pc.__init__(cf,cf._x,cf._y,cf._z) # pc.__init__(kalman.stateX,kalman.stateY,kalman.stateZ) print('Setting position {}'.format(position)) x=position[0]-cf._x y=position[1]-cf._y z=position[2]-cf._z velocity=math.sqrt(x*x+y*y+z*z)/position[3] g.go_to(cf,position[0], position[1], position[2],velocity) time.sleep(0.1) land(cf, sequence[-1]) except Exception as e: print(e)
def slightly_more_complex_usage(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander( scf, x=1.0, y=1.0, z=0.0, default_velocity=0.3, default_height=0.5, controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc: # Go to a coordinate pc.go_to(2.0, 2.0, 1) time.sleep(2)
def room_border(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: start_position_printing(scf) with PositionHlCommander( scf, x=0, y=0, z=0, default_velocity=0.3, default_height=0.2, controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc: height = 1 pc.go_to(-1.3,-1.8,height) pc.go_to(1.3,-1.8,height) pc.go_to(1.3,1.8,height) pc.go_to(-1.3,1.8,height) pc.go_to(-1.3,-1.8,height)
def complex_flight(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander( scf, x=1.0, y=1.0, z=0.0, default_velocity=0.3, default_height=0.5, controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc: pc.set_default_velocity(0.3) pc.set_default_height(1.0) #pc.go_to(1.0, 1.0) pc.go_to(3.0, 1.0) pc.go_to(2.0, 3.0) pc.go_to(1.0, 1.0)
def rectangle_sequence(): tfm = CoordTransform() center_floor = tfm.transformF2F(0.5, 0.5, 0) with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander( scf, x=center_floor[0], y=center_floor[1], z=center_floor[2], default_velocity=0.3, default_height=0.5, controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc: fastSpeed = .5 # not more than 1.4! pc.go_to(*tfm.transformF2F(0.5, 0.5, 1)) pc.go_to(*tfm.transformF2F(0, 0, 1), fastSpeed) pc.go_to(*tfm.transformF2F(1, 0, 1), fastSpeed) pc.go_to(*tfm.transformF2F(1, 1, 1), fastSpeed) pc.go_to(*tfm.transformF2F(0, 1, 1), fastSpeed) pc.go_to(*tfm.transformF2F(0, 0, 1), fastSpeed) pc.go_to(*tfm.transformF2F(0.5, 0.5, 1), fastSpeed) pc.go_to(*tfm.transformF2F(0.5, 0.5, 0))
def fly_square(self, id): """ Example of simple logico to make the drone fly in a square trajectory at fixed speed""" # Sync with drone with SyncCrazyflie(id, cf=self._cf) as scf: # Send position commands with PositionHlCommander(scf) as pc: pc.up(1.0) # print('Moving up') pc.forward(1.0) # print('Moving forward') pc.left(1.0) # print('Moving left') pc.back(1.0) # print('Moving back') pc.right(1.0) # print('Moving right') self._disconnected
def run_sequence(scf): cf = scf.cf with PositionHlCommander(scf, default_velocity=0.1) as pc: path = get_path() cf.param.set_value('ring.effect', '13') set_led_color(cf, [0, 0, 0]) pc.go_to(path[0][0], path[0][2], path[0][1]) last_pos = path[0] set_led_color(cf, [0, 100, 0]) for position in get_path(): pc.go_to(position[0], position[2], position[1]) last_pos = position set_led_color(cf, [0, 0, 0]) pc.go_to(0, 0, 0.1) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1)
def simple_sequence(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander(scf) as pc: time.sleep(1) x_home, y_home, z_home = pc.get_position() print(x_home, y_home) pc.go_to(0.3, 0.0) time.sleep(2) pc.go_to(0.0, 0) time.sleep(3) pc.go_to(0.0, 0.0, 0.1) # time.sleep(3) for _ in range(5): print('Doing other work') time.sleep(0.4)
def double_square(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander( scf, x=0, y=0, z=0, default_velocity=0.5, default_height=0.2, controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc: size = .8 max_height = 1.8 pc.go_to(-size,-size,1) pc.go_to(size,-size,1) pc.go_to(size,size,1) pc.go_to(-size,size,1) pc.go_to(-size,-size,1) pc.go_to(-size,-size,max_height) pc.go_to(size,-size,max_height) pc.go_to(size,size,max_height) pc.go_to(-size,size,max_height) pc.go_to(-size,-size,max_height) pc.go_to(0,0,.2)
def move_baduanjin_hl_p3(scf): with PositionHlCommander( scf, x=1.4, y=2.2, z=0.0, default_velocity=0.5, default_height=0.7, # default = 0.7 controller=PositionHlCommander.CONTROLLER_PID) as pc: print('Setting position [1.4, 2.2, {}]'.format(DEFAULT_HEIGHT)) t_init = time.time() ## Delay 6 sec time.sleep(6) t1 = time.time() - t_init print("t1: ", t1) ## Go up: d_fly meter/4.5 sec print('Setting position [1.4, 2.2, {}]'.format(d_abs)) pc.up(d_fly, velocity=d_fly/4.5) t2 = time.time() - t_init print("t2: ", t2) ## Delay 2.5 sec time.sleep(2.5) t3 = time.time() - t_init print("t3: ", t3) ## Go down: d_fly meter/4.5 sec print('Setting position [1.4, 2.2, {}]'.format(DEFAULT_HEIGHT)) pc.down(d_fly, velocity=d_fly/4.5) t4 = time.time() - t_init print("t4: ", t4) ## Delay 2.5 sec time.sleep(2.5) t5 = time.time() - t_init print("t5: ", t5)
def run_drone(scf, params): uri, height = params print(uri + ': waiting for estimator to find position...') find_initial_position(scf) init_lock.acquire() num_init += 1 # wait for all threads to initialise to their position if num_init == len(SPEC): init_lock.release() with init_condition: init_condition.notify_all() else: init_lock.release() with init_condition: init_condition.wait() # enter/exit hlcommander triggers takeoff/land with PositionHlCommander(scf, default_velocity=VELOCITY, default_height=height) as pc: run_swarm_sequence(pc, unique_height)
def slightly_more_complex_usage(scf): # with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander( scf, x=2.2, y=1.7, z=0.0, default_velocity=0.2, default_height=0.5, controller=PositionHlCommander.CONTROLLER_PID) as pc: # Go to a coordinate # print('Setting position {2.0, 2.0, 0.5}') # pc.forward(0.5) # print('Setting position {2.0, 3.5, 0.5}') # pc.left(1.5) print('Setting position {2.2, 1.7, 1.0}') print("t1: ", time.time()) # pc.go_to(2.2, 1.7, 1.0, velocity=0.2) pc.up(1.8, velocity=0.05) # print('Setting position {2.2, 1.7, 1.5}') # print("t2: ", time.time()) # # pc.go_to(2.2, 1.7, 1.5, velocity=0.1) # pc.up(0.5, velocity=0.05) # print('Setting position {2.2, 1.7, 1.0}') # print("t3: ", time.time()) # # pc.go_to(2.2, 1.7, 1.0, velocity=0.05) # pc.down(0.5, velocity=0.05) print('Setting position {2.2, 1.7, 0.5}') print("t4: ", time.time()) # pc.go_to(2.2, 1.7, 0.5, velocity=0.05) pc.down(1.8, velocity=0.05) print("t5: ", time.time())
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)