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
class UAVController():
def __init__(self, targetURI=None):
"""
Function: __init__
Purpose: Initialize all necessary UAV functionality
Inputs: none
Outputs: none
"""
cflib.crtp.init_drivers()
#self.FD = open("logFile.txt", "w")
self.timeout = True
self.available = []
self.UAV = Crazyflie()
self.param = None
self.airborne = False
self._recentDataPacket = None
self._receivingDataPacket = False
#Attempt to locate UAV by scanning available interface
for _ in range(0, 500):
if (len(self.available) > 0):
self.timeout = False
break #If a UAV is found via scanning, break out of this loop
else:
self.available = cflib.crtp.scan_interfaces()
if (len(self.available) > 0):
if (targetURI != None):
for i in range(len(self.available)):
if (self.available[i][0] == targetURI):
self.UAV.open_link(self.available[i][0])
self.connectedToTargetUAV = True
else:
self.connectedToTargetUAV = False
else:
self.UAV.open_link(self.available[0][0])
while (self.UAV.is_connected() == False):
time.sleep(0.1)
self.MC = MotionCommander(self.UAV)
#Create desired logging parameters
self.UAVLogConfig = LogConfig(name="UAVLog", period_in_ms=100)
#self.UAVLogConfig.add_variable('pm.batteryLevel', 'float')
self.UAVLogConfig.add_variable('pm.vbat', 'float')
self.UAVLogConfig.add_variable('pm.batteryLevel', 'float')
#self.UAVLogConfig.add_variable('stateEstimate.x', 'float')
#self.UAVLogConfig.add_variable('stateEstimate.y', 'float')
self.UAVLogConfig.add_variable('stateEstimate.z', 'float')
self.UAVLogConfig.add_variable('pm.chargeCurrent', 'float')
self.UAVLogConfig.add_variable('pm.extCurr', 'float')
self.UAVLogConfig.add_variable('pwm.m1_pwm', 'float')
#self.UAVLogConfig.add_variable('baro.pressure', 'float')
#self.UAVLogConfig.add_variable('extrx.thrust', 'float')
#Add more variables here for logging as desired
self.UAV.log.add_config(self.UAVLogConfig)
if (self.UAVLogConfig.valid):
self.UAVLogConfig.data_received_cb.add_callback(
self._getUAVDataPacket)
self.UAVLogConfig.start()
else:
logger.warning("Could not setup log configuration")
self._startTime = time.time() #For testing purposes
self._trialRun = 0
#End of function
def done(self):
"""
Function: done
Purpose: Close connection to UAV to terminate all threads running
Inputs: none
Outputs: none
"""
self.UAVLogConfig.stop()
self.UAV.close_link()
self.airborne = False
return
def launch(self):
"""
Function: launch
Purpose: Instruct the UAV to takeoff from current position to the default height
Inputs: none
Outputs: none
"""
self.airborne = True
self.MC.take_off()
#End of function
return
def land(self):
"""
Function: launch
Purpose: Instruct the UAV to land on the ground at current position
Inputs: none
Outputs: none
"""
self.MC.land()
return
def move(self, distanceX, distanceY, distanceZ, velocity):
"""
Function: move
Purpose: A wrapper function to instruct an UAV to move <x, y, z> distance from current point
Inputs: distance - a floating point value distance in meters
velocity - a floating point value velocity in meters per second
Outputs: none
"""
if (self.airborne == False):
self.launch()
self.MC.move_distance(distanceX, distanceY, distanceZ, velocity)
#End of function
return
def rotate(self, degree):
"""
Function: rotate
Purpose: A wrapper function to instruct an UAV to rotate
Inputs: degree - a floating point value in degrees
Outputs: none
"""
if (self.airborne == False):
self.launch()
if (degree < 0):
print("UC: rotate - Going Right")
locDeg = 0
#self.MC.turn_right(abs(degree))
for _ in range(1, int(abs(degree) / 1)):
locDeg += 1
self.MC.turn_right(1)
self.MC.turn_right(abs(degree) - locDeg)
else:
print("UC: rotate - Going Left")
self.MC.turn_left(abs(degree))
#Delay by 1 seclond, to allow for total rotation time
time.sleep(1)
return
def getBatteryLevel(self):
"""
Function: getBatteryLevel
Purpose: A function that reads the UAV battery level from a IOStream
Inputs: none
Outputs: none
Description:
"""
retVal = None
if (self._recentDataPacket != None
and self._receivingDataPacket == False):
retVal = self._recentDataPacket["pm.vbat"]
return retVal
def getHeight(self):
"""
Function: getCurrentHeight
Purpose: A function that reads the UAV height from a IOStream
Inputs: none
Outputs: none
Description:
"""
retVal = None
if (self._recentDataPacket != None
and self._receivingDataPacket == False):
retVal = self._recentDataPacket["stateEstimate.z"]
return retVal
def isCharging(self):
"""
Function: getCurrentHeight
Purpose: A function that reads the UAV height from a IOStream
Inputs: none
Outputs: none
Description:
"""
retVal = None
if (self._recentDataPacket != None
and self._receivingDataPacket == False):
retVal = self._recentDataPacket["pm.chargeCurrent"]
return retVal
def _getUAVDataPacket(self, ident, data, logconfig):
"""
Function: getUAVDataPacket
Purpose: Process a data packet received from the UAV
Inputs: ident -
data -
logconfig -
Outputs: None
Description: A user should never call this function.
"""
self._receivingDataPacket = True
self._recentDataPacket = data
self._receivingDataPacket = False
def appendToLogFile(self):
"""
Function: appendToLogFile
Purpose: append log variables to log file 'log.txt.'
Inputs: none
Outputs: none
Description:
"""
#retVal = None
if (self._recentDataPacket != None
and self._receivingDataPacket == False):
current = self._recentDataPacket["pm.chargeCurrent"]
extcurrent = self._recentDataPacket["pm.extCurr"]
voltage = self._recentDataPacket["pm.vbat"]
bat_level = self._recentDataPacket["pm.batteryLevel"]
height = self._recentDataPacket["stateEstimate.z"]
#x = self._recentDataPacket["stateEstimate.y"]
#y = self._recentDataPacket["stateEstimate.x"]
m1_pwm = self._recentDataPacket["pwm.m1_pwm"]
#extrx_thrust = self._recentDataPacket["extrx.thrust"]
#baro_pressure = self._recentDataPacket["baro.pressure"]
with open('log2.txt', 'a') as file:
#print(batlevel)
file.write(str(datetime.now()) + '\n')
file.write('bat_voltage: ' + str(voltage) + '\n')
file.write('bat_level: ' + str(bat_level) + '\n')
file.write('crg_current: ' + str(current) + '\n')
file.write('ext_current: ' + str(extcurrent) + '\n')
file.write('height: ' + str(height) + '\n')
#file.write('x: ' + str(x) + '\n')
#file.write('y: ' + str(y) + '\n')
file.write('m1_pwm: ' + str(m1_pwm) + '\n')
class UAVController():
def __init__(self, targetURI=None):
"""
Function: __init__
Purpose: Initialize all necessary UAV functionality
Inputs: none
Outputs: none
Description: An initializer function that finds a Crazyflie, can be particular target or not, and sets up all necessary data values for logging data values from the Crazyflie.
"""
#Enable the CrazyRadio PA drivers to communicate with the UAV.
cflib.crtp.init_drivers()
self.timeout = True
self.available = []
self.UAV = Crazyflie()
self.param = None
self.airborne = False
self._recentDataPacket = None
self._receivingDataPacket = False
#Attempt to locate UAV by scanning available interface
for _ in range(0, 500):
if (len(self.available) > 0):
self.timeout = False
break #If a UAV is found via scanning, break out of this loop
else:
self.available = cflib.crtp.scan_interfaces()
#If there are Crazyflies available, connect to one of them
if (len(self.available) > 0):
#If there is a specific target, parse through the returned array to locate specific Crazyflie
if (targetURI != None):
for i in range(len(self.available)):
if (self.available[i][0] == targetURI):
#If found, open a link to the Crazyflie
self.UAV.open_link(self.available[i][0])
self.connectedToTargetUAV = True
else:
self.connectedToTargetUAV = False
else:
#If there is not a specific target, simply connect to the first Crazyflie available
self.UAV.open_link(self.available[0][0])
#While the Crazyflie is not connected, wait until the connection occurs to allow for Motion Commander initialization
while (self.UAV.is_connected() == False):
time.sleep(0.1)
#Pass the connected Crazyflie to the Motion Commander class
self.MC = MotionCommander(self.UAV)
#Create desired logging parameters
self.UAVLogConfig = LogConfig(name="UAVLog", period_in_ms=100)
self.UAVLogConfig.add_variable('pm.vbat', 'float')
self.UAVLogConfig.add_variable('stateEstimate.x', 'float')
self.UAVLogConfig.add_variable('stateEstimate.y', 'float')
self.UAVLogConfig.add_variable('stateEstimate.z', 'float')
self.UAVLogConfig.add_variable('pm.chargeCurrent', 'float')
#Add more variables here for logging as desired
#Add the configured logger to the Crazyflie to begin grabbing data packets
self.UAV.log.add_config(self.UAVLogConfig)
if (self.UAVLogConfig.valid):
self.UAVLogConfig.data_received_cb.add_callback(
self._getUAVDataPacket)
self.UAVLogConfig.start()
else:
logger.warning("Could not setup log configuration")
#End of function
def done(self):
"""
Function: done
Purpose: Close connection to UAV to terminate all threads running
Inputs: none
Outputs: none
Description: See purpose.
"""
self.UAVLogConfig.stop()
self.UAV.close_link()
self.airborne = False
return
def launch(self):
"""
Function: launch
Purpose: Instruct the UAV to takeoff from current position to the default height
Inputs: none
Outputs: none
Description: A wrapper function that calls the Crazyflie Motion Commander take_off function. See Bitcraze Crazyflie documentation for further details.
"""
self.airborne = True
self.MC.take_off()
#End of function
return
def land(self):
"""
Function: launch
Purpose: Instruct the UAV to land on the ground at current position
Inputs: none
Outputs: none
Description: A function that calls the Crazyflie Motion Commander Land function. See Bitcraze Crazyflie documentation for further details.
"""
self.airborne = False
self.MC.land()
return
def move(self, distanceX, distanceY, distanceZ, velocity):
"""
Function: move
Purpose: A wrapper function to instruct an UAV to move <x, y, z> distance from current point
Inputs: distanceX - a floating point value that represents the distance to move the in the X-dimension, measured in meters.
distanceY - a floating point value that represents the distance to move the in the Y-dimension, measured in meters.
distanceZ - a floating point value that represents the distance to move the in the Z-dimension, measured in meters.
velocity - a floating point value that represents the velocity of the UAV during movement, measured in meters per second
Outputs: none
Description: See purpose.
"""
if (self.airborne == False):
self.launch()
self.MC.move_distance(distanceX, distanceY, distanceZ, velocity)
#End of function
return
def rotate(self, degree):
"""
Function: rotate
Purpose: A wrapper function to instruct an UAV to rotate
Inputs: degree - a floating point value in degrees
Outputs: none
Description: Currently this function is not in use as rotating the Crazyflie regularly introduces positional errors. In particular, the Crazyflie will rotate on a motor, not the center of the drone.
"""
if (self.airborne == False):
self.launch()
if (degree < 0):
print("UC: rotate - Going Right")
locDeg = 0
#self.MC.turn_right(abs(degree))
for _ in range(1, int(abs(degree) / 1)):
locDeg += 1
self.MC.turn_right(1)
self.MC.turn_right(abs(degree) - locDeg)
else:
print("UC: rotate - Going Left")
self.MC.turn_left(abs(degree))
#Delay by 1 second, to allow for total rotation time
time.sleep(1)
return
def getBatteryLevel(self):
"""
Function: getBatteryLevel
Purpose: A function that reads the UAV battery level from an IOStream
Inputs: none
Outputs: retVal - a floating point value that represents the battery voltage of the UAV.
Description: See purpose.
"""
retVal = None
if (self._recentDataPacket != None
and self._receivingDataPacket == False):
retVal = self._recentDataPacket["pm.vbat"]
return retVal
def getHeight(self):
"""
Function: getCurrentHeight
Purpose: A function that reads the UAV height from a IOStream.
Inputs: none
Outputs: retVal - a floating point value that represents the onboard height detection of the UAV.
Description: See purpose.
"""
retVal = None
if (self._recentDataPacket != None
and self._receivingDataPacket == False):
retVal = self._recentDataPacket["stateEstimate.z"]
return retVal
def isCharging(self):
"""
Function: getCurrentHeight
Purpose: A function that reads the UAV charge current from a IOStream
Inputs: none
Outputs: retVal - a floating point value that represents the charge current of the UAV.
Description: See purpose.
"""
retVal = None
if (self._recentDataPacket != None
and self._receivingDataPacket == False):
retVal = self._recentDataPacket["pm.chargeCurrent"]
return retVal
def _getUAVDataPacket(self, ident, data, logconfig):
"""
Function: getUAVDataPacket
Purpose: A callback function to process a data packet received from the UAV
Inputs: ident - identifier of the UAV
data - data from the UAV
logconfig - log configuration from the UAV
Outputs: None
Description: A user should NEVER call this function.
"""
self._receivingDataPacket = True
self._recentDataPacket = data
self._receivingDataPacket = False
class CrazyflieControl:
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()
"""
Service Callbacks
"""
def _reset_position_estimator_cb(self, req):
pass
def _send_hover_setpoint_cb(self, req):
vx = req.vx
vy = req.vy
z = req.z
yaw_rate = req.yaw_rate
self._cf.commander.send_hover_setpoint(vx, vy, yaw_rate, z)
return []
def _set_param_cb(self, req):
self._cf.param.set_value(req.param, req.value)
print("set %s to %s" % (req.param, req.value))
return SetParamResponse()
def _velocity_control_cb(self, req):
try:
obj = pickle.loads(req.pickle)
print(self._mc)
if isinstance(obj, SetVelSetpoint):
self._mc._set_vel_setpoint(obj.vx, obj.vy, obj.vz,
obj.rate_yaw)
elif isinstance(obj, StartBack):
self._mc.start_back(velocity=obj.velocity)
elif isinstance(obj, StartCircleLeft):
self._mc.start_circle_left(obj.radius_m, velocity=obj.velocity)
elif isinstance(obj, StartCircleRight):
self._mc.start_turn_right(obj.radius_m, velocity=obj.velocity)
elif isinstance(obj, StartDown):
self._mc.start_down(velocity=obj.velocity)
elif isinstance(obj, StartForward):
self._mc.start_forward(velocity=obj.velocity)
elif isinstance(obj, StartLeft):
self._mc.start_left(velocity=obj.velocity)
elif isinstance(obj, StartLinearMotion):
self._mc.start_linear_motion(obj.vx, obj.vy, obj.vz)
elif isinstance(obj, StartRight):
self._mc.start_right(velocity=obj.velocity)
elif isinstance(obj, StartTurnLeft):
self._mc.start_turn_left(rate=obj.rate)
elif isinstance(obj, StartTurnRight):
self._mc.start_turn_right(rate=obj.rate)
elif isinstance(obj, StartUp):
self._mc.start_up(velocity=obj.velocity)
elif isinstance(obj, Stop):
self._mc.stop()
else:
return 'Object is not a valid velocity command'
except Exception as e:
print(str(e))
raise e
return 'ok'
"""
Action Implementations
"""
def _position_control_cb(self, goal):
try:
obj = pickle.loads(goal.pickle)
if isinstance(obj, Back):
self._mc.back(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, CircleLeft):
self._mc.circle_left(obj.radius_m,
velocity=obj.velocity,
angle_degrees=obj.angle_degrees)
elif isinstance(obj, CircleRight):
self._mc.circle_right(obj.radius_m,
velocity=obj.velocity,
angle_degrees=obj.angle_degrees)
elif isinstance(obj, Down):
self._mc.down(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, Forward):
self._mc.forward(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, Land):
self._mc.land(velocity=obj.velocity)
elif isinstance(obj, Left):
self._mc.left(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, MoveDistance):
self._mc.move_distance(obj.x,
obj.y,
obj.z,
velocity=obj.velocity)
elif isinstance(obj, Right):
self._mc.right(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, TakeOff):
self._mc.take_off(height=obj.height, velocity=obj.velocity)
elif isinstance(obj, TurnLeft):
self._mc.turn_left(obj.angle_degrees, rate=obj.rate)
elif isinstance(obj, TurnRight):
self._mc.turn_right(obj.angle_degrees, rate=obj.rate)
elif isinstance(obj, Up):
self._mc.up(obj.distance_m, velocity=obj.velocity)
except Exception as e:
print('Exception in action server %s' % self._name +
'/position_control')
print(str(e))
print('Action aborted')
self._position_control_as.set_aborted()
return
self._position_control_as.set_succeeded()
def _takeoff(self, goal):
try:
self._mc.take_off(height=goal.height)
time.sleep(5)
except BaseException as e:
self._takeoff_as.set_aborted()
print(e)
return
self._takeoff_as.set_succeeded(TakeOffResult(True))
def _land(self, goal):
try:
self._mc.land(velocity=goal.velocity)
except BaseException as e:
self._land_as.set_aborted()
print(e)
return
self._land_as.set_succeeded(LandResult(True))
def _move_distance(self, goal):
try:
x = goal.x
y = goal.y
z = goal.z
velocity = goal.velocity
dist = np.sqrt(x**2 + y**2 + z**2)
vx = x / dist * velocity
vy = y / dist * velocity
vz = z / dist * velocity
# self._velocity_setpoint_pub.publish(Vector3(vx, vy, vz))
self._mc.move_distance(x, y, z, velocity=velocity)
# self._velocity_setpoint_pub.publish(Vector3(vx, vy, vz))
except BaseException as e:
self._move_distance_as.set_aborted()
print(e)
return
self._move_distance_as.set_succeeded()
class FlightCtrl:
#initialize variables for attitude tracking
yawInit = 0
yawCurr = 0
theta = 0
rtCoef = [0, -1]
lfCoef = [0, 1]
fwCoef = [1, 0]
bkCoef = [-1, 0]
lvSpeed = 0.3
def __init__(self, _scf):
self.mc = MotionCommander(_scf, default_height=0.7)
self.scf = _scf
self.scf.open_link()
def perform_gesture(self, g_id):
global consecDoubleTaps
global inMotion
d = 0.3
if g_id[0] == DOUBLE_TAP:
if self.mc._is_flying:
print("Hovering...")
#mc.stop()
self.resetYawInit()
else:
consecDoubleTaps = 0
print("Taking off...")
inMotion = True
self.mc.take_off()
inMotion = False
self.resetYawInit()
threadUpdate = Thread(target=self._updateYaw,
args=(self.scf, ))
threadUpdate.start()
elif g_id[0] == NO_POSE and g_id[1] == yaw_id:
print("Roll...")
inMotion = True
self.mc.move_distance(0, math.copysign(d, g_id[2]), 0)
inMotion = False
"""if (g_id[2] > 0):
#turn left
print("turning left")
inMotion = True
self.mc.move_distance(self.lvSpeed * self.lfCoef[0], self.lvSpeed * self.lfCoef[1], 0)
inMotion = False
else:
#turn right
print("turning right")
inMotion = True
self.mc.move_distance(self.lvSpeed * self.rtCoef[0], self.lvSpeed * self.rtCoef[1], 0)
inMotion = False
"""
elif g_id[0] == NO_POSE and g_id[1] == pitch_id:
print("Pitch...")
inMotion = True
self.mc.move_distance(-math.copysign(d, g_id[2]), 0, 0)
inMotion = False
"""if (g_id[2] < 0):
#move forward
print("moving forward")
inMotion = True
self.mc.move_distance(self.lvSpeed * self.fwCoef[0], self.lvSpeed * self.fwCoef[1], 0)
inMotion = False
else:
#move backward
print("moving backward")
inMotion = True
self.mc.move_distance(self.lvSpeed * self.bkCoef[0], self.lvSpeed * self.bkCoef[1], 0)
inMotion = False
"""
elif g_id[0] == FINGERS_SPREAD and g_id[1] == pitch_id:
if g_id[2] > 0:
if self.mc._thread.get_height() + d < self.mc.max_height:
print("Up...")
inMotion = True
self.mc.up(d)
inMotion = False
else:
print("Max. height %.2fm reached: requested height: %.2f"
) % (self.mc.max_height,
self.mc._thread.get_height() + d)
else:
if self.mc._thread.get_height() - d < self.mc.min_height:
print("Down...")
inMotion = True
self.mc.down(d)
inMotion = False
else:
print("Max. height %.2fm reached: requested height: %.2f"
) % (self.mc.max_height,
self.mc._thread.get_height() + d)
elif g_id[0] == FIST and g_id[1] == yaw_id:
print('Yaw...')
if g_id[2] > 0:
inMotion = True
self.mc.turn_left(30)
inMotion = False
else:
inMotion = True
self.mc.turn_right(30)
inMotion = False
elif g_id[0] == LAND:
print("Landing...")
inMotion = True
self.mc.land()
inMotion = False
else: #rest behaviour
if self.mc._is_flying:
inMotion = True
self.mc.stop()
inMotion = False
"""Functions to update attitude by reading storage text file"""
def updateCoef(self):
try:
self.theta = (self.yawCurr - self.yawInit) * (3.1415926 / 180)
self.rtCoef = [-sin(self.theta), -cos(self.theta)]
self.lfCoef = [sin(self.theta), cos(self.theta)]
self.fwCoef = [cos(self.theta), -sin(self.theta)]
self.bkCoef = [-cos(self.theta), sin(self.theta)]
except Exception, e:
print str(e)
print("Update failed")
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_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 = _SetPointThread_mock()
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_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
