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 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_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)
