class Drone:
"""Drone instance. Starts and flies drone"""
logging.basicConfig(level=logging.ERROR)
URI = 'radio://0/80/250K'
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)
#self.multiranger = Multiranger(self.scf)
def take_off(self, height=1.0, velocity=0.2):
"""has the drone take off"""
# drone takes off
self.motionCommander.take_off(height=height, velocity=velocity)
def land(self):
self.motionCommander.land()
def move(self, vector):
"""moves the drone at a constant speed in one direction"""
if vector['y'] > 0.1:
print "move up by {}m".format(vector['y'])
self.motionCommander.up(vector['y'], velocity=0.4)
elif vector['y'] < -0.1:
print "move down by {}m".format(abs(vector['y']))
self.motionCommander.down(abs(vector['y']), velocity=0.4)
if vector['x'] > 0.1:
print "move left by {}m".format(vector['x'])
self.motionCommander.left(vector['x'], velocity=0.4)
elif vector['x'] < -0.1:
print "move right by {}m".format(abs(vector['x']))
self.motionCommander.right(abs(vector['x']), velocity=0.4)
if vector['z'] > 2.1:
print "move fowards by {}m".format(vector['z'] - 2.0)
self.motionCommander.forward(vector['z'] - 2.0, velocity=0.4)
self
elif vector['z'] < 1.9:
print "move backwards by {}m".format(2.0 - vector['z'])
self.motionCommander.back(2.0 - vector['z'], velocity=0.4)
def disconnect(self):
self.scf.close_link()
def is_close(range):
MIN_DISTANCE = 0.4 # m
if range is None:
return False
else:
return range < MIN_DISTANCE
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 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
from cflib.positioning.motion_commander import MotionCommander
URI = 'radio://0/80/2M'
if __name__ == "__main__":
mp = MotionCommander(URI)
mp.take_off(height=0.3, velocity=0.2)
mp.land(velocity=0.2)
print("zero")
elif (calculator.distance_x > 30):
if (mc.isRunning):
mc.stop()
mc.start_linear_motion(0.1, 0.0, 0.0)
mc.setIsRunning(True)
print("para frente")
elif (calculator.distance_x < 25):
if (mc.isRunning):
mc.stop()
mc.start_linear_motion(-0.1, 0.0, 0.0)
mc.setIsRunning(True)
print("para tras")
else:
if (mc.isRunning):
mc.stop()
cont += 1
if cv2.waitKey(10) & 0xFF == ord('q'):
break
soma = 0
for t in exeTime:
soma += t
print("A media é ", soma / len(exeTime))
cv2.waitKey()
cv2.destroyAllWindows()
mc.land()
sync.close_link()
class Aruco_tracker():
def __init__(self, distance):
"""
Inicialização dos drivers, parâmetros do controle PID e decolagem do drone.
"""
self.distance = distance
self.pid_foward = PID(distance, 0.01, 0.0001, 0.01, 500, -500, 0.7,
-0.7)
self.pid_yaw = PID(0, 0.33, 0.0, 0.33, 500, -500, 100, -100)
self.pid_angle = PID(0.0, 0.01, 0.0, 0.01, 500, -500, 0.3, -0.3)
self.pid_height = PID(0.0, 0.002, 0.0002, 0.002, 500, -500, 0.3, -0.2)
cflib.crtp.init_drivers(enable_debug_driver=False)
self.factory = CachedCfFactory(rw_cache='./cache')
self.URI4 = 'radio://0/40/2M/E7E7E7E7E4'
self.cf = Crazyflie(rw_cache='./cache')
self.sync = SyncCrazyflie(self.URI4, cf=self.cf)
self.sync.open_link()
self.mc = MotionCommander(self.sync)
self.cont = 0
self.notFoundCount = 0
self.calculator = DistanceCalculator()
self.cap = cv2.VideoCapture(1)
self.mc.take_off()
time.sleep(1)
def search_marker(self):
"""
Interrompe o movimento se nao encontrar o marcador por tres frames
consecutivos. Após 4 segundos, inicia movimento de rotação para
procurar pelo marcador.
"""
if ((3 <= self.notFoundCount <= 20) and self.mc.isRunning):
self.mc.stop()
self.mc.setIsRunning(False)
elif (self.notFoundCount > 20):
self.mc._set_vel_setpoint(0.0, 0.0, 0.0, 80)
self.mc.setIsRunning(True)
return
def update_motion(self):
"""
Atualiza as velocidades utilizando controlador PID.
"""
horizontal_distance = self.calculator.horizontal_distance
vertical_distance = self.calculator.vertical_distance
x_distance = self.calculator.distance_x
alpha = self.calculator.alpha
velocity_x = self.pid_foward.update(x_distance)
Velocity_z = self.pid_height.update(vertical_distance)
if (x_distance < (self.distance + 10)):
velocity_y = self.pid_angle.update(alpha)
else:
velocity_y = 0
velocity_z = 0
yaw = self.pid_yaw.update(horizontal_distance)
self.mc._set_vel_setpoint(velocity_x, velocity_y, 0, yaw)
self.mc.setIsRunning(True)
return
def track_marker(self):
"""
Programa principal, drone segue um marcador aruco.
"""
while (self.cap.isOpened()):
ret, frame = self.cap.read()
if (frame is None):
break
if (self.cont % 6 == 0):
thread = threading.Thread(
target=self.calculator.calculateDistance, args=(frame, ))
thread.setDaemon(True)
thread.start()
if (self.calculator.markerFound):
self.notFoundCount = 0
self.update_motion()
else:
self.notFoundCount += 1
self.search_marker()
self.calculator.writeDistance(frame)
cv2.imshow('frame', frame)
self.cont += 1
if cv2.waitKey(10) & 0xFF == ord('q'):
self.mc.land()
cv2.destroyAllWindows()
break
cv2.waitKey()
self.sync.close_link()
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 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
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")
def key_ctrl(self, scf):
mc = MotionCommander(scf)
mc._reset_position_estimator()
print 'Spacebar to start'
raw_input()
pygame.display.set_mode((400, 300))
print 'WASD for throttle & yaw; arrow keys for left/right/forward/backward'
print 'Spacebar to land'
vel_x = 0
vel_y = 0
vel_z = 0
yaw_rate = 0
try:
mc.take_off()
#set inital Yaw value
with open('SensorMaster.txt', 'r') as stbFile:
stbLines = stbFile.readlines()
while len(stbLines) == 0:
with open('SensorMaster.txt', 'r') as stbFile:
stbLines = stbFile.readlines()
currAttitude = stbLines[len(stbLines) - 1]
need, to, currentYaw, test = currAttitude.split(',')
self.yawCurr = float(currentYaw)
Thread(target=self._updateYaw, args=(scf, )).start()
while True:
for event in pygame.event.get():
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_w:
vel_z = 0.3
if event.key == pygame.K_SPACE:
print 'Space pressed, landing'
mc.land()
time.sleep(2)
print 'Closing link...'
scf.close_link()
print 'Link closed'
pygame.quit()
sys.exit(0)
if event.key == pygame.K_a:
yaw_rate = -45
if event.key == pygame.K_s:
vel_z = -0.3
if event.key == pygame.K_d:
yaw_rate = 45
if event.key == pygame.K_UP:
#vel_x = 0.3
vel_x = 0.2 * self.fwCoef[0]
vel_y = 0.2 * self.fwCoef[1]
print('vel_x = %.2f, vel_y = %.2f' %
(vel_x * self.fwCoef[0],
vel_y * self.fwCoef[1]))
print("the current yaw is:")
print(self.yawCurr)
if event.key == pygame.K_DOWN:
#vel_x = -0.3
vel_x = 0.2 * self.bkCoef[0]
vel_y = 0.2 * self.bkCoef[1]
print('vel_x = %.2f, vel_y = %.2f' %
(vel_x * self.bkCoef[0],
vel_y * self.bkCoef[1]))
print("the current yaw is:")
print(self.yawCurr)
if event.key == pygame.K_LEFT:
#vel_y = 0.3
vel_x = 0.2 * self.lfCoef[0]
vel_y = 0.2 * self.lfCoef[1]
print('vel_x = %.2f, vel_y = %.2f' %
(vel_x * self.bkCoef[0],
vel_y * self.bkCoef[1]))
print("the current yaw is:")
print(self.yawCurr)
if event.key == pygame.K_RIGHT:
#vel_y = -0.3
vel_x = 0.2 * self.rtCoef[0]
vel_y = 0.2 * self.rtCoef[1]
print('vel_x = %.2f, vel_y = %.2f' %
(vel_x * self.bkCoef[0],
vel_y * self.bkCoef[1]))
print("the current yaw is:")
print(self.yawCurr)
if event.key == pygame.K_r:
#set recalibrate initial Yaw value
with open('SensorMaster.txt', 'r') as stbFile:
stbLines = stbFile.readlines()
while len(stbLines) == 0:
with open('SensorMaster.txt', 'r') as stbFile:
stbLines = stbFile.readlines()
print("yaw angle recalibrated :")
newInitAttitude = stbLines[len(stbLines) - 1]
need, to, initYaw, test = newInitAttitude.split(
',')
print(initYaw)
self.yawInit = float(initYaw)
if event.key == pygame.K_RCTRL:
mc.stop()
if event.type == pygame.KEYUP:
if event.key == pygame.K_w or event.key == pygame.K_s:
vel_z = 0
if event.key == pygame.K_a or event.key == pygame.K_d:
yaw_rate = 0
if event.key == pygame.K_UP or event.key == pygame.K_DOWN:
vel_x = 0
if event.key == pygame.K_LEFT or event.key == pygame.K_RIGHT:
vel_y = 0
mc._set_vel_setpoint(vel_x, vel_y, vel_z, yaw_rate)
except KeyboardInterrupt:
print('\nShutting down...')
scf.close_link()
except Exception, e:
print str(e)
scf.close_link()
import logging
import time
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.positioning.motion_commander import MotionCommander
URI = 'radio://0/60/2M/E7E7E7E7E7'
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
# We take off when the commander is create
mc = MotionCommander(scf)
mc.take_off(0.5, 0.3)
time.sleep(1)
mc.circle_right(1, velocity=0.6, angle_degrees=180)
mc.up(0.5)
mc.circle_right(1, velocity=0.6, angle_degrees=180)
mc.land(0.2)
class MyCrazyFlieClient:
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 land(self):
self.client.land()
self.scf.close_link()
def check_if_in_target(self, goal):
pos = self.get_position()
distance = np.sqrt(
np.power((goal[0] - pos[0]), 2) + np.power((goal[1] - pos[1]), 2))
if distance < 1:
self.land()
return True
return False
def get_position(self):
self.logger_pos.connect()
data = self.logger_pos.next()[1]
self.logger_pos.disconnect()
self.logger_pos._queue.empty()
return [data['kalman.stateX'], -data['kalman.stateY']]
def get_orientation(self):
self.logger_orientation.connect()
data = self.logger_orientation.next()[1]
self.logger_orientation.disconnect()
self.logger_orientation._queue.empty()
return -data['stabilizer.yaw']
def straight(self, speed):
self.client.forward(0.25, speed)
start = time.time()
return start
def yaw_right(self):
self.client.turn_right(30, 72)
start = time.time()
return start
def yaw_left(self):
self.client.turn_left(30, 72)
start = time.time()
return start
def take_action(self, action):
start = time.time()
collided = False
if action == 0:
start = self.straight(0.25)
if action == 1:
start = self.yaw_right()
if action == 2:
start = self.yaw_left()
# print(start)
return collided
def goal_direction(self, goal, pos):
yaw = self.get_orientation()
print('yaw now %s' % yaw)
pos_angle = math.atan2(goal[1] - pos[1], goal[0] - pos[0])
pos_angle = math.degrees(pos_angle) % 360
print('pos angle %s' % pos_angle)
track = pos_angle - yaw
track = ((track - 180) % 360) - 180
print('Track = %s ' % track)
return track
def observe(self, goal):
position_now = self.get_position()
track = self.goal_direction(goal, position_now)
distance = np.sqrt(
np.power((goal[0] - position_now[0]), 2) +
np.power((goal[1] - position_now[1]), 2))
distance_sensor = Multiranger(self.scf)
distance_sensor.start()
front_distance_sensor = distance_sensor.front
while front_distance_sensor is None:
time.sleep(0.01)
front_distance_sensor = distance_sensor.front
distance_sensor.stop()
print('Position now = %s ' % position_now)
# print('Track = %s ' % track)
print('Distance to target: %s' % distance)
print('Front distance: %s' % front_distance_sensor)
return position_now[0], position_now[
1], track, distance, front_distance_sensor
class CrazyDrone(Drone):
def __init__(self, link_uri):
super().__init__()
cache = "./cache"
if getattr(sys, 'frozen', False):
cache = sys._MEIPASS + os.path.sep + "cache"
self._cf = Crazyflie(rw_cache=cache)
self.motion_commander = None
self.multiranger = None
# maximum speeds
self.max_vert_speed = 1
self.max_horiz_speed = 1
self.max_rotation_speed = 90
self.logger = None
# Connect some callbacks from the Crazyflie API
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)
print('Connecting to %s' % link_uri)
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print('Connected to %s' % link_uri)
self.connection.emit("progress")
# The definition of the logconfig can be made before connecting
self.logger = LogConfig("Battery", 1000) # delay
self.logger.add_variable("pm.vbat", "float")
try:
self._cf.log.add_config(self.logger)
self.logger.data_received_cb.add_callback(
lambda e, f, g: self.batteryValue.emit(float(f['pm.vbat'])))
# self.logger.error_cb.add_callback(lambda: print('error'))
self.logger.start()
except KeyError as e:
print(e)
self.connection.emit("on")
self.motion_commander = MotionCommander(self._cf, 0.5)
self.multiranger = Multiranger(self._cf, rate_ms=50)
self.multiranger.start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
self.is_connected = False
self.connection.emit("off")
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
self.connection.emit("off")
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
self.connection.emit("off")
def take_off(self):
if self._cf.is_connected(
) and self.motion_commander and not self.motion_commander._is_flying:
self.motion_commander.take_off()
self.is_flying_signal.emit(True)
def land(self):
if self._cf.is_connected(
) and self.motion_commander and self.motion_commander._is_flying:
self.motion_commander.land()
self.is_flying_signal.emit(False)
def stop(self):
if not (self.logger is None):
self.logger.stop()
if self.motion_commander:
self.motion_commander.land()
if self.multiranger:
self.multiranger.stop()
self._cf.close_link()
def is_flying(self):
if self._cf.is_connected() and self.motion_commander:
return self.motion_commander._is_flying
return False
def process_motion(self, _up, _rotate, _front, _right):
if self.motion_commander:
# WARNING FOR CRAZYFLY
# positive X is forward, # positive Y is left # positive Z is up
velocity_z = _up * self.max_vert_speed
velocity_yaw = _rotate * self.max_rotation_speed
velocity_x = _front * self.max_horiz_speed
velocity_y = -_right * self.max_horiz_speed
# print("PRE", velocity_x, velocity_y, velocity_z, velocity_yaw)
# protect against collision by reducing speed depending on distance
ranger = self.multiranger
if ranger.front and ranger.front < ANTI_COLLISION_DISTANCE and velocity_x > 0:
velocity_x = velocity_x * (
ranger.front -
ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE
velocity_x = max(0, velocity_x)
if ranger.back and ranger.back < ANTI_COLLISION_DISTANCE and velocity_x < 0:
velocity_x = velocity_x * (
ranger.back -
ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE
velocity_x = min(0, velocity_x)
if ranger.left and ranger.left < ANTI_COLLISION_DISTANCE and velocity_y > 0:
velocity_y = velocity_y * (
ranger.left -
ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE
velocity_y = max(0, velocity_y)
if ranger.right and ranger.right < ANTI_COLLISION_DISTANCE and velocity_y < 0:
velocity_y = velocity_y * (
ranger.left -
ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE
velocity_y = min(0, velocity_y)
if ranger.up and ranger.up < ANTI_COLLISION_DISTANCE and velocity_z > 0:
velocity_z = velocity_z * (ranger.up - ANTI_COLLISION_MIN_DIST
) / ANTI_COLLISION_DISTANCE
velocity_z = max(0, velocity_z)
# print("POST", velocity_x, velocity_y, velocity_z, velocity_yaw)
if self.motion_commander._is_flying:
self.motion_commander._set_vel_setpoint(
velocity_x, velocity_y, velocity_z, velocity_yaw)
class Mission:
def __init__(self, URI):
# Tool to process the data from drone's sensors
self.processing = Processing(URI)
cflib.crtp.init_drivers(enable_debug_driver=False)
self.cf = Crazyflie(ro_cache=None, rw_cache='./cache')
# Connect callbacks from the Crazyflie API
self.cf.connected.add_callback(self.connected)
self.cf.disconnected.add_callback(self.disconnected)
# Connect to the Crazyflie
self.cf.open_link(URI)
time.sleep(3)
self.pose_home = self.position
self.pose_home[2] = 0.1
self.velocity = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0}
self.goal = np.array([0.5, 0.0, 0.3])
self.mc = MotionCommander(self.cf)
time.sleep(3)
self.mc.take_off(0.15, 0.2)
time.sleep(1)
self.mc.circle_right(0.8, velocity=0.5, angle_degrees=360)
time.sleep(2)
self.mc.up(0.1)
time.sleep(1)
self.mc.circle_right(0.8, velocity=0.5, angle_degrees=360)
time.sleep(2)
self.mc.up(0.1)
time.sleep(1)
self.mc.circle_right(0.8, velocity=0.5, angle_degrees=360)
time.sleep(2)
self.mc.land(0.2)
''' Mission to a goal and return to home position '''
# self.mc.take_off(0.5, 0.2)
# time.sleep(1)
# rate = rospy.Rate(10)
# while not rospy.is_shutdown():
# self.sendVelocityCommand()
# rate.sleep()
def coverage_mission(self, height=0.2, length=1.0, width=0.3, numiters=2):
self.mc.take_off(height, 0.2)
time.sleep(1)
for _ in range(numiters):
self.mc.forward(length)
time.sleep(0.1)
self.mc.turn_right(90)
time.sleep(0.1)
self.mc.forward(width)
time.sleep(0.1)
self.mc.turn_right(90)
time.sleep(0.1)
self.mc.forward(length)
time.sleep(0.1)
self.mc.turn_left(90)
time.sleep(0.1)
self.mc.forward(width)
time.sleep(0.1)
self.mc.turn_left(90)
time.sleep(0.1)
self.mc.land(0.2)
time.sleep(1)
def square_mission(self, numiters=2):
'''
Square trajectory to collect data with multiranger.
'''
self.mc.take_off(0.15, 0.2)
time.sleep(1)
self.mc.turn_left(45)
time.sleep(0.1)
# sequence of repeated squares
for _ in range(numiters):
# sqaure
for _ in range(4):
self.mc.forward(1.4)
time.sleep(2)
self.mc.turn_right(90)
time.sleep(1)
self.mc.up(0.1)
time.sleep(1)
for _ in range(numiters):
# sqaure
for _ in range(4):
self.mc.forward(1.4)
time.sleep(2)
self.mc.turn_right(90)
time.sleep(1)
self.mc.land(0.2)
time.sleep(1)
def sendVelocityCommand(self):
direction = normalize(self.goal - self.position)
v_x = SPEED_FACTOR * direction[0]
v_y = SPEED_FACTOR * direction[1]
v_z = SPEED_FACTOR * direction[2]
# Local movement correction from obstacles
dV = 0.1 # [m/s]
if is_close(self.measurement['left']):
# print('Obstacle on the LEFT')
v_y -= dV
if is_close(self.measurement['right']):
# print('Obstacle on the RIGHT')
v_y += dV
self.velocity['x'] = v_x
self.velocity['y'] = v_y
self.velocity['z'] = v_z
# print('Sending velocity:', self.velocity)
goal_dist = norm(self.goal - self.position)
# print('Distance to goal %.2f [m]:' %goal_dist)
if goal_dist < GOAL_TOLERANCE: # goal is reached
# print('Goal is reached. Going home...')
self.goal = self.pose_home
self.cf.commander.send_velocity_world_setpoint(self.velocity['x'],
self.velocity['y'],
self.velocity['z'],
self.velocity['yaw'])
def disconnected(self, URI):
print('Disconnected')
def connected(self, URI):
print('We are now connected to {}'.format(URI))
# The definition of the logconfig can be made before connecting
lpos = LogConfig(name='Position', period_in_ms=100)
lpos.add_variable('lighthouse.x')
lpos.add_variable('lighthouse.y')
lpos.add_variable('lighthouse.z')
lpos.add_variable('stabilizer.roll')
lpos.add_variable('stabilizer.pitch')
lpos.add_variable('stabilizer.yaw')
try:
self.cf.log.add_config(lpos)
lpos.data_received_cb.add_callback(self.pos_data)
lpos.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Position log config, bad configuration.')
lmeas = LogConfig(name='Meas', period_in_ms=100)
lmeas.add_variable('range.front')
lmeas.add_variable('range.back')
lmeas.add_variable('range.up')
lmeas.add_variable('range.left')
lmeas.add_variable('range.right')
lmeas.add_variable('range.zrange')
lmeas.add_variable('stabilizer.roll')
lmeas.add_variable('stabilizer.pitch')
lmeas.add_variable('stabilizer.yaw')
try:
self.cf.log.add_config(lmeas)
lmeas.data_received_cb.add_callback(self.meas_data)
lmeas.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Measurement log config, bad configuration.')
lbat = LogConfig(
name='Battery',
period_in_ms=500) # read battery status with 2 Hz rate
lbat.add_variable('pm.vbat', 'float')
try:
self.cf.log.add_config(lbat)
lbat.data_received_cb.add_callback(self.battery_data)
lbat.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Measurement log config, bad configuration.')
def pos_data(self, timestamp, data, logconf):
# Transformation according to https://wiki.bitcraze.io/doc:lighthouse:setup
position = [
-data['lighthouse.z'], -data['lighthouse.x'], data['lighthouse.y']
# data['stateEstimate.x'],
# data['stateEstimate.y'],
# data['stateEstimate.z']
]
orientation = [
data['stabilizer.roll'], data['stabilizer.pitch'],
data['stabilizer.yaw']
]
self.position = np.array(position)
self.orientation = np.array(orientation)
self.processing.set_position(position, orientation)
def meas_data(self, timestamp, data, logconf):
measurement = {
'roll': data['stabilizer.roll'],
'pitch': data['stabilizer.pitch'],
'yaw': data['stabilizer.yaw'],
'front': data['range.front'],
'back': data['range.back'],
'up': data['range.up'],
'down': data['range.zrange'],
'left': data['range.left'],
'right': data['range.right']
}
self.measurement = measurement
self.processing.set_measurement(measurement)
def battery_data(self, timestamp, data, logconf):
self.V_bat = data['pm.vbat']
# print('Battery status: %.2f [V]' %self.V_bat)
if self.V_bat <= V_BATTERY_TO_GO_HOME:
self.battery_state = 'needs_charging'
# print('Battery is not charged: %.2f' %self.V_bat)
self.goal = self.pose_home
elif self.V_bat >= V_BATTERY_CHARGED:
self.battery_state = 'fully_charged'
class CrazyflieThread(threading.Thread):
def __init__(self, ctrl_message, state_message, message_lock, barrier):
threading.Thread.__init__(self)
self.ctrl = ctrl_message
self.state = state_message
self.lock = message_lock
self.cf = None
self.mc = None
self.scf = None
self.runSM = True
self.cmd_height_old = uglyConst.TAKEOFF_HEIGHT
self.b = barrier
self.descCounter = 0
def raise_exception(self):
self.runSM = False
#-- FSM condition funcitons
def isCloseXYP(self, r):
"""Determines if drone is within radius r and aligned."""
x = self.ctrl.errorx
y = self.ctrl.errory
if (np.sqrt(x*x+y*y) > r) or (np.abs(self.ctrl.erroryaw) > uglyConst.FAR_ANGL):
return False
else:
return True
def isCloseCone(self):
"""Determines if drone within an inverted cone (defined in constants)."""
x = self.ctrl.errorx
y = self.ctrl.errory
r = (self.mc._thread.get_height()-uglyConst.DIST_IGE)*uglyConst.FAR_CONE
if (np.sqrt(x*x+y*y) > r): # or (np.abs(self.ctrl.erroryaw) > uglyConst.FAR_ANGL):
return False
else:
return True
def isClosePix(self):
"""Determines if drone is within radius (in pixels, defined in constants)."""
x = self.ctrl.errorpixx
y = self.ctrl.errorpixy
if (np.sqrt(x*x+y*y) > uglyConst.CLOSE_PIX):
return False
else:
return True
def limOutputVel(self, vx, vy, vz):
"""Limits output of velocity controller."""
return min(vx, uglyConst.MAX_XVEL), min(vy, uglyConst.MAX_YVEL), min(vz, uglyConst.MAX_ZVEL)
#-- FSM state functions
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 enterTakingOff(self):
"""Entry to state: Take off"""
print("enterTakingOff")
self.mc.take_off(uglyConst.TAKEOFF_HEIGHT,uglyConst.TAKEOFF_ZVEL)
def stateTakingOff(self):
"""State: Taking off"""
print("stateTakingOff")
if self.mc._thread.get_height() >= uglyConst.TAKEOFF_HEIGHT:
return self.stateSeeking
else:
time.sleep(0.05)
return self.stateTakingOff
def stateSeeking(self):
"""State: Seeking. Slowly ascend until marker is detected. TODO: Implement some kind of search algorithm (circle outward?)"""
self.mc._set_vel_setpoint(0.0, 0.0, 0.01, 0.0)
print("stateSeeking")
if self.state.isMarkerDetected:
return self.stateNearing
else:
time.sleep(0.05)
return self.stateSeeking
def stateNearing(self):
"""
State: Nearing
Control in pixel frame. Takes in error in pixels (note: camera coordinates), outputs velocity command in x,y,z. Z vel inversely proportional to radius.
"""
x = self.ctrl.errorpixx
y = self.ctrl.errorpixy
cmdx = y*uglyConst.PIX_Kx
cmdy = x*uglyConst.PIX_Ky
r = np.sqrt(x*x+y*y)
if r > 0.0:
cmdz = (1/r)*uglyConst.PIX_Kz
else:
cmdz = 1
cmdx, cmdy, cmdz = self.limOutputVel(cmdx, cmdy, cmdz)
self.mc._set_vel_setpoint(cmdx, cmdy, -cmdz, 0.0)
print("stateNearing")
if self.isClosePix() and self.mc._thread.get_height() < uglyConst.NEARING2APPROACH_HEIGHT:
self.state.cv_mode = uglyConst.CVMODE_POSE
return self.stateApproachin
else:
time.sleep(0.05)
return self.stateNearing
def stateApproachingXY(self):
"""
State: Approaching (XY plane)
Control in the horizontal XY plane and yaw angle.
"""
#self.mc._set_vel_setpoint(self.ctrl.errorx*(uglyConst.Kx+self.ctrl.K), self.ctrl.errory*(uglyConst.Ky+self.ctrl.K), 0.0, 30.0)
self.mc._set_vel_setpoint(self.ctrl.errorx*uglyConst.Kx, self.ctrl.errory*uglyConst.Ky, 0.0, -self.ctrl.erroryaw*uglyConst.Kyaw)
print("stateApproachingXY")
if not self.isClosePix:
return self.stateNearing
if self.isCloseCone() and np.abs(self.ctrl.erroryaw) < uglyConst.FAR_ANGL:
return self.stateApproachingXYZ
else:
time.sleep(0.05)
return self.stateApproachingXY
def stateApproachingXYZ(self):
"""
State: Approaching
Control in world frame. Takes in error in meters, outputs velocity command in x,y. Constant vel cmd in z.
"""
self.mc._set_vel_setpoint(self.ctrl.errorx*uglyConst.Kx, self.ctrl.errory*uglyConst.Ky, -uglyConst.APPROACH_ZVEL, -self.ctrl.erroryaw*uglyConst.Kyaw)
print("stateApproachingXYZ")
if not self.isCloseCone:
return self.stateApproachingXY
elif self.mc._thread.get_height() < uglyConst.APPROACH2LANDING_HEIGHT:
return self.stateDescending
else:
time.sleep(0.05)
return self.stateApproachingXYZ
def stateDescending(self):
"""
State: Landing
Procedure: Descend to a set height, then stop and land.
"""
self.mc._set_vel_setpoint(self.ctrl.errorx*uglyConst.Kx*2.0, self.ctrl.errory*uglyConst.Ky*2.0, -uglyConst.LANDING_ZVEL, -self.ctrl.erroryaw*uglyConst.Kyaw)
print("stateDescending")
if self.mc._thread.get_height() > uglyConst.APPROACH2LANDING_HEIGHT:
return self.stateApproaching
elif self.mc._thread.get_height() < uglyConst.LANDING2LANDED_HEIGHT:
#self.exitLanding()
#return self.stateTerminating
return self.stateLanding
else:
time.sleep(0.01)
return self.stateDescending
def stateLanding(self):
""""""
self.mc._set_vel_setpoint(self.ctrl.errorx*uglyConst.Kx*4.0, self.ctrl.errory*uglyConst.Ky*4.0, -uglyConst.MAX_ZVEL, -self.ctrl.erroryaw*uglyConst.Kyaw*2.0)
self.descCounter += 1
print("stateLanding")
if self.descCounter > 10:
self.mc.land()
return self.stateTerminating
else:
time.sleep(0.01)
return self.stateLanding
def exitLanding(self):
"""
Exit from state: Landing
Stop movement (vel cmds=0), then descend.
"""
self.mc.land()
print("exitLandning")
def stateTerminating(self):
"""
State: Landed
Dummy state.
"""
print("stateTerminating")
return None
def run(self):
"""Main loop, state machine"""
try:
state = self.stateInit # initial state
self.stateNum = uglyConst.S0_INIT
while state and self.runSM:
state = state()
finally:
#-- Clean up
print('Stopping cf_thread')
if self.cf is not None:
if self.mc._is_flying: # if cf has not landed, do so
self.mc.stop()
print('Finally landing')
self.mc.land()
#-- Close link and logging
self.scf.close_link()
self.file.close()
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 CF:
def __init__(self, scf, available):
# get yaw-angle and battery level of crazyflie
self.vbat, self.blevel, self.m1_pwm, self.temp = 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
self.psi = 0
self.roll = 0
self.pitch = 0
self.theta = 0
self.phi = 0
def update(self, des_angle, eul, turnRate):
if 'psi' in eul and abs(des_angle) > self.psi_limit:
self.des_angle = des_angle
if des_angle > 0:
if not self.mc.turn_right(abs(self.des_angle), turnRate):
pass
else:
if not self.mc.turn_left(abs(self.des_angle), turnRate):
pass
# Compute current angle (yaw) of CF
self.vbat, self.blevel, self.m1_pwm, self.temp = getCFparams(self.scf, self.available)
return(self.vbat, self.blevel, self.m1_pwm, self.temp)
# Move cf left or right
def update_x(self, dist):
if dist is not None and dist != 0:
if not self.move_distance(0, -dist, 0):
pass
elif dist is not None and dist == 0 and self.des_angle == 0:
self.mc.stop()
def takeoff(self):
self.mc.take_off()
def land(self):
self.mc.land()
def move_distance(self, x, y, z):
'''
Move in a straight line, {CF frame}.
positive X is forward [cm]
positive Y is left [cm]
positive Z is up [cm]
'''
velocity = 0.08
x = x/100
y = y/100
z = z/100
distance = math.sqrt(x**2 + y**2 + z**2)
if x != 0:
flight_time = distance / velocity
velocity_x = velocity * x / distance
else:
velocity_x = 0
if y != 0:
velocity_y = velocity * y / distance
else:
velocity_y = 0
if z != 0:
velocity_z = velocity * z / distance
else:
velocity_z = 0
self.mc.start_linear_motion(velocity_x, velocity_y, velocity_z)
if x != 0:
time.sleep(flight_time)
return(False)
class Drone:
def __init__(self, URI):
cflib.crtp.init_drivers(enable_debug_driver=False)
self.cf = Crazyflie(ro_cache=None, rw_cache='./cache')
# Connect callbacks from the Crazyflie API
self.cf.connected.add_callback(self.connected)
self.cf.disconnected.add_callback(self.disconnected)
# Connect to the Crazyflie
self.cf.open_link(URI)
# Tool to process the data from drone's sensors
# self.processing = Processing()
self.motion_commander = MotionCommander(self.cf)
time.sleep(3)
self.motion_commander.take_off(0.3, 0.2)
time.sleep(1)
self.motion_commander.start_forward(0.15)
time.sleep(0.5)
self.stop_recording = []
thread.start_new_thread(self.land_command, ())
self.start_mission()
def land_command(self):
raw_input()
self.stop_recording.append(True)
print('Landing...')
self.motion_commander.stop()
self.motion_commander.land(0.2)
time.sleep(1)
def start_mission(self):
rate = rospy.Rate(4000)
while not self.stop_recording:
print('fly')
self.sendHoverCommand()
rate.sleep()
def sendHoverCommand(self):
"""
Change UAV flight direction based on obstacles detected with multiranger.
"""
if is_close(self.measurement['front']
) and self.measurement['left'] > self.measurement['right']:
print('FRONT RIGHT')
self.motion_commander.stop()
self.motion_commander.turn_left(60, 70)
self.motion_commander.start_forward(0.15)
if is_close(self.measurement['front']
) and self.measurement['left'] < self.measurement['right']:
print('FRONT LEFT')
self.motion_commander.stop()
self.motion_commander.turn_right(60, 70)
self.motion_commander.start_forward(0.15)
if is_close(self.measurement['left']):
print('LEFT')
self.motion_commander.right(0.1, 0.2)
self.motion_commander.stop()
self.motion_commander.turn_right(45, 70)
self.motion_commander.start_forward(0.15)
if is_close(self.measurement['right']):
print('RIGHT')
self.motion_commander.left(0.1, 0.2)
self.motion_commander.stop()
self.motion_commander.turn_left(45, 70)
self.motion_commander.start_forward(0.15)
def disconnected(self, URI):
print('Disconnected')
def connected(self, URI):
print('We are now connected to {}'.format(URI))
# The definition of the logconfig can be made before connecting
lpos = LogConfig(name='Position', period_in_ms=100)
lpos.add_variable('lighthouse.x')
lpos.add_variable('lighthouse.y')
lpos.add_variable('lighthouse.z')
lpos.add_variable('stabilizer.roll')
lpos.add_variable('stabilizer.pitch')
lpos.add_variable('stabilizer.yaw')
try:
self.cf.log.add_config(lpos)
lpos.data_received_cb.add_callback(self.pos_data)
lpos.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Position log config, bad configuration.')
lmeas = LogConfig(name='Meas', period_in_ms=100)
lmeas.add_variable('range.front')
lmeas.add_variable('range.back')
lmeas.add_variable('range.up')
lmeas.add_variable('range.left')
lmeas.add_variable('range.right')
lmeas.add_variable('range.zrange')
lmeas.add_variable('stabilizer.roll')
lmeas.add_variable('stabilizer.pitch')
lmeas.add_variable('stabilizer.yaw')
try:
self.cf.log.add_config(lmeas)
lmeas.data_received_cb.add_callback(self.meas_data)
lmeas.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Measurement log config, bad configuration.')
lbat = LogConfig(
name='Battery',
period_in_ms=500) # read battery status with 2 Hz rate
lbat.add_variable('pm.vbat', 'float')
try:
self.cf.log.add_config(lbat)
lbat.data_received_cb.add_callback(self.battery_data)
lbat.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Measurement log config, bad configuration.')
def pos_data(self, timestamp, data, logconf):
# Transformation according to https://wiki.bitcraze.io/doc:lighthouse:setup
position = [
-data['lighthouse.z'], -data['lighthouse.x'], data['lighthouse.y']
# data['stateEstimate.x'],
# data['stateEstimate.y'],
# data['stateEstimate.z']
]
orientation = [
data['stabilizer.roll'], data['stabilizer.pitch'],
data['stabilizer.yaw']
]
self.position = position
self.orientation = orientation
# self.processing.set_position(position, orientation)
def meas_data(self, timestamp, data, logconf):
measurement = {
'roll': data['stabilizer.roll'],
'pitch': data['stabilizer.pitch'],
'yaw': data['stabilizer.yaw'],
'front': data['range.front'],
'back': data['range.back'],
'up': data['range.up'],
'down': data['range.zrange'],
'left': data['range.left'],
'right': data['range.right']
}
self.measurement = measurement
# self.processing.set_measurement(measurement)
def battery_data(self, timestamp, data, logconf):
self.V_bat = data['pm.vbat']
# print('Battery status: %.2f [V]' %self.V_bat)
if self.V_bat <= V_BATTERY_TO_GO_HOME:
self.battery_state = 'needs_charging'
# print('Battery is not charged: %.2f' %self.V_bat)
elif self.V_bat >= V_BATTERY_CHARGED:
self.battery_state = 'fully_charged'
