def stateInit(self):
"""Initialize CF (scan, open link) and logging framework"""
#--- Scan for cf
cflib.crtp.init_drivers(enable_debug_driver=False)
print('Scanning interfaces for Crazyflies...')
available = cflib.crtp.scan_interfaces()
if len(available) == 0:
print("No cf found, aborting cf code.")
self.cf = None
else:
print('Crazyflies found:')
for i in available:
print(str(i[0]))
self.URI = 'radio://0/80/2M' #available[0][0]
self.cf = Crazyflie(rw_cache='./cache')
if self.cf is None:
self.b.wait()
print('Not running cf code.')
return None
self.scf = SyncCrazyflie(self.URI,cf=Crazyflie(rw_cache='./cache'))
self.scf.open_link()
self.mc = MotionCommander(self.scf)
self.file = open("ugly_log.txt","a")
#-- Barrier to wait for CV thread
self.b.wait()
self.lgr = UglyLogger(self.URI, self.scf, self.file)
self.enterTakingOff()
return self.stateTakingOff
def gesture_ctrl(self, scf, fc, g):
mc = MotionCommander(scf)
mc._reset_position_estimator()
print ('Enter to start (c to cancel)')
char = raw_input()
if char == 'c':
print('\nClosing link...')
scf.close_link()
print('Shutting down...')
os.kill(os.getpid(), signal.SIGINT)
try:
while True:
g_id = gesture.get_gesture()
if gesture is not None:
fc.perform_gesture(g_id, mc)
except Exception, e:
print (str(e))
scf.close_link()
def __init__(self, name, crazyflie):
# Instantiate motion commander
self._cf = crazyflie
self._name = name
self._mc = MotionCommander(self._cf)
# Topic Publishers
self._velocity_setpoint_pub = rospy.Publisher(self._name +
'/velocity_setpoint',
Vector3,
queue_size=10)
"""
Services hosted for this crazyflie controller
"""
self._reset_position_estimator_srv = rospy.Service(
self._name + '/reset_position_estimator', ResetPositionEstimator,
self._reset_position_estimator_cb)
self._send_hover_setpoint_srv = rospy.Service(
self._name + '/send_hover_setpoint', SendHoverSetpoint,
self._send_hover_setpoint_cb)
self._set_param_srv = rospy.Service(self._name + '/set_param',
SetParam, self._set_param_cb)
self._velocity_control_srv = rospy.Service(
self._name + '/velocity_control', VelocityControl,
self._velocity_control_cb)
"""
Action servers for this crazyflie controller
"""
self._position_control_as = actionlib.SimpleActionServer(
self._name + '/position_control', PositionControlAction,
self._position_control_cb, False)
self._position_control_as.start()
def __init__(self, scf, available):
# get yaw-angle and battery level of crazyflie
self.psi, self.theta, self.phi, self.vbat = getCFparams(scf, available)
self.scf = scf
self.available = available
self.mc = MotionCommander(scf)
self.psi_limit = 0.7 # Don't cmd an angle less than this [deg]
self.des_angle = 0 # Set to zero initially
def setUp(self):
self.commander_mock = MagicMock(spec=Commander)
self.param_mock = MagicMock(spec=Param)
self.cf_mock = MagicMock(spec=Crazyflie)
self.cf_mock.commander = self.commander_mock
self.cf_mock.param = self.param_mock
self.cf_mock.is_connected.return_value = True
self.sut = MotionCommander(self.cf_mock)
def __init__(self, URI='radio://0/80/250K'):
"""
Init commad connects to drone
:param URI: URI
"""
self._cf = Crazyflie(rw_cache='./cache')
cflib.crtp.init_drivers(enable_debug_driver=False)
self.scf = SyncCrazyflie(URI, cf=self._cf)
self.scf.__enter__()
self.motionCommander = MotionCommander(self.scf)
def test_that_it_goes_up_to_default_height(self, _SetPointThread_mock,
sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
sut = MotionCommander(self.cf_mock, default_height=0.4)
# Test
sut.take_off(velocity=0.6)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls(
[call(0.0, 0.0, 0.6, 0.0),
call(0.0, 0.0, 0.0, 0.0)])
sleep_mock.assert_called_with(0.4 / 0.6)
def test_that_it_goes_up_to_default_height(
self, _SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
sut = MotionCommander(self.cf_mock, default_height=0.4)
# Test
sut.take_off(velocity=0.6)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.6, 0.0),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(0.4 / 0.6)
def move_box_limit(scf):
with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc:
# mc.start_forward()
# while (1):
# if position_estimate[0] > BOX_LIMIT:
# print("Back")
# mc.start_back(velocity=0.2)
# elif position_estimate[0] < -BOX_LIMIT:
# print("Forward")
# mc.start_forward(velocity=0.2)
# time.sleep(0.1) # without this line the CF cannot reach the order
body_x_cmd = 0.2
body_y_cmd = 0.1
max_vel = 0.2
while (1):
if position_estimate[0] > BOX_LIMIT:
body_x_cmd = -max_vel
elif position_estimate[0] < -BOX_LIMIT:
body_x_cmd = max_vel
if position_estimate[1] > BOX_LIMIT:
body_y_cmd = -max_vel
elif position_estimate[1] < -BOX_LIMIT:
body_y_cmd = max_vel
mc.start_linear_motion(body_x_cmd, body_y_cmd, 0)
time.sleep(0.1)
def main():
cflib.crtp.init_drivers(enable_debug_driver=False)
numRepeticions = int(input("Quants cops vols repetir la ruta?"))
ruta = input("Quina ruta vols realitzar?")
with SyncCrazyflie(URI) as scf:
with MotionCommander(scf) as mq90:
if ruta == rutaABC:
mq90.up(1)
mq90.forward(1)
...
...
elif ruta == rutaBC:
mq90.up(1)
mq90.forward(1)
...
...
elif ruta == rutaCB:
mq90.up(1)
mq90.forward(1)
...
...
elif ruta == rutaCA:
mq90.up(1)
mq90.forward(1)
...
...
elif ruta == rutaAC:
mq90.up(1)
mq90.forward(1)
...
...
mq90.land()
def move_baduanjin_mc_p3(scf):
with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc:
print("Target Height: {}".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("Target Height: {}".format(d_abs))
mc.move_distance(0, 0, d_fly, velocity=d_fly/4.5) # the final posistion will be "d_abs = DEFAULT_HEIGHT + d_fly"
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("Target Height: {}".format(DEFAULT_HEIGHT))
mc.move_distance(0, 0, -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 main():
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(URI) as scf:
with MotionCommander(scf) as mq90:
print("Take off done!")
mq90.up(1)
def move_baduanjin_mc_p1(scf):
with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc:
print("Target Height: {}".format(DEFAULT_HEIGHT))
time.sleep(1)
t_init = time.time()
## Go up: d_fly meter/4 sec
print("Target Height: {}".format(d_abs))
mc.move_distance(
0, 0, d_fly, velocity=d_fly /
5) # the final posistion will be "d_abs = DEFAULT_HEIGHT + d_fly"
t1 = time.time() - t_init
print("t1: ", t1)
## Delay 4 sec
# mc.stop()
time.sleep(4)
t2 = time.time() - t_init
print("t2: ", t2)
## Go down: d_fly meter/5 sec
print("Target Height: {}".format(DEFAULT_HEIGHT))
mc.move_distance(0, 0, -d_fly, velocity=d_fly / 5)
t3 = time.time() - t_init
print("t3: ", t3)
time.sleep(1)
def default_fly_to(self, x, y, results_folder):
logging.debug('DroneController: Instructed to move to : (' + str(x) +
',' + str(y) + ')')
logging.debug('Connecting to the crazyflie')
crazyflie = self.connect_to_crazyflie()
logging.debug('DroneController: Setting up the variable logging')
self.setup_logging(crazyflie, results_folder)
# We take off when the commander is created
with MotionCommander(crazyflie) as mc:
# Allowing the drone to take off
time.sleep(1)
# Flying forward the Y amount
if y != 0:
mc.forward(y)
time.sleep(1)
# Flying to the right for the X amount
if x != 0:
mc.right(x)
time.sleep(1)
mc.stop()
# Landing occurs when the Motion Commander goes out of scope
# Here we are keeping the application alive as the Crazyflie library makes
# non blocking calls
while self.is_connected:
time.sleep(1)
def selectDrone(self):
self.cfs = []
self.scfs = []
self.mcs = []
self.selected = []
message = 'Quais drones deseja controlar? (1, 2, 3, 4 ou combinacao delas):'
while (len(self.selected) == 0):
user_input_numbers = input(message)
if (user_input_numbers.find('1') != -1):
self.selected.append(1)
if (user_input_numbers.find('2') != -1):
self.selected.append(2)
if (user_input_numbers.find('3') != -1):
self.selected.append(3)
if (user_input_numbers.find('4') != -1):
self.selected.append(4)
if (len(self.selected) == 0):
print("comando invalido")
try:
for i in self.selected:
cf = Crazyflie(rw_cache='./cache')
self.cfs.append(cf)
sync = SyncCrazyflie(self.uris[i - 1], cf=cf)
sync.open_link()
self.mcs.append(MotionCommander(sync))
self.scfs.append(sync)
except:
print("Exception: Too many packets lost\n")
self.selected = []
def take_off_simple(scf):
with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc:
for i in range(FLIGHT_DURATION_S):
if (i % CORRECTION_INTERVAL_S == 0):
return_to_zero(mc, current_x, current_y, current_z)
time.sleep(1)
mc.stop()
def main():
cflib.crtp.init_drivers(enable_debug_driver=False)
numRepeticiones = int(input("¿Cuántas veces quieres repetir la ruta?"))
ruta = input("¿Qué ruta quieres realizar?")
with SyncCrazyflie(URI) as scf:
with MotionCommander(scf) as mq90:
if ruta == rutaABC:
mq90.up(1)
mq90.forward(1)
...
...
elif ruta == rutaBC:
mq90.up(1)
mq90.forward(1)
...
...
elif ruta == rutaCB:
mq90.up(1)
mq90.forward(1)
...
...
elif ruta == rutaCA:
mq90.up(1)
mq90.forward(1)
...
...
elif ruta == rutaAC:
mq90.up(1)
mq90.forward(1)
...
...
mq90.land()
def move_baduanjin_mc_p1(scf, event2):
with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc:
print("Target Height: {}".format(DEFAULT_HEIGHT))
time.sleep(1)
t_init = time.time()
## Go up: d_fly meter/6 sec
print("Target Height: {}".format(d_abs))
mc.move_distance(
0, 0, d_fly, velocity=d_fly /
6) # the final posistion will be "d_abs = DEFAULT_HEIGHT + d_fly"
t1 = time.time() - t_init
print("t1: ", t1)
## Delay 4 sec
# mc.stop()
time.sleep(4)
t2 = time.time() - t_init
print("t2: ", t2)
## Go down: d_fly meter/6 sec
print("Target Height: {}".format(DEFAULT_HEIGHT))
mc.move_distance(0, 0, -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 move_box_limit(scf):
with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc:
body_x_cmd = 0.2
body_y_cmd = 0.1
max_vel = 0.2
while (1):
'''if position_estimate[0] > BOX_LIMIT:
mc.start_back()
elif position_estimate[0] < -BOX_LIMIT:
mc.start_forward()
'''
if position_estimate[0] > BOX_LIMIT:
body_x_cmd = -max_vel
elif position_estimate[0] < -BOX_LIMIT:
body_x_cmd = max_vel
if position_estimate[1] > BOX_LIMIT:
body_y_cmd = -max_vel
elif position_estimate[1] < -BOX_LIMIT:
body_y_cmd = max_vel
mc.start_linear_motion(body_x_cmd, body_y_cmd, 0)
time.sleep(0.1)
def push(scf):
cf=scf.cf
with MotionCommander(scf) as motion_commander:
with Multiranger(scf) as multiranger:
keep_flying = True
while keep_flying:
VELOCITY = 0.3
velocity_x = 0.0
velocity_y = 0.0
if is_close(multiranger.front):
velocity_x -= VELOCITY
if is_close(multiranger.back):
velocity_x += VELOCITY
if is_close(multiranger.left):
velocity_y -= VELOCITY
if is_close(multiranger.right):
velocity_y += VELOCITY
if is_close(multiranger.up):
keep_flying = False
motion_commander.start_linear_motion(
velocity_x, velocity_y, 0)
time.sleep(0.1)
print('Demo terminated!')
def __init__(self, link_uri):
""" Initialize and run the test with the specified link_uri """
self._cf = Crazyflie()
self.mc = MotionCommander(self._cf, default_height=1.05)
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._cf.open_link(link_uri)
print('Connecting to %s' % link_uri)
self.is_connected = True
def main():
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(URI) as scf:
with MotionCommander(scf) as mq90:
mq90.up(valueToMove)
mq90.forward(valueGoForward)
mq90.land()
def __init__(self):
cflib.crtp.init_drivers(enable_debug_driver=False)
cf = Crazyflie(rw_cache='./cache')
self.scf = SyncCrazyflie(URI, cf=cf)
self.scf.open_link()
self.client = None
log_config = LogConfig(name='kalman', period_in_ms=100)
log_config.add_variable('kalman.stateX', 'float')
log_config.add_variable('kalman.stateY', 'float')
self.logger_pos = SyncLogger(self.scf, log_config)
log_config_2 = LogConfig(name='stabilizer', period_in_ms=100)
log_config_2.add_variable('stabilizer.yaw', 'float')
self.logger_orientation = SyncLogger(self.scf, log_config_2)
self.home_pos = self.get_position()
print('Home = %s ' % self.home_pos)
self.orientation = self.get_orientation()
self.client = MotionCommander(self.scf)
self.client.take_off(height=0.6)
def setUp(self):
self.commander_mock = MagicMock(spec=Commander)
self.param_mock = MagicMock(spec=Param)
self.cf_mock = MagicMock(spec=Crazyflie)
self.cf_mock.commander = self.commander_mock
self.cf_mock.param = self.param_mock
self.cf_mock.is_connected.return_value = True
self.sut = MotionCommander(self.cf_mock)
def move_linear_simple(scf):
with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc:
time.sleep(1)
mc.forward(0.5)
time.sleep(1)
mc.turn_left(180)
time.sleep(1)
mc.forward(0.5)
time.sleep(1)
def __init__(self, cfobject):
#get initial orientation
# We take off when the commander is created
with MotionCommander(cfobject) as mc:
#update current Yaw angle at 100 Hz
threading.Timer(10.0, self._updateYawCurr).start()
#use seperate thread for motor operation
Thread(target=self._quadMotion(mc)).start()
class Node:
def __init__(self):
self.vx = 0
self.vy = 0
self.vz = 0
self.va = 0
self.vel = Twist()
self.vel.linear.x = self.vx
self.vel.linear.y = self.vy
self.vel.linear.z = self.vz
self.vel.angular.z = self.va
self.vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
cflib.crtp.init_drivers(enable_debug_driver=False)
self.crazyflie = SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache'))
self.commander = MotionCommander(self.crazyflie)
self.cf = Crazyflie()
self.crazyflie.open_link()
self.commander.take_off()
def shut_down(self):
try:
self.pitch_log.stop()
finally:
self.crazyflie.close_link()
def run_node(self):
self.vel.linear.x = 0
self.vel.linear.y = 0
self.vel.linear.z = 0.2
self.vel.angular.z = 0
self.vel_pub.publish(self.vel)
self.commander._set_vel_setpoint(0, 0, 0.2, 0)
time.sleep(3)
self.vel.linear.x = 0
self.vel.linear.y = 0
self.vel.linear.z = 0
self.vel.angular.z = 72
self.vel_pub.publish(self.vel)
self.commander._set_vel_setpoint(0, 0, 0, 72)
time.sleep(3)
self.vel.linear.x = 0
self.vel.linear.y = 0
self.vel.linear.z = -0.2
self.vel.angular.z = 0
self.vel_pub.publish(self.vel)
self.commander._set_vel_setpoint(0, 0, -0.2, 0)
time.sleep(3)
def __init__(self, mc):
self.mc = mc
self.calculator = DistanceCalculator()
myMotionCommander = MotionCommander(self.mc)
self.cap = cv2.VideoCapture(1)
CenterLimitH = [305, 335]
self.savedX = 50
self.savedY = 0
self.savedAlpha = 180
self.savedBeta = 90
def main():
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(URI) as scf:
with MotionCommander(scf) as mq90:
mq90.up(valueToMove)
mq90.forward(valueGoForwardBig)
mq90.turn_right(totalDegreesTurnRight)
mq90.forward(valueGoForwardSmall)
mq90.land()
def main():
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(URI) as scf:
with MotionCommander(scf) as mq90:
for i in range(5):
mq90.forward(0.5)
mq90.turn_right(144)
mq90.land()
def move(link_uri, forward=2, back=2, velocity=0.05, height=0.2):
cflib.crtp.init_drivers()
with SyncCrazyflie(link_uri=link_uri,
cf=Crazyflie(rw_cache="./cache")) as scf:
with MotionCommander(crazyflie=scf, default_height=height) as mc:
time.sleep(5)
mc.forward(distance_m=forward, velocity=velocity)
time.sleep(2)
mc.back(distance_m=back, velocity=velocity)
time.sleep(5)
def __init__(self, mc):
self.mc = mc
self.calculator = DistanceCalculator()
myMotionCommander = MotionCommander(self.mc)
self.cap = cv2.VideoCapture("video_localizacao.webm")
CenterLimitH = [305, 335]
self.savedX = 50
self.savedY = 0
self.savedAlpha = 180
self.savedBeta = 90
self.map_view = MapView()
class TestMotionCommander(unittest.TestCase):
def setUp(self):
self.commander_mock = MagicMock(spec=Commander)
self.param_mock = MagicMock(spec=Param)
self.cf_mock = MagicMock(spec=Crazyflie)
self.cf_mock.commander = self.commander_mock
self.cf_mock.param = self.param_mock
self.cf_mock.is_connected.return_value = True
self.sut = MotionCommander(self.cf_mock)
def test_that_the_estimator_is_reset_on_take_off(
self, _SetPointThread_mock, sleep_mock):
# Fixture
# Test
self.sut.take_off()
# Assert
self.param_mock.set_value.assert_has_calls([
call('kalman.resetEstimation', '1'),
call('kalman.resetEstimation', '0')
])
def test_that_take_off_raises_exception_if_not_connected(
self, _SetPointThread_mock, 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, _SetPointThread_mock, 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, _SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
# Test
self.sut.take_off(height=0.4, velocity=0.5)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.5, 0.0),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(0.4 / 0.5)
def test_that_it_goes_up_to_default_height(
self, _SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
sut = MotionCommander(self.cf_mock, default_height=0.4)
# Test
sut.take_off(velocity=0.6)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.6, 0.0),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(0.4 / 0.6)
def test_that_the_thread_is_started_on_takeoff(
self, _SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
# Test
self.sut.take_off()
# Assert
thread_mock.start.assert_called_with()
def test_that_it_goes_down_on_landing(
self, _SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
thread_mock.get_height.return_value = 0.4
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.land(velocity=0.5)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, -0.5, 0.0),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(0.4 / 0.5)
def test_that_it_takes_off_and_lands_as_context_manager(
self, _SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
thread_mock.reset_mock()
thread_mock.get_height.return_value = 0.3
# Test
with self.sut:
pass
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.2, 0.0),
call(0.0, 0.0, 0.0, 0.0),
call(0.0, 0.0, -0.2, 0.0),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(0.3 / 0.2)
sleep_mock.assert_called_with(0.3 / 0.2)
def test_that_it_starts_moving_multi_dimensional(
self, _SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.start_linear_motion(0.1, 0.2, 0.3)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.1, 0.2, 0.3, 0.0),
])
def test_that_it_starts_moving(
self, _SetPointThread_mock, sleep_mock):
# Fixture
self.sut.take_off()
vel = 0.3
data = [
[self.sut.start_forward, (vel, 0.0, 0.0, 0.0)],
[self.sut.start_back, (-vel, 0.0, 0.0, 0.0)],
[self.sut.start_left, (0.0, vel, 0.0, 0.0)],
[self.sut.start_right, (0.0, -vel, 0.0, 0.0)],
[self.sut.start_up, (0.0, 0.0, vel, 0.0)],
[self.sut.start_down, (0.0, 0.0, -vel, 0.0)],
]
# Test
# Assert
for line in data:
self._verify_start_motion(line[0], vel, line[1],
_SetPointThread_mock)
def test_that_it_moves_multi_dimensional_distance(
self, _SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
x = 0.1
y = 0.2
z = 0.3
vel = 0.2
distance = math.sqrt(x * x + y * y + z * z)
expected_time = distance / vel
expected_vel_x = vel * x / distance
expected_vel_y = vel * y / distance
expected_vel_z = vel * z / distance
self.sut.take_off()
thread_mock.reset_mock()
sleep_mock.reset_mock()
# Test
self.sut.move_distance(x, y, z)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(expected_vel_x, expected_vel_y, expected_vel_z, 0.0),
])
sleep_mock.assert_called_with(expected_time)
def test_that_it_moves(self, _SetPointThread_mock, sleep_mock):
# Fixture
vel = 0.3
self.sut.take_off()
data = [
[self.sut.forward, (vel, 0.0, 0.0, 0.0)],
[self.sut.back, (-vel, 0.0, 0.0, 0.0)],
[self.sut.left, (0.0, vel, 0.0, 0.0)],
[self.sut.right, (0.0, -vel, 0.0, 0.0)],
[self.sut.up, (0.0, 0.0, vel, 0.0)],
[self.sut.down, (0.0, 0.0, -vel, 0.0)],
]
# Test
# Assert
for test in data:
self._verify_move(test[0], vel, test[1],
_SetPointThread_mock, sleep_mock)
def test_that_it_starts_turn_right(self, _SetPointThread_mock, sleep_mock):
# Fixture
rate = 20
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.start_turn_right(rate)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.0, rate),
])
def test_that_it_starts_turn_left(self, _SetPointThread_mock, sleep_mock):
# Fixture
rate = 20
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.start_turn_left(rate)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.0, -rate),
])
def test_that_it_starts_circle_right(
self, _SetPointThread_mock, sleep_mock):
# Fixture
velocity = 0.5
radius = 0.9
expected_rate = 360 * velocity / (2 * radius * math.pi)
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.start_circle_right(radius, velocity=velocity)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(velocity, 0.0, 0.0, expected_rate),
])
def test_that_it_starts_circle_left(
self, _SetPointThread_mock, sleep_mock):
# Fixture
velocity = 0.5
radius = 0.9
expected_rate = 360 * velocity / (2 * radius * math.pi)
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.start_circle_left(radius, velocity=velocity)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(velocity, 0.0, 0.0, -expected_rate),
])
def test_that_it_turns_right(self, _SetPointThread_mock, sleep_mock):
# Fixture
rate = 20
angle = 45
turn_time = angle / rate
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.turn_right(angle, rate=rate)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.0, rate),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(turn_time)
def test_that_it_turns_left(self, _SetPointThread_mock, sleep_mock):
# Fixture
rate = 20
angle = 45
turn_time = angle / rate
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.turn_left(angle, rate=rate)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(0.0, 0.0, 0.0, -rate),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(turn_time)
def test_that_it_circles_right(self, _SetPointThread_mock, sleep_mock):
# Fixture
radius = 0.7
velocity = 0.3
angle = 45
distance = 2 * radius * math.pi * angle / 360.0
turn_time = distance / velocity
rate = angle / turn_time
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.circle_right(radius, velocity, angle)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(velocity, 0.0, 0.0, rate),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(turn_time)
def test_that_it_circles_left(self, _SetPointThread_mock, sleep_mock):
# Fixture
radius = 0.7
velocity = 0.3
angle = 45
distance = 2 * radius * math.pi * angle / 360.0
turn_time = distance / velocity
rate = angle / turn_time
thread_mock = _SetPointThread_mock()
self.sut.take_off()
thread_mock.reset_mock()
# Test
self.sut.circle_left(radius, velocity, angle)
# Assert
thread_mock.set_vel_setpoint.assert_has_calls([
call(velocity, 0.0, 0.0, -rate),
call(0.0, 0.0, 0.0, 0.0)
])
sleep_mock.assert_called_with(turn_time)
######################################################################
def _verify_start_motion(self, function_under_test, velocity, expected,
_SetPointThread_mock):
# Fixture
thread_mock = _SetPointThread_mock()
thread_mock.reset_mock()
# Test
function_under_test(velocity=velocity)
# Assert
try:
thread_mock.set_vel_setpoint.assert_has_calls([
call(*expected),
])
except AssertionError as e:
self._eprint('Failed when testing function ' +
function_under_test.__name__)
raise e
def _verify_move(self, function_under_test, velocity, expected,
_SetPointThread_mock, sleep_mock):
# Fixture
thread_mock = _SetPointThread_mock()
distance = 1.2
expected_time = distance / velocity
thread_mock.reset_mock()
sleep_mock.reset_mock()
# Test
function_under_test(distance, velocity=velocity)
# Assert
try:
thread_mock.set_vel_setpoint.assert_has_calls([
call(*expected),
call(0.0, 0.0, 0.0, 0.0),
])
sleep_mock.assert_called_with(expected_time)
except AssertionError as e:
print('Failed when testing function ' +
function_under_test.__name__)
raise e
