def main():
try:
#logfile reset
myfile = open('SensorMaster.txt', 'w')
myfile.write
myfile.close()
cflib.crtp.init_drivers(enable_debug_driver=False)
scf = SyncCrazyflie(URI)
fc = FlightCtrl(scf)
m = Myo()
Thread(target=fc.gesture_ctrl, args=(scf, fc, gesture)).start()
Thread(target=m.gesture_detection, args=(gesture, )).start()
while True:
time.sleep(0.5)
except KeyboardInterrupt:
print('\nClosing link...')
#perhaps land gracefully here
scf.close_link()
print('Shutting down...')
os.kill(os.getpid(), signal.SIGINT)
except Exception, e:
print(str(e))
print('\nShutting down...')
scf.close_link()
sys.exit(0)
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
def main():
try:
#logfile reset
myfile = open('SensorMaster.txt', 'w')
myfile.write
myfile.close()
cflib.crtp.init_drivers(enable_debug_driver=False)
scf = SyncCrazyflie(URI)
fc = FlightCtrl(scf)
m = Myo()
Thread(target=fc.gesture_ctrl, args=(fc, gesture)).start()
Thread(target=m.gesture_detection, args=(gesture, )).start()
prev_t = 0
while True:
t = int(time.time())
if scf.vbat is not None:
if t % 20 == 0 and t != prev_t: # print every 20 seconds
print "Battery voltage: %.2fV" % (scf.vbat, )
prev_t = t
if t % 5 == 0 and t != prev_t and scf.vbat > MIN_VBAT and scf.vbat < WARN_VBAT:
print "WARN: Battery voltage: %.2fV. Landing soon..." % (
scf.vbat, )
prev_t = t
if t % 5 == 0 and scf.vbat < MIN_VBAT:
print "Battery voltage %.2f too low; landing..." % (
scf.vbat)
fc.mc.land()
os.kill(os.getpid(), signal.SIGINT)
time.sleep(0.1)
except KeyboardInterrupt:
print('\nClosing link...')
scf.close_link()
print('Shutting down...')
os.kill(os.getpid(), signal.SIGINT)
except Exception, e:
print(str(e))
print('\nShutting down...')
scf.close_link()
sys.exit(0)
class Node:
def __init__(self):
self.vx = 0
self.vy = 0
self.vz = 0
self.va = 0
self.vel = Twist()
self.vel.linear.x = self.vx
self.vel.linear.y = self.vy
self.vel.linear.z = self.vz
self.vel.angular.z = self.va
self.vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
cflib.crtp.init_drivers(enable_debug_driver=False)
self.crazyflie = SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache'))
self.commander = MotionCommander(self.crazyflie)
self.cf = Crazyflie()
self.crazyflie.open_link()
self.commander.take_off()
def shut_down(self):
try:
self.pitch_log.stop()
finally:
self.crazyflie.close_link()
def run_node(self):
self.vel.linear.x = 0
self.vel.linear.y = 0
self.vel.linear.z = 0.2
self.vel.angular.z = 0
self.vel_pub.publish(self.vel)
self.commander._set_vel_setpoint(0, 0, 0.2, 0)
time.sleep(3)
self.vel.linear.x = 0
self.vel.linear.y = 0
self.vel.linear.z = 0
self.vel.angular.z = 72
self.vel_pub.publish(self.vel)
self.commander._set_vel_setpoint(0, 0, 0, 72)
time.sleep(3)
self.vel.linear.x = 0
self.vel.linear.y = 0
self.vel.linear.z = -0.2
self.vel.angular.z = 0
self.vel_pub.publish(self.vel)
self.commander._set_vel_setpoint(0, 0, -0.2, 0)
time.sleep(3)
def initDrone(self, posAddressList):
print("Resetting and locating ", self.address)
scf = SyncCrazyflie(self.address, cf=Crazyflie(rw_cache='./cache'))
scf.open_link()
self.reset_estimator(scf)
self.start_position_printing(scf)
time.sleep(0.2)
self.pos
posAddressList.append([self.pos, self.address])
print("added", [self.pos, self.address])
scf.close_link()
class CfCom:
def __init__(self, URI):
cflib.crtp.init_drivers(enable_debug_driver=False)
self.URI = URI
self.scf = SyncCrazyflie(self.URI, cf=Crazyflie(rw_cache='./cache'))
def __del__(self):
self.scf.close_link()
def open_link(self):
self.scf.open_link()
def close_link(self):
self.scf.close_link()
def __enter__(self):
self.scf.open_link()
return self
def __exit__(self, exc_type, exc_val, exc_tb):
self.scf.close_link()
class Drone:
# RIGIDBODYMASS = 1
# RIGIDBODYRADIUS = 0.1
# LINEARDAMPING = 0.97
# SENSORRANGE = 0.6
# TARGETFORCE = 3
# AVOIDANCEFORCE = 25
# FORCEFALLOFFDISTANCE = .5
RIGIDBODYMASS = 0.5
RIGIDBODYRADIUS = 0.1
LINEARDAMPING = 0.95
SENSORRANGE = 0.6
TARGETFORCE = 1
AVOIDANCEFORCE = 10
FORCEFALLOFFDISTANCE = .5
def __init__(self,
manager,
position: Vec3,
uri="-1",
printDebugInfo=False):
self.base = manager.base
self.manager = manager
# The position of the real drone this virtual drone is connected to.
# If a connection is active, this value is updated each frame.
self.realDronePosition = Vec3(0, 0, 0)
self.canConnect = False # true if the virtual drone has a uri to connect to a real drone
self.isConnected = False # true if the connection to a real drone is currently active
self.uri = uri
if self.uri != "-1":
self.canConnect = True
self.id = int(uri[-1])
self.randVec = Vec3(random.uniform(-1, 1), random.uniform(-1, 1),
random.uniform(-1, 1))
# add the rigidbody to the drone, which has a mass and linear damping
self.rigidBody = BulletRigidBodyNode(
"RigidSphere") # derived from PandaNode
self.rigidBody.setMass(self.RIGIDBODYMASS) # body is now dynamic
self.rigidBody.addShape(BulletSphereShape(self.RIGIDBODYRADIUS))
self.rigidBody.setLinearSleepThreshold(0)
self.rigidBody.setFriction(0)
self.rigidBody.setLinearDamping(self.LINEARDAMPING)
self.rigidBodyNP = self.base.render.attachNewNode(self.rigidBody)
self.rigidBodyNP.setPos(position)
# self.rigidBodyNP.setCollideMask(BitMask32.bit(1))
self.base.world.attach(self.rigidBody)
# add a 3d model to the drone to be able to see it in the 3d scene
model = self.base.loader.loadModel(self.base.modelDir +
"/drones/drone1.egg")
model.setScale(0.2)
model.reparentTo(self.rigidBodyNP)
self.target = position # the long term target that the virtual drones tries to reach
self.setpoint = position # the immediate target (setpoint) that the real drone tries to reach, usually updated each frame
self.waitingPosition = Vec3(position[0], position[1], 0.7)
self.lastSentPosition = self.waitingPosition # the position that this drone last sent around
self.printDebugInfo = printDebugInfo
if self.printDebugInfo: # put a second drone model on top of drone that outputs debug stuff
model = self.base.loader.loadModel(self.base.modelDir +
"/drones/drone1.egg")
model.setScale(0.4)
model.setPos(0, 0, .2)
model.reparentTo(self.rigidBodyNP)
# initialize line renderers
self.targetLineNP = self.base.render.attachNewNode(LineSegs().create())
self.velocityLineNP = self.base.render.attachNewNode(
LineSegs().create())
self.forceLineNP = self.base.render.attachNewNode(LineSegs().create())
self.actualDroneLineNP = self.base.render.attachNewNode(
LineSegs().create())
self.setpointNP = self.base.render.attachNewNode(LineSegs().create())
def connect(self):
"""Connects the virtual drone to a real one with the uri supplied at initialization."""
if not self.canConnect:
return
print(self.uri, "connecting")
self.isConnected = True
self.scf = SyncCrazyflie(self.uri, cf=Crazyflie(rw_cache='./cache'))
self.scf.open_link()
self._reset_estimator()
self.start_position_printing()
# MOVE THIS BACK TO SENDPOSITIONS() IF STUFF BREAKS
self.scf.cf.param.set_value('flightmode.posSet', '1')
def sendPosition(self):
"""Sends the position of the virtual drone to the real one."""
cf = self.scf.cf
self.setpoint = self.getPos()
# send the setpoint
cf.commander.send_position_setpoint(self.setpoint[0], self.setpoint[1],
self.setpoint[2], 0)
def disconnect(self):
"""Disconnects the real drone."""
print(self.uri, "disconnecting")
self.isConnected = False
cf = self.scf.cf
cf.commander.send_stop_setpoint()
time.sleep(0.1)
self.scf.close_link()
def update(self):
"""Update the virtual drone."""
# self.updateSentPositionBypass(0)
self._updateTargetForce()
self._updateAvoidanceForce()
self._clampForce()
if self.isConnected:
self.sendPosition()
# draw various lines to get a better idea of whats happening
self._drawTargetLine()
# self._drawVelocityLine()
self._drawForceLine()
# self._drawSetpointLine()
self._printDebugInfo()
def updateSentPosition(self, timeslot):
transmissionProbability = 1
if self.id == timeslot:
if random.uniform(0, 1) < transmissionProbability:
# print(f"drone {self.id} updated position")
self.lastSentPosition = self.getPos()
else:
pass
# print(f"drone {self.id} failed!")
def updateSentPositionBypass(self, timeslot):
self.lastSentPosition = self.getPos()
def getPos(self) -> Vec3:
return self.rigidBodyNP.getPos()
def getLastSentPos(self) -> Vec3:
return self.lastSentPosition
def _updateTargetForce(self):
"""Applies a force to the virtual drone which moves it closer to its target."""
dist = (self.target - self.getPos())
if (dist.length() > self.FORCEFALLOFFDISTANCE):
force = dist.normalized()
else:
force = (dist / self.FORCEFALLOFFDISTANCE)
self.addForce(force * self.TARGETFORCE)
def _updateAvoidanceForce(self):
"""Applies a force the the virtual drone which makes it avoid other drones."""
# get all drones within the sensors reach and put them in a list
nearbyDrones = []
for drone in self.manager.drones:
dist = (drone.getLastSentPos() - self.getPos()).length()
if drone.id == self.id: # prevent drone from detecting itself
continue
if dist < self.SENSORRANGE:
nearbyDrones.append(drone)
# calculate and apply forces
for nearbyDrone in nearbyDrones:
distVec = nearbyDrone.getLastSentPos() - self.getPos()
if distVec.length() < 0.2:
print("BONK")
distMult = self.SENSORRANGE - distVec.length()
avoidanceDirection = self.randVec.normalized(
) * 2 - distVec.normalized() * 10
avoidanceDirection.normalize()
self.addForce(avoidanceDirection * distMult * self.AVOIDANCEFORCE)
def _clampForce(self):
"""Clamps the total force acting in the drone."""
totalForce = self.rigidBody.getTotalForce()
if totalForce.length() > 2:
self.rigidBody.clearForces()
self.rigidBody.applyCentralForce(totalForce.normalized())
def targetVector(self) -> Vec3:
"""Returns the vector from the drone to its target."""
return self.target - self.getPos()
def _printDebugInfo(self):
if self.printDebugInfo:
pass
def setTarget(self, target: Vec3):
"""Sets a new target for the drone."""
self.target = target
def setRandomTarget(self):
"""Sets a new random target for the drone."""
self.target = self.manager.getRandomRoomCoordinate()
def setNewRandVec(self):
self.randVec = Vec3(random.uniform(-1, 1), random.uniform(-1, 1),
random.uniform(-1, 1))
def addForce(self, force: Vec3):
self.rigidBody.applyCentralForce(force)
def setPos(self, position: Vec3):
self.rigidBodyNP.setPos(position)
def getVel(self) -> Vec3:
return self.rigidBody.getLinearVelocity()
def setVel(self, velocity: Vec3):
return self.rigidBody.setLinearVelocity(velocity)
def _drawTargetLine(self):
self.targetLineNP.removeNode()
ls = LineSegs()
# ls.setThickness(1)
ls.setColor(1.0, 0.0, 0.0, 1.0)
ls.moveTo(self.getPos())
ls.drawTo(self.target)
node = ls.create()
self.targetLineNP = self.base.render.attachNewNode(node)
def _drawVelocityLine(self):
self.velocityLineNP.removeNode()
ls = LineSegs()
# ls.setThickness(1)
ls.setColor(0.0, 0.0, 1.0, 1.0)
ls.moveTo(self.getPos())
ls.drawTo(self.getPos() + self.getVel())
node = ls.create()
self.velocityLineNP = self.base.render.attachNewNode(node)
def _drawForceLine(self):
self.forceLineNP.removeNode()
ls = LineSegs()
# ls.setThickness(1)
ls.setColor(0.0, 1.0, 0.0, 1.0)
ls.moveTo(self.getPos())
ls.drawTo(self.getPos() + self.rigidBody.getTotalForce() * 0.2)
node = ls.create()
self.forceLineNP = self.base.render.attachNewNode(node)
def _drawSetpointLine(self):
self.setpointNP.removeNode()
ls = LineSegs()
# ls.setThickness(1)
ls.setColor(1.0, 1.0, 1.0, 1.0)
ls.moveTo(self.getPos())
ls.drawTo(self.setpoint)
node = ls.create()
self.setpointNP = self.base.render.attachNewNode(node)
def _wait_for_position_estimator(self):
"""Waits until the position estimator reports a consistent location after resetting."""
print('Waiting for estimator to find position...')
log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
log_config.add_variable('kalman.varPX', 'float')
log_config.add_variable('kalman.varPY', 'float')
log_config.add_variable('kalman.varPZ', 'float')
var_y_history = [1000] * 10
var_x_history = [1000] * 10
var_z_history = [1000] * 10
threshold = 0.001
with SyncLogger(self.scf, log_config) as logger:
for log_entry in logger:
data = log_entry[1]
var_x_history.append(data['kalman.varPX'])
var_x_history.pop(0)
var_y_history.append(data['kalman.varPY'])
var_y_history.pop(0)
var_z_history.append(data['kalman.varPZ'])
var_z_history.pop(0)
min_x = min(var_x_history)
max_x = max(var_x_history)
min_y = min(var_y_history)
max_y = max(var_y_history)
min_z = min(var_z_history)
max_z = max(var_z_history)
if (max_x - min_x) < threshold and (
max_y - min_y) < threshold and (max_z -
min_z) < threshold:
break
def _reset_estimator(self):
"""Resets the position estimator, this should be run before flying the drones or they might report a wrong position."""
cf = self.scf.cf
cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', '0')
self._wait_for_position_estimator()
def position_callback(self, timestamp, data, logconf):
"""Updates the variable holding the position of the actual drone. It is not called in the update method, but by the drone itself (I think)."""
x = data['kalman.stateX']
y = data['kalman.stateY']
z = data['kalman.stateZ']
self.realDronePosition = Vec3(x, y, z)
# print('pos: ({}, {}, {})'.format(x, y, z))
def start_position_printing(self):
"""Activate logging of the position of the real drone."""
log_conf = LogConfig(name='Position', period_in_ms=50)
log_conf.add_variable('kalman.stateX', 'float')
log_conf.add_variable('kalman.stateY', 'float')
log_conf.add_variable('kalman.stateZ', 'float')
self.scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(self.position_callback)
log_conf.start()
cflib.crtp.init_drivers(enable_debug_driver=False)
scf = SyncCrazyflie(URI)
startMotion = flightCtrl(scf)
print('I will graph at one point')
fig = plt.figure()
axRoll = fig.add_subplot(3, 1, 1)
axYaw = fig.add_subplot(3, 1, 2)
axPitch = fig.add_subplot(3, 1, 3)
animate = displayStb(axRoll, axYaw, axPitch)
ani = animation.FuncAnimation(fig, animate.update, interval=20)
plt.show()
except KeyboardInterrupt:
print('\nCtrl-C detected, shutting down...')
pygame.quit()
plt.close("all")
scf.close_link()
sys.exit(0)
except Exception, e:
print(str(e))
print('\nShutting down...')
pygame.quit()
plt.close("all")
scf.close_link()
sys.exit(0)
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()
pr.execute() pr.putCommand(paralelo.HORARIO, mc1) # pr.putCommand(paralelo.HORARIO, mc2) pr.execute() pr.putCommand(paralelo.ZIGUEZAGUE, mc1) # pr.putCommand(paralelo.ZIGUEZAGUE, mc2) pr.execute() pr.putCommand(paralelo.DIAGONAL, mc1) # pr.putCommand(paralelo.DIAGONAL, mc2) pr.execute() pr.putCommand(paralelo.ARCO, mc1) # pr.putCommand(paralelo.DEGRAU, mc2) pr.execute() pr.putCommand(paralelo.LOOP, mc1) pr.execute() pr.putCommand(paralelo.ESPIRAL, mc1) pr.execute() pr.putCommand(paralelo.LAND, mc1) # pr.putCommand(paralelo.LAND, mc2) pr.execute() scf1.close_link() # scf2.close_link()
class CrazyflieConnection(connection.Connection):
DEFAULT_VELOCITY = 0.2 # [m/s] the default velocity to use for position commands
def __init__(self, uri, threaded=False):
super().__init__(threaded)
# TODO: maybe add a parameter so people can change the default velocity
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# the connection to the crazyflie
self._scf = SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache'))
# temp (or maybe will be permanent) state variable
self._is_open = False
self._running = False
self._send_rate = 5 # want to send messages at 5Hz NOTE: the minimum is 2 Hz
self._out_msg_queue = queue.Queue(
) # a queue for sending data between threads
self._write_handle = threading.Thread(target=self.command_loop)
self._write_handle.daemon = True
# since can only command velocities and not positions, the connection
# needs some awareness of the current position to be able to do
# the math necessary
self._current_position_xyz = [0.0, 0.0, 0.0] # [x, y, z]
# state information is to be updated and managed by this connection class
# for the crazyflie, since the crazyflie doesn't exactly pass down the
# state information
#
# defining the states to be:
# armed -> should roughly mimic connection state (though this does have a problem at the end...)
# guided -> this seems to only be used at the end condition.....
@property
def open(self):
"""
Returns:
Boolean. True if connection is able to send and/or receive messages, False otherwise.
"""
# TODO: figure out the open condition for crazyflie
if self._is_open == -1:
return False
return True
def start(self):
"""Command to start a connection with a drone"""
self._scf.open_link(
) # this is a blocking function that will not return until the link is opened
# TODO: need a better version of this
self._is_open = True
# need to now register for callbacks on the data of interest from the crazyflie
# TODO: decide on the appropriate rates
log_pos = LogConfig(name='LocalPosition', period_in_ms=100)
log_pos.add_variable('kalman.stateX', 'float')
log_pos.add_variable('kalman.stateY', 'float')
log_pos.add_variable('kalman.stateZ', 'float')
try:
self._scf.cf.log.add_config(log_pos)
# This callback will receive the data
log_pos.data_received_cb.add_callback(self._cf_callback_pos)
# This callback will be called on errors
log_pos.error_cb.add_callback(self._cf_callback_error)
# Start the logging
log_pos.start()
except KeyError as e:
print('Could not start position log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Position log config, bad configuration.')
log_vel = LogConfig(name='LocalVelocity', period_in_ms=500)
log_vel.add_variable('kalman.statePX', 'float')
log_vel.add_variable('kalman.statePY', 'float')
log_vel.add_variable('kalman.statePZ', 'float')
try:
self._scf.cf.log.add_config(log_vel)
# This callback will receive the data
log_vel.data_received_cb.add_callback(self._cf_callback_vel)
# This callback will be called on errors
log_vel.error_cb.add_callback(self._cf_callback_error)
# Start the logging
log_vel.start()
except KeyError as e:
print('Could not start velocity log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add velocity log config, bad configuration.')
log_att = LogConfig(name='Attitude', period_in_ms=100)
log_att.add_variable('stabilizer.roll', 'float')
log_att.add_variable('stabilizer.pitch', 'float')
log_att.add_variable('stabilizer.yaw', 'float')
try:
self._scf.cf.log.add_config(log_att)
# This callback will receive the data
log_att.data_received_cb.add_callback(self._cf_callback_att)
# This callback will be called on errors
log_att.error_cb.add_callback(self._cf_callback_error)
# Start the logging
log_att.start()
except KeyError as e:
print('Could not start attitude log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add attitude log config, bad configuration.')
log_state = LogConfig(name='State', period_in_ms=1000)
log_state.add_variable('kalman.inFlight',
'uint8_t') # TODO: check the data type
try:
self._scf.cf.log.add_config(log_state)
log_state.data_received_cb.add_callback(self._cf_callback_state)
log_state.error_cb.add_callback(self._cf_callback_error)
# Start the logging
log_state.start()
except KeyError as e:
print('Could not start position log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add state log config, bad configuration.')
# start the write thread now that the connection is open
self._running = True
self._write_handle.start()
def stop(self):
"""Command to stop a connection with a drone"""
# need to send a stop command
cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_STOP, None)
self._out_msg_queue.put(cmd)
time.sleep(
0.5
) # add a sleep to make sure this command is sent and executed properly
self._running = False # this is to stop the command thread
time.sleep(1) # make sure the command thread has stopped
self._scf.close_link() # close the link
# TODO: find a better way to handle this...
self._is_open = False
def dispatch_loop(self):
"""Main loop that triggers callbacks as messages are recevied"""
# NOTE: nothing needed here for the crazyflie
# NOTE: the crazyflie API sends data down as callbacks already, so this
# NOTE: connection class is nothing but a passthrough for those
pass
def command_loop(self):
"""loop to send commands at a specified rate"""
# the last time a command was sent
# to be used to ensure commands are at the desired rate
last_write_time = time.time()
cmd_start_time = 0 # the time [s] that the command started -> needed for distance commands
# the current command that should be being sent, default to 0 everything
current_cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_STOP, None)
# the last commanded height
# if this is not 0, want the hold commands to be hover to hold the specific height
current_height = 0
while self._running:
# empty out the queue of pending messages -> want to always send the messages asap
# NOTE: this effectively only sends the last command in the queue....
# TODO: see if need to handle the fact that maybe want to send all the commands in the queue...
cmd = None
new_cmd = False
while not self._out_msg_queue.empty():
try:
cmd = self._out_msg_queue.get_nowait()
except queue.Empty:
# if there is no msgs in the queue, will just continue
pass
else:
if cmd is not None:
new_cmd = True
current_cmd = cmd
cmd_start_time = time.time()
# TODO: maybe want to handle the command here...
self._out_msg_queue.task_done()
# DEBUG
# print("recevied command, type {}, cmd {}, delay {}".format(
# current_cmd.type, current_cmd.cmd, current_cmd.delay))
# first thing to check is the timer, if applicable
if current_cmd.delay is not None:
if time.time() - cmd_start_time >= current_cmd.delay:
# DEBUG
# print("command timer completed, completed command: {}, {}".format(
# current_cmd.type, current_cmd.cmd))
# time to stop
new_cmd = True
if current_height > 0:
current_cmd = CrazyflieCommand(
CrazyflieCommand.CMD_TYPE_HOVER,
(0.0, 0.0, 0.0, current_height))
else:
current_cmd = CrazyflieCommand(
CrazyflieCommand.CMD_TYPE_VELOCITY,
(0.0, 0.0, 0.0, 0.0))
# if this isn't a new command, want to rate limit accordingly
# rate limit the loop
if not new_cmd:
current_time = time.time()
if (current_time - last_write_time) < (1.0 / self._send_rate):
continue
last_write_time = time.time()
# DEBUG
# print("sending command, type {}, cmd {}, delay {}".format(
# current_cmd.type, current_cmd.cmd, current_cmd.delay))
# TODO: probably need to constantly update the velocity commands here....
# TODO: effectively need to wrap a high level control loop, at the very least on height
# now do the actual sending of the command
if current_cmd.type == CrazyflieCommand.CMD_TYPE_VELOCITY:
self._scf.cf.commander.send_velocity_world_setpoint(
*current_cmd.cmd)
elif current_cmd.type == CrazyflieCommand.CMD_TYPE_HOVER:
current_height = current_cmd.cmd[3]
self._scf.cf.commander.send_hover_setpoint(*current_cmd.cmd)
elif current_cmd.type == CrazyflieCommand.CMD_TYPE_ATTITUDE_THRUST:
self._scf.cf.commander.send_setpoint(*current_cmd.cmd)
elif current_cmd.type == CrazyflieCommand.CMD_TYPE_ATTITUDE_DIST:
current_height = current_cmd.cmd[3]
self._scf.cf.commander.send_zdistance_setpoint(
*current_cmd.cmd)
elif current_cmd.type == CrazyflieCommand.CMD_TYPE_STOP:
# TODO: probably want to send appropriate flags that state disarmed, etc
# TODO: basically need to update the drone state here
self._scf.cf.commander.send_stop_setpoint()
else:
print("invalid command type!")
def _cf_callback_pos(self, timestamp, data, logconf):
x = data['kalman.stateX']
y = data['kalman.stateY']
z = data['kalman.stateZ']
# print("current height: {}".format(z))
self._current_position_xyz = [x, y, z] # save for our internal use
pos = mt.LocalFrameMessage(timestamp, x, -y, -z)
self.notify_message_listeners(MsgID.LOCAL_POSITION, pos)
def _cf_callback_vel(self, timestamp, data, logconf):
x = data['kalman.statePX']
y = data['kalman.statePY']
z = data['kalman.statePZ']
vel = mt.LocalFrameMessage(timestamp, x, y, z)
self.notify_message_listeners(MsgID.LOCAL_VELOCITY, vel)
def _cf_callback_att(self, timestamp, data, logconf):
roll = data['stabilizer.roll']
pitch = data['stabilizer.pitch']
yaw = data['stabilizer.yaw']
fm = mt.FrameMessage(timestamp, roll, pitch, yaw)
self.notify_message_listeners(MsgID.ATTITUDE, fm)
def _cf_callback_state(self, timestamp, data, logconf):
# in_flight = data['kalman.inFlight']
# armed = False
# guided = False
# if in_flight:
# armed = True
# guided = True
# TODO: probably need a better metric for armed / guided
# since the quad is basically always armed and guided comes into play
# once the connection is made, so basically the second the script starts...
# state = mt.StateMessage(timestamp, armed, guided)
# self.notify_message_listeners(MsgID.STATE, state)
pass
def _cf_callback_error(self, logconf, msg):
print('Error when logging %s: %s' % (logconf.name, msg))
def _reset_position_estimator(self):
"""reset the estimator to give the best performance possible"""
self._scf.cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
self._scf.cf.param.set_value('kalman.resetEstimation', '0')
# TODO: instead of a sleep, probably want a condition on the variance
time.sleep(2)
def arm(self):
"""Command to arm the drone"""
# NOTE: this doesn't exist for the crazyflie
# TODO: could have arm or take control be where we reset the estimator...
pass
def disarm(self):
"""Command to disarm the drone"""
# NOTE: this doesn't exist for the crazyflie
pass
def take_control(self):
"""
Command the drone to switch into a mode that allows external control.
e.g. for PX4 this commands 'offboard' mode, while for APM this commands 'guided' mode
"""
# NOTE: this doesn't exist for the crazyflie
pass
def release_control(self):
"""Command to return the drone to a manual mode"""
# NOTE: this doesn't exist for the crazyflie
pass
def cmd_attitude(self, roll, pitch, yawrate, thrust):
"""Command to set the desired attitude and thrust
Args:
yaw: the desired yaw in radians
pitch: the desired pitch in radians
roll: the deisred roll in radians
thrust: the normalized desired thrust level on [0, 1]
"""
pass
def cmd_attitude_rate(self, roll_rate, pitch_rate, yaw_rate, thrust):
"""Command to set the desired attitude rates and thrust
Args:
yaw_rate: the desired yaw rate in radians/second
pitch_rate: the desired pitch rate in radians/second
roll_rate: the desired roll rate in radians/second
thrust: the normalized desired thrust level on [0, 1]
"""
pass
def cmd_moment(self, roll_moment, pitch_moment, yaw_moment, thrust):
"""Command to set the desired moments and thrust
Args:
roll_moment: the desired roll moment in Newton*meter
yaw_moment: the desired yaw moment in Newton*meter
pitch_moment: the desired pitch moment in Newton*meter
thrust: the normalized desired thrust level in Newton
"""
pass
def cmd_velocity(self, vn, ve, vd, heading):
"""Command to set the desired velocity (NED frame) and heading
Args:
vn: desired north velocity component in meters/second
ve: desired east velocity component in meters/second
vd: desired down velocity component in meters/second (note: positive down!)
heading: desired drone heading in radians
"""
pass
def cmd_motors(self, motor1, motor2, motor3, motor4):
"""Command the thrust levels for each motor on a quadcopter
Args:
motor1: normalized thrust level for motor 1 on [0, 1]
motor2: normalized thrust level for motor 2 on [0, 1]
motor3: normalized thrust level for motor 3 on [0, 1]
motor4: normalized thrust level for motor 4 on [0, 1]
"""
pass
def cmd_position(self, n, e, d, heading):
""" NOTE: THIS CURRENTLY COMMANDS RELATIVE POSITION!!!!! BODY ALIGNED!!!! """
"""Command to set the desired position (NED frame) and heading
Args:
n: desired north position in meters
e: desired east position in meters
d: desired down position in meters (note: positive down!)
heading: desired drone heading in radians
"""
# need to know the current position: for now going to simply map NED to XYZ!!!
# x is forward
# y is left
# z is up
# also completely ignoring heading for now
# DEBUG
# print("current position (x,y,z): ({}, {}, {})".format(
# self._current_position_xyz[0], self._current_position_xyz[1], self._current_position_xyz[2]))
# print("commanded position (x,y,z): ({}, {}, {})".format(n, -e, -d))
# calculate the change vector needed
# note the slight oddity that happens in converting NED to XYZ
# as things are used as XYZ internally for the crazyflie
dx = n - self._current_position_xyz[0]
dy = -e - self._current_position_xyz[1]
z = -1 * d # holding a specific altitude, so just pass altitude through directly
# DEBUG
# print("move vector: ({}, {}) at height {}".format(dx, dy, z))
distance = math.sqrt(dx * dx + dy * dy)
delay_time = distance / self.DEFAULT_VELOCITY
# DEBUG
# print("the delay time for the move command: {}".format(delay_time))
# need to now calculate the velocity vector -> need to have a magnitude of default velocity
vx = self.DEFAULT_VELOCITY * dx / distance
vy = self.DEFAULT_VELOCITY * dy / distance
# DEBUG
# print("vel vector: ({}, {})".format(vx, vy))
# create and send the command
# TODO: determine if would want to use the hover command instead of the velocity command....
# TODO: problem with the hover command is have no feedback on the current altitude!!
cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_HOVER,
(vx, vy, 0.0, z), delay_time)
self._out_msg_queue.put(cmd)
def cmd_relative_position(self, dx, dy, z, heading):
print("move vector: ({}, {}) at height {}".format(dx, dy, z))
distance = math.sqrt(dx * dx + dy * dy)
delay_time = distance / self.DEFAULT_VELOCITY
print("the delay time for the move command: {}".format(delay_time))
# need to now calculate the velocity vector -> need to have a magnitude of default velocity
vx = self.DEFAULT_VELOCITY * dx / distance
vy = self.DEFAULT_VELOCITY * dy / distance
print("vel vector: ({}, {})".format(vx, vy))
# create and send the command
# TODO: determine if would want to use the hover command instead of the velocity command....
# TODO: problem with the hover command is have no feedback on the current altitude!!
cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_HOVER,
(vx, vy, 0.0, z), delay_time)
self._out_msg_queue.put(cmd)
def takeoff(self, n, e, d):
"""Command the drone to takeoff.
Note some autopilots need a full position for takeoff
and since this class is not aware of current position.`n` and `e`
must be passed along with `d` for this command.
Args:
n: current north position in meters
e: current east position in meters
altitde: desired altitude
"""
# first step: reset the estimator to make sure all is good
self._reset_position_estimator()
# TODO: add to queue a command with 0 x,y vel, 0 yawrate, and the desired height off the ground
cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_HOVER,
(0.0, 0.0, 0.0, d))
self._out_msg_queue.put(cmd)
def land(self, n, e):
"""Command the drone to land.
Note some autopilots need a full position for landing
and since this class is not aware of current position.`n` and `e`
must be passed along with `d` for this command.
Args:
n: current north position in meters
e: current east position in meters
"""
# need to know the current height here...
current_height = self._current_position_xyz[2]
decent_velocity = -self.DEFAULT_VELOCITY # [m/s]
# calculate how long that command should be executed for
# we aren't going to go all the way down before then sending a stop command
# TODO: figure out a way to do this without sleeping!!
delay_time = (current_height - 0.02) / (-1 * decent_velocity
) # the wait time in seconds
# DEBUG
# print("current height: {}, delay time: {}".format(current_height, delay_time));
cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_VELOCITY,
(0.0, 0.0, decent_velocity, 0.0), delay_time)
self._out_msg_queue.put(cmd)
# wait the desired amount of time and then send a stop command to kill the motors
time.sleep(delay_time)
self._out_msg_queue.put(
CrazyflieCommand(CrazyflieCommand.CMD_TYPE_STOP, None))
def set_home_position(self, lat, lon, alt):
"""Command to change the home position of the drone.
Args:
lat: desired home latitude in decimal degrees
lon: desired home longitude in decimal degrees
alt: desired home altitude in meters (AMSL)
"""
# NOTE: this concept doesn't exist for the crazyflie
pass
class Node:
def __init__(self):
self.vx = 0
self.vy = 0
self.vz = 0
self.va = 0
self.vel = Twist()
self.pose = Pose()
self.vel.linear.x = self.vx
self.vel.linear.y = self.vy
self.vel.linear.z = self.vz
self.vel.angular.z = self.va
cflib.crtp.init_drivers(enable_debug_driver=False)
self.crazyflie = SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache'))
self.commander = MotionCommander(self.crazyflie)
self.cf = Crazyflie()
self.crazyflie.open_link()
self.commander.take_off()
def write_to_file(self, data):
# Two loggers should yield an even number of rows of data
# being collected in the end.
# There is one packet missing if the array only contains
# an even number of rows of data.
if len(data) % 2 is not 0:
data = data[:len(data) - 1]
temp_df = pd.DataFrame(data)
m, _ = temp_df.shape
even_rows = temp_df.iloc[np.arange(0, m, 2), :]
odd_rows = temp_df.iloc[np.arange(1, m, 2), :]
columns = [
'timestamp_start', 'timestamp_end', 'acc.x', 'acc.y', 'acc.z',
'gyro.x', 'gyro.y', 'gyro.z', 'stateEstimate.x', 'stateEstimate.y',
'stateEstimate.z', 'stabilizer.pitch', 'stabilizer.roll',
'stabilizer.yaw'
]
df = pd.DataFrame(data=np.zeros((int(m / 2), 14)), columns=columns)
df[['gyro.x', 'gyro.y',
'gyro.z']] = np.array(even_rows[['gyro.x', 'gyro.y', 'gyro.z']])
df[['stabilizer.pitch', 'stabilizer.roll',
'stabilizer.yaw']] = np.array(even_rows[[
'stabilizer.pitch', 'stabilizer.roll', 'stabilizer.yaw'
]])
df[["acc.x", "acc.y",
"acc.z"]] = np.array(odd_rows[["acc.x", "acc.y", "acc.z"]])
df[["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"]] = \
np.array(odd_rows[["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"]])
df['timestamp_start'] = np.array(even_rows.timestamp)
df['timestamp_end'] = np.array(odd_rows.timestamp)
# df.to_csv("data/project2/drone2/data_set_label_"
# +class_label+"_packet_"+packet_num+".csv")
df.to_csv("test.csv")
def write(self, data):
print(data)
def log1(self):
lg1 = LogConfig(name='pose_acc', period_in_ms=10)
lg1.add_variable('stateEstimate.x', 'float')
lg1.add_variable('stateEstimate.y', 'float')
lg1.add_variable('stateEstimate.z', 'float')
lg1.add_variable('acc.x', 'float')
lg1.add_variable('acc.y', 'float')
lg1.add_variable('acc.z', 'float')
return lg1
def log2(self):
lg2 = LogConfig(name='stabilizer_gyro', period_in_ms=10)
lg2.add_variable('stabilizer.roll', 'float')
lg2.add_variable('stabilizer.pitch', 'float')
lg2.add_variable('stabilizer.yaw', 'float')
lg2.add_variable('gyro.x', 'float')
lg2.add_variable('gyro.y', 'float')
lg2.add_variable('gyro.z', 'float')
return lg2
def sync(self, position_pub, data):
switch = 0
with SyncLogger(self.crazyflie, self.log1()) as logger1, \
SyncLogger(self.crazyflie, self.log2()) as logger2:
end_time = time.time() + 24
while time.time() < end_time:
if switch == 0:
logger = logger2
elif switch == 1:
logger = logger1
for log_entry in logger:
row = log_entry[1]
row["timestamp"] = log_entry[0]
if switch == 1:
x = row['stateEstimate.x']
y = row['stateEstimate.y']
z = row['stateEstimate.z']
self.pose.position.x = x
self.pose.position.y = y
self.pose.position.z = z
position_pub.publish(self.pose)
print('x:', x, ' y:', y, ' z:', z)
print()
data.append(row)
switch += 1
break
if switch == 2:
switch = 0
return None
def shut_down(self):
self.crazyflie.close_link()
def cmd_vel(self, msg):
self.vx = msg.linear.x
self.vy = msg.linear.y
self.vz = msg.linear.z
self.va = msg.angular.z
self.commander._set_vel_setpoint(self.vx, self.vy, self.vz, self.va)
print(self.vx, self.vy, self.vz, self.va)
class SyncCrazyflieTest(unittest.TestCase):
def setUp(self):
self.uri = 'radio://0/60/2M'
self.cf_mock = MagicMock(spec=Crazyflie)
self.cf_mock.connected = Caller()
self.cf_mock.connection_failed = Caller()
self.cf_mock.disconnected = Caller()
self.cf_mock.open_link = AsyncCallbackCaller(
cb=self.cf_mock.connected,
args=[self.uri]).trigger
self.sut = SyncCrazyflie(self.uri, self.cf_mock)
def test_different_underlying_cf_instances(self):
# Fixture
# Test
scf1 = SyncCrazyflie('uri 1')
scf2 = SyncCrazyflie('uri 2')
# Assert
actual1 = scf1.cf
actual2 = scf2.cf
self.assertNotEqual(actual1, actual2)
def test_open_link(self):
# Fixture
# Test
self.sut.open_link()
# Assert
self.assertTrue(self.sut.is_link_open())
def test_failed_open_link_raises_exception(self):
# Fixture
expected = 'Some error message'
self.cf_mock.open_link = AsyncCallbackCaller(
cb=self.cf_mock.connection_failed,
args=[self.uri, expected]).trigger
# Test
try:
self.sut.open_link()
except Exception as e:
actual = e.args[0]
else:
self.fail('Expect exception')
# Assert
self.assertEqual(expected, actual)
self._assertAllCallbacksAreRemoved()
def test_open_link_of_already_open_link_raises_exception(self):
# Fixture
self.sut.open_link()
# Test
# Assert
with self.assertRaises(Exception):
self.sut.open_link()
def test_close_link(self):
# Fixture
self.sut.open_link()
# Test
self.sut.close_link()
# Assert
self.cf_mock.close_link.assert_called_once_with()
self.assertFalse(self.sut.is_link_open())
self._assertAllCallbacksAreRemoved()
def test_close_link_that_is_not_open(self):
# Fixture
# Test
self.sut.close_link()
# Assert
self.assertFalse(self.sut.is_link_open())
def test_closed_if_connection_is_lost(self):
# Fixture
self.sut.open_link()
# Test
AsyncCallbackCaller(
cb=self.cf_mock.disconnected,
args=[self.uri]).call_and_wait()
# Assert
self.assertFalse(self.sut.is_link_open())
self._assertAllCallbacksAreRemoved()
def test_open_close_with_context_mangement(self):
# Fixture
# Test
with SyncCrazyflie(self.uri, self.cf_mock) as sut:
self.assertTrue(sut.is_link_open())
# Assert
self.cf_mock.close_link.assert_called_once_with()
self._assertAllCallbacksAreRemoved()
def _assertAllCallbacksAreRemoved(self):
self.assertEqual(0, len(self.cf_mock.connected.callbacks))
self.assertEqual(0, len(self.cf_mock.connection_failed.callbacks))
self.assertEqual(0, len(self.cf_mock.disconnected.callbacks))
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 SyncCrazyflieTest(unittest.TestCase):
def setUp(self):
self.uri = 'radio://0/60/2M'
self.cf_mock = MagicMock(spec=Crazyflie)
self.cf_mock.connected = Caller()
self.cf_mock.connection_failed = Caller()
self.cf_mock.disconnected = Caller()
self.cf_mock.open_link = AsyncCallbackCaller(
cb=self.cf_mock.connected,
args=[self.uri]).trigger
self.sut = SyncCrazyflie(self.uri, self.cf_mock)
def test_different_underlying_cf_instances(self):
# Fixture
# Test
scf1 = SyncCrazyflie('uri 1')
scf2 = SyncCrazyflie('uri 2')
# Assert
actual1 = scf1.cf
actual2 = scf2.cf
self.assertNotEquals(actual1, actual2)
def test_open_link(self):
# Fixture
# Test
self.sut.open_link()
# Assert
self.assertTrue(self.sut.is_link_open())
def test_failed_open_link_raises_exception(self):
# Fixture
expected = 'Some error message'
self.cf_mock.open_link = AsyncCallbackCaller(
cb=self.cf_mock.connection_failed,
args=[self.uri, expected]).trigger
# Test
try:
self.sut.open_link()
except Exception as e:
actual = e.args[0]
else:
self.fail('Expect exception')
# Assert
self.assertEqual(expected, actual)
def test_open_link_of_already_open_link_raises_exception(self):
# Fixture
self.sut.open_link()
# Test
# Assert
with self.assertRaises(Exception):
self.sut.open_link()
def test_close_link(self):
# Fixture
self.sut.open_link()
# Test
self.sut.close_link()
# Assert
self.cf_mock.close_link.assert_called_once_with()
self.assertFalse(self.sut.is_link_open())
def test_close_link_that_is_not_open(self):
# Fixture
# Test
self.sut.close_link()
# Assert
self.assertFalse(self.sut.is_link_open())
def test_closed_if_connection_is_lost(self):
# Fixture
self.sut.open_link()
# Test
AsyncCallbackCaller(
cb=self.cf_mock.disconnected,
args=[self.uri]).call_and_wait()
# Assert
self.assertFalse(self.sut.is_link_open())
def test_open_close_with_context_mangement(self):
# Fixture
# Test
with SyncCrazyflie(self.uri, self.cf_mock) as sut:
self.assertTrue(sut.is_link_open())
# Assert
self.cf_mock.close_link.assert_called_once_with()
class SyncCrazyflieTest(unittest.TestCase):
def setUp(self):
self.uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
self.cf_mock = MagicMock(spec=Crazyflie)
self.cf_mock.connected = Caller()
self.cf_mock.connection_failed = Caller()
self.cf_mock.disconnected = Caller()
self.cf_mock.open_link = AsyncCallbackCaller(
cb=self.cf_mock.connected,
args=[self.uri],
delay=0.2
).trigger
self.close_link_mock = AsyncCallbackCaller(
cb=self.cf_mock.disconnected,
args=[self.uri],
delay=0.2
)
self.cf_mock.close_link = self.close_link_mock.trigger
# Mock the behaviour that param values are updated(downloaded) after connection
self.param_mock = MagicMock(spec=Param)
self.param_mock.all_updated = Caller()
self.cf_mock.param = self.param_mock
# Register a callback to be called when connected. Use it to trigger a callback
# to trigger the call to the param.all_updated() callback
self.cf_mock.connected.add_callback(self._connected_callback)
self.sut = SyncCrazyflie(self.uri, cf=self.cf_mock)
def test_different_underlying_cf_instances(self):
# Fixture
# Test
scf1 = SyncCrazyflie('uri 1')
scf2 = SyncCrazyflie('uri 2')
# Assert
actual1 = scf1.cf
actual2 = scf2.cf
self.assertNotEqual(actual1, actual2)
def test_open_link(self):
# Fixture
# Test
self.sut.open_link()
# Assert
self.assertTrue(self.sut.is_link_open())
def test_failed_open_link_raises_exception(self):
# Fixture
expected = 'Some error message'
self.cf_mock.open_link = AsyncCallbackCaller(
cb=self.cf_mock.connection_failed,
args=[self.uri, expected]).trigger
# Test
try:
self.sut.open_link()
except Exception as e:
actual = e.args[0]
else:
self.fail('Expect exception')
# Assert
self.assertEqual(expected, actual)
self._assertAllCallbacksAreRemoved()
def test_open_link_of_already_open_link_raises_exception(self):
# Fixture
self.sut.open_link()
# Test
# Assert
with self.assertRaises(Exception):
self.sut.open_link()
def test_wait_for_params(self):
# Fixture
self.sut.open_link()
# Test
self.sut.wait_for_params()
# Assert
self.assertTrue(self.sut.is_params_updated())
def test_do_not_wait_for_params(self):
# Fixture
# Test
self.sut.open_link()
# Assert
self.assertFalse(self.sut.is_params_updated())
def test_close_link(self):
# Fixture
self.sut.open_link()
# Test
self.sut.close_link()
# Assert
self.assertEqual(1, self.close_link_mock.call_count)
self.assertFalse(self.sut.is_link_open())
self._assertAllCallbacksAreRemoved()
def test_close_link_that_is_not_open(self):
# Fixture
# Test
self.sut.close_link()
# Assert
self.assertFalse(self.sut.is_link_open())
def test_closed_if_connection_is_lost(self):
# Fixture
self.sut.open_link()
# Test
AsyncCallbackCaller(
cb=self.cf_mock.disconnected,
args=[self.uri]).call_and_wait()
# Assert
self.assertFalse(self.sut.is_link_open())
self._assertAllCallbacksAreRemoved()
def test_open_close_with_context_management(self):
# Fixture
# Test
with SyncCrazyflie(self.uri, self.cf_mock) as sut:
self.assertTrue(sut.is_link_open())
# Assert
self.assertEqual(1, self.close_link_mock.call_count)
self._assertAllCallbacksAreRemoved()
def test_wait_for_params_with_context_management(self):
# Fixture
# Test
with SyncCrazyflie(self.uri, cf=self.cf_mock) as sut:
sut.wait_for_params()
self.assertTrue(sut.is_params_updated())
# Assert
self._assertAllCallbacksAreRemoved()
def test_multiple_open_close_of_link(self):
# Fixture
# Test
self.sut.open_link()
self.sut.close_link()
self.sut.open_link()
self.sut.close_link()
# Assert
self.assertEqual(2, self.close_link_mock.call_count)
self.assertFalse(self.sut.is_link_open())
self._assertAllCallbacksAreRemoved()
def test_wait_for_params_with_multiple_open_close_of_link(self):
# Fixture
# Test
self.sut.open_link()
self.assertFalse(self.sut.is_params_updated())
self.sut.wait_for_params()
self.assertTrue(self.sut.is_params_updated())
self.sut.close_link()
self.assertFalse(self.sut.is_params_updated())
self.sut.open_link()
self.assertFalse(self.sut.is_params_updated())
self.sut.wait_for_params()
self.assertTrue(self.sut.is_params_updated())
self.sut.close_link()
# Assert
self.assertFalse(self.sut.is_params_updated())
self._assertAllCallbacksAreRemoved()
def _assertAllCallbacksAreRemoved(self):
# Remove our probe callback
self.cf_mock.connected.remove_callback(self._connected_callback)
self.assertEqual(0, len(self.cf_mock.connected.callbacks))
self.assertEqual(0, len(self.cf_mock.connection_failed.callbacks))
self.assertEqual(0, len(self.cf_mock.disconnected.callbacks))
self.assertEqual(0, len(self.param_mock.all_updated.callbacks))
def _connected_callback(self, uri):
AsyncCallbackCaller(
cb=self.param_mock.all_updated,
delay=0.2
).trigger()
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 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 CrazyflieConnection(connection.Connection):
def __init__(self, uri, velocity=0.2, threaded=False):
super().__init__(threaded)
# set the default velocity
self._velocity = velocity
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# the connection to the crazyflie
self._scf = SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache'))
# temp (or maybe will be permanent) state variable
self._is_open = False
self._running = False
self._send_rate = 50 # want to send messages at 5Hz NOTE: the minimum is 2 Hz
self._out_msg_queue = queue.Queue(
) # a queue for sending data between threads
self._write_handle = threading.Thread(target=self.command_loop)
self._write_handle.daemon = True
# since can only command velocities and not positions, the connection
# needs some awareness of the current position to be able to do
# the math necessary
self._current_position_xyz = np.array([0.0, 0.0, 0.0]) # [x, y, z]
# due to a "bug" in the crazyflie's position estimator with the flow
# deck that results in the estimator to reset the position to 0,0,0 mid
# flight if there are changes in lighting or terrain (note: may also
# be under other conditions, but so far only seen in those conditions)
self._dynamic_home_xyz = np.array([0.0, 0.0, 0.0]) # [x, y, z]
self._home_position_xyz = np.array([0.0, 0.0, 0.0]) # [x, y, z]
self._cmd_position_xyz = np.array([0.0, 0.0,
0.0]) # the commanded position
# state information is to be updated and managed by this connection class
# for the crazyflie, since the crazyflie doesn't exactly pass down the
# state information
#
# defining the states to be:
# armed -> should roughly mimic connection state (though this does have a problem at the end...)
# guided -> this seems to only be used at the end condition.....
self._armed = True
self._guided = True
# kalman filter state
self._converged = False
self._var_y_history = [1000] * 10
self._var_x_history = [1000] * 10
self._var_z_history = [1000] * 10
self._filter_threshold = 0.001
@property
def open(self):
"""
Returns:
Boolean. True if connection is able to send and/or receive messages, False otherwise.
"""
# TODO: figure out the open condition for crazyflie
if self._is_open == -1:
return False
return True
def start(self):
"""Command to start a connection with a drone"""
self._scf.open_link(
) # this is a blocking function that will not return until the link is opened
# TODO: need a better version of this
self._is_open = True
# need to now register for callbacks on the data of interest from the crazyflie
# TODO: decide on the appropriate rates
log_pos = LogConfig(name='LocalPosition', period_in_ms=150)
log_pos.add_variable('kalman.stateX', 'float')
log_pos.add_variable('kalman.stateY', 'float')
log_pos.add_variable('kalman.stateZ', 'float')
try:
self._scf.cf.log.add_config(log_pos)
# This callback will receive the data
log_pos.data_received_cb.add_callback(self._cf_callback_pos)
# This callback will be called on errors
log_pos.error_cb.add_callback(self._cf_callback_error)
# Start the logging
log_pos.start()
except KeyError as e:
print('Could not start position log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Position log config, bad configuration.')
log_vel = LogConfig(name='LocalVelocity', period_in_ms=100)
log_vel.add_variable('kalman.statePX', 'float')
log_vel.add_variable('kalman.statePY', 'float')
log_vel.add_variable('kalman.statePZ', 'float')
try:
self._scf.cf.log.add_config(log_vel)
# This callback will receive the data
log_vel.data_received_cb.add_callback(self._cf_callback_vel)
# This callback will be called on errors
log_vel.error_cb.add_callback(self._cf_callback_error)
# Start the logging
log_vel.start()
except KeyError as e:
print('Could not start velocity log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add velocity log config, bad configuration.')
log_att = LogConfig(name='Attitude', period_in_ms=500)
log_att.add_variable('stabilizer.roll', 'float')
log_att.add_variable('stabilizer.pitch', 'float')
log_att.add_variable('stabilizer.yaw', 'float')
try:
self._scf.cf.log.add_config(log_att)
# This callback will receive the data
log_att.data_received_cb.add_callback(self._cf_callback_att)
# This callback will be called on errors
log_att.error_cb.add_callback(self._cf_callback_error)
# Start the logging
log_att.start()
except KeyError as e:
print('Could not start attitude log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add attitude log config, bad configuration.')
log_state = LogConfig(name='State', period_in_ms=1000)
log_state.add_variable('kalman.inFlight',
'uint8_t') # TODO: check the data type
try:
self._scf.cf.log.add_config(log_state)
log_state.data_received_cb.add_callback(self._cf_callback_state)
log_state.error_cb.add_callback(self._cf_callback_error)
# Start the logging
log_state.start()
except KeyError as e:
print('Could not start position log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add state log config, bad configuration.')
# start the write thread now that the connection is open
self._running = True
self._write_handle.start()
# reset the estimator
self._reset_position_estimator()
def stop(self):
"""Command to stop a connection with a drone"""
# need to send a stop command
cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_STOP, None)
self._out_msg_queue.put(cmd)
time.sleep(
0.5
) # add a sleep to make sure this command is sent and executed properly
self._running = False # this is to stop the command thread
time.sleep(1) # make sure the command thread has stopped
self._scf.close_link() # close the link
# TODO: find a better way to handle this...
self._is_open = False
def dispatch_loop(self):
"""Main loop that triggers callbacks as messages are recevied"""
# NOTE: nothing needed here for the crazyflie
# NOTE: the crazyflie API sends data down as callbacks already, so this
# NOTE: connection class is nothing but a passthrough for those
pass
def _send_command(self, cmd):
"""helper function to send the appropriate CF command
based on the desired command type, send the corresponding setpoint command to the crazyflie.
Args:
cmd: the CrazyflieCommand to send
"""
# based on the command type, send the appropriate crazyflie command
if cmd.type == CrazyflieCommand.CMD_TYPE_VELOCITY:
self._scf.cf.commander.send_velocity_world_setpoint(*cmd.cmd)
elif cmd.type == CrazyflieCommand.CMD_TYPE_HOVER:
# TODO: see if maybe want to get this current height information
# back to the write loop...
# current_height = cmd.cmd[3]
self._scf.cf.commander.send_hover_setpoint(*cmd.cmd)
elif cmd.type == CrazyflieCommand.CMD_TYPE_ATTITUDE_THRUST:
self._scf.cf.commander.send_setpoint(*cmd.cmd)
elif cmd.type == CrazyflieCommand.CMD_TYPE_ATTITUDE_DIST:
# current_height = cmd.cmd[3]
self._scf.cf.commander.send_zdistance_setpoint(*cmd.cmd)
elif cmd.type == CrazyflieCommand.CMD_TYPE_STOP:
# TODO: probably want to send appropriate flags that state disarmed, etc
# TODO: basically need to update the drone state here
self._scf.cf.commander.send_stop_setpoint()
else:
print("invalid command type!")
def command_loop(self):
"""loop to send commands at a specified rate"""
# the last time a command was sent
# to be used to ensure commands are at the desired rate
last_write_time = time.time()
cmd_start_time = 0 # the time [s] that the command started -> needed for distance commands
# the current command that should be being sent, default to 0 everything
# current_cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_STOP, None)
current_cmd = CrazyflieCommand(
CrazyflieCommand.CMD_TYPE_ATTITUDE_THRUST, (0, 0, 0, 0), None)
# the last commanded height
# if this is not 0, want the hold commands to be hover to hold the specific height
current_height = 0
while self._running:
# want to make sure the kalman filter has converged before sending any command
while not self._converged:
self._send_command(
CrazyflieCommand(CrazyflieCommand.CMD_TYPE_ATTITUDE_THRUST,
(0, 0, 0, 0), None))
continue
# empty out the queue of pending messages -> want to always send the messages asap
# NOTE: the commands are immediately sent, which can result in fast back to back command sets, but
# ensures all the commands are sent
cmd = None
while not self._out_msg_queue.empty():
try:
cmd = self._out_msg_queue.get_nowait()
except queue.Empty:
# if there is no msgs in the queue, will just continue
pass
else:
if cmd is not None:
current_cmd = cmd
cmd_start_time = time.time()
# mark this entity as being parsed
self._out_msg_queue.task_done()
# convert the position command to a velocity (hover) command if needed
if cmd.type == CrazyflieCommand.CMD_TYPE_POSITION:
current_cmd = self._pos_cmd_to_cf_vel_cmd(
np.array(cmd.cmd[0:3]), cmd.cmd[3])
# immediately handle the new command
self._send_command(current_cmd)
# DEBUG
# print("recevied command, type {}, cmd {}, delay {}".format(
# current_cmd.type, current_cmd.cmd, current_cmd.delay))
# now that have handled any potentially new commands, let's handle the timer
if current_cmd.delay is not None:
if time.time() - cmd_start_time >= current_cmd.delay:
# DEBUG
# print("command timer completed, completed command: {}, {}".format(
# current_cmd.type, current_cmd.cmd))
# time to stop -> want to hold the commanded height (instead of the current height as current
# height may drift and that will cause the crazyflie to bob a lot)
current_height = self._cmd_position_xyz[2]
# print("stopping and holding a cmd height of {}".format(current_height))
if current_height > 0.05:
current_cmd = CrazyflieCommand(
CrazyflieCommand.CMD_TYPE_HOVER,
(0.0, 0.0, 0.0, current_height))
else:
current_cmd = CrazyflieCommand(
CrazyflieCommand.CMD_TYPE_VELOCITY,
(0.0, 0.0, 0.0, 0.0))
# now immediately handle the new command
self._send_command(current_cmd)
# want to make sure that the commands are set at minimum specified rate
# so send the command again if that rate timer requires it
current_time = time.time()
if (current_time - last_write_time) < (1.0 / self._send_rate):
continue
# resend the command and update the timestamp for when the last command was sent
last_write_time = current_time
self._send_command(current_cmd)
def _cf_callback_pos(self, timestamp, data, logconf):
"""callback on the crazyflie's position update"""
x = data['kalman.stateX']
y = data['kalman.stateY']
z = data['kalman.stateZ']
# print("current height: {}".format(z))
# compute a difference between the previou current position and this one
# if there is a large jump, that means there is a chance the estimator has reset!
new_position = np.array([x, y, z])
dpos = new_position - self._current_position_xyz
dx = dpos[0]
dy = dpos[1]
pos_change = math.sqrt(dx * dx + dy * dy)
# DEBUG
# print("position change: ({}, {})".format(dx, dy))
# TODO: find the correct limit here for defining a jump
if pos_change >= 1:
# print("esitmator has reset, adjusting home position")
self._dynamic_home_xyz += np.array([dx, dy, 0])
# print("\tdynamic adjustment = ({}, {})".format(self._dynamic_home_xyz[0], self._dynamic_home_xyz[1]))
self._current_position_xyz = np.array([x, y,
z]) # save for our internal use
# the position that should be published is an NED position, adjusted for the set home position
adjusted_pos = self._current_position_xyz - self._home_position_xyz - self._dynamic_home_xyz
pos = mt.LocalFrameMessage(timestamp, adjusted_pos[0],
-adjusted_pos[1], -adjusted_pos[2])
self.notify_message_listeners(MsgID.LOCAL_POSITION, pos)
def _cf_callback_vel(self, timestamp, data, logconf):
"""callback on the crazyflie's velocity update"""
if not self._converged:
return
x = data['kalman.statePX']
y = data['kalman.statePY']
z = data['kalman.statePZ']
vel = mt.LocalFrameMessage(timestamp, x, -y, -z)
self.notify_message_listeners(MsgID.LOCAL_VELOCITY, vel)
def _cf_callback_att(self, timestamp, data, logconf):
"""callback on the crazyflie's attitude update"""
if not self._converged:
return
roll = data['stabilizer.roll']
pitch = data['stabilizer.pitch']
yaw = data['stabilizer.yaw']
fm = mt.FrameMessage(timestamp, roll, pitch, yaw)
self.notify_message_listeners(MsgID.ATTITUDE, fm)
def _cf_callback_state(self, timestamp, data, logconf):
"""callback on the crazyflie's state information"""
# in_flight = data['kalman.inFlight']
# armed = False
# guided = False
# if in_flight:
# armed = True
# guided = True
# send a state message to the drone to set armed and guided to be True
# since these constructs don't exist for the crazyflie, but the armed -> guided transition needs to be
# robust to work from the sim to the crazyflie with minimal changes
state = mt.StateMessage(timestamp, self._armed, self._guided)
self.notify_message_listeners(MsgID.STATE, state)
# TODO: probably need a better metric for armed / guided
# since the quad is basically always armed and guided comes into play
# once the connection is made, so basically the second the script starts...
# state = mt.StateMessage(timestamp, armed, guided)
# self.notify_message_listeners(MsgID.STATE, state)
pass
def _cf_callback_kf_variance(self, timestamp, data, logconf):
"""callback on the crazyflie's KF varaince information"""
self._var_x_history.append(data['kalman.varPX'])
self._var_x_history.pop(0)
self._var_y_history.append(data['kalman.varPY'])
self._var_y_history.pop(0)
self._var_z_history.append(data['kalman.varPZ'])
self._var_z_history.pop(0)
min_x = min(self._var_x_history)
max_x = max(self._var_x_history)
min_y = min(self._var_y_history)
max_y = max(self._var_y_history)
min_z = min(self._var_z_history)
max_z = max(self._var_z_history)
# print("filter variances: {} {} {}".format(max_x - min_x, max_y - min_y, max_z - min_z))
dx = max_x - min_x
dy = max_y - min_y
dz = max_z - min_z
if dx < self._filter_threshold and dy < self._filter_threshold and dz < self._filter_threshold:
print("filter has converge, position is good!")
self._converged = True
self._kf_log_config.stop(
) # no longer care to keep getting the kalman filter variance
def _cf_callback_error(self, logconf, msg):
"""callback for an error from one of the loggers"""
print('Error when logging %s: %s' % (logconf.name, msg))
def _wait_for_position_estimator(self):
"""Start listening for the kalman filter variance to determine when it converges"""
print('Waiting for estimator to find position...')
# configure the log for the variance
self._kf_log_config = LogConfig(name='Kalman Variance',
period_in_ms=100)
self._kf_log_config.add_variable('kalman.varPX', 'float')
self._kf_log_config.add_variable('kalman.varPY', 'float')
self._kf_log_config.add_variable('kalman.varPZ', 'float')
try:
self._scf.cf.log.add_config(self._kf_log_config)
# This callback will receive the data
self._kf_log_config.data_received_cb.add_callback(
self._cf_callback_kf_variance)
# This callback will be called on errors
self._kf_log_config.error_cb.add_callback(self._cf_callback_error)
# Start the logging
self._kf_log_config.start()
except KeyError as e:
print('Could not start kalman log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add kalman log config, bad configuration.')
def _reset_position_estimator(self):
"""reset the estimator to give the best performance possible"""
self._scf.cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
self._scf.cf.param.set_value('kalman.resetEstimation', '0')
# wait for the variance of the estimator to become small enough
self._wait_for_position_estimator()
def _convert_to_cf_xyz(self, pos):
"""convert the position to a position in the crazyflie's current frame
handle the conversion from the user's drone frame to the frame being used in the crazyflie.
Args:
pos: numpy array of the desired position in the XYZ frame
Returns:
the XYZ position vector in the crazyflie's coordinate frame
numpy array
"""
return pos + self._dynamic_home_xyz + self._home_position_xyz
def _create_velocity_cmd(self, dx, dy, z, heading):
"""helper function to create a velocity command given a desired change in position.
computes the velocity command given the velocity setting and then passed in position change.
also computes the necessary delay time to wait before finishing the command.
Args:
dx: distance to travel in the crazyflie's X direction
dy: distance to travel in the crazyflie's Y direction
dz: distance to travel in the crazyflie's Z direction
z: the height above ground to hold
heading: desired heading
Returns:
a velocity command (CF type HOVER) to execute the desired motion.
CrazyflieCommand
"""
# calculate the distance needed to travel and the delay time for the command
distance = math.sqrt(dx * dx + dy * dy)
delay_time = distance / self._velocity
print("the delay time for the move command: {}".format(delay_time))
# need to now calculate the velocity vector -> need to have a magnitude of default velocity
vx = self._velocity * dx / distance
vy = self._velocity * dy / distance
# vz = self._velocity * dz / distance
vz = 0
print("vel vector: ({}, {}, {})".format(vx, vy, vz))
# create and send the command
# TODO: determine if would want to use the hover command instead of the velocity command....
# TODO: problem with the hover command is have no feedback on the current altitude!!
# return CrazyflieCommand(CrazyflieCommand.CMD_TYPE_VELOCITY, (vx, vy, vz, 0.0), delay_time)
return CrazyflieCommand(CrazyflieCommand.CMD_TYPE_HOVER,
(vx, vy, 0.0, z), delay_time)
def _pos_cmd_to_cf_vel_cmd(self, cmd_pos_xyz, heading):
"""convert an XYZ command from the user to a velocity command in the crazyflie's frame
handles the translation that needs to occur to take into account the set home position and any potential
dynamic home adjustments needed due to the estimator onboard the crazyflie resetting.
then computes the velocity command and delay time needed to reach the desired point, creating the necessary
crazyflie command to successfully fly to the commanded position.
Note: the user's XYZ frame is a frame with (0,0) at the location at which `set_home_position()` was called.
Args:
cmd_pos_xyz: the desired position in the user's XYZ frame
heading: the desired vehicle heading
Returns:
a velocity move command (CF type hover) required to reach the desired position.
CrazyflieCommand
"""
# convert from the user's frame to the cf's internal frame
cmd_pos_cf_xyz = self._convert_to_cf_xyz(cmd_pos_xyz)
self._cmd_position_xyz = np.copy(cmd_pos_cf_xyz)
# DEBUG - position info
# print("current positions:")
# print("\tvehicle: ({}, {}, {})".format(self._current_position_xyz[0], self._current_position_xyz[1],
# self._current_position_xyz[2]))
# print("\thome: ({}, {}, {})".format(self._home_position_xyz[0], self._home_position_xyz[1],
# self._home_position_xyz[2]))
# print("\tdynamic: ({}, {}, {})".format(self._dynamic_home_xyz[0], self._dynamic_home_xyz[1],
# self._dynamic_home_xyz[2]))
# DEBUG - command info
# print("command detailed:")
# print("\tuser xyz frame: ({}, {}, {})".format(cmd_pos_xyz[0], cmd_pos_xyz[1], cmd_pos_xyz[2]))
# print("\tcf frame: ({}, {}, {})".format(cmd_pos_cf_xyz[0], cmd_pos_cf_xyz[1], cmd_pos_cf_xyz[2]))
# calculate the change vector needed
# note the slight oddity that happens in converting NED to XYZ
# as things are used as XYZ internally for the crazyflie
dx = cmd_pos_cf_xyz[0] - self._current_position_xyz[0]
dy = cmd_pos_cf_xyz[1] - self._current_position_xyz[1]
# dz = cmd_pos_cf_xyz[2] - self._current_position_xyz[2]
z = cmd_pos_cf_xyz[2]
return self._create_velocity_cmd(dx, dy, z, heading)
def set_velocity(self, velocity):
"""set the velocity the drone should use in flight"""
self._velocity = velocity
def arm(self):
"""Command to arm the drone"""
# NOTE: this doesn't exist for the crazyflie
pass
def disarm(self):
"""Command to disarm the drone"""
# NOTE: this doesn't exist for the crazyflie
pass
def take_control(self):
"""
Command the drone to switch into a mode that allows external control.
e.g. for PX4 this commands 'offboard' mode,
while for APM this commands 'guided' mode
"""
# NOTE: this doesn't exist for the crazyflie
# however, if this command is being used, want to make sure
# the state output conforms to the expected changes
self._armed = True
self._guided = True
def release_control(self):
"""Command to return the drone to a manual mode"""
# NOTE: this doesn't exist for the crazyflie
# however, if this command is being used, want to make sure
# the state output conforms to the expected changes
self._armed = False
self._guided = False
def cmd_attitude(self, roll, pitch, yaw, thrust):
"""Command to set the desired attitude and thrust
Args:
yaw: the desired yaw in radians
pitch: the desired pitch in radians
roll: the deisred roll in radians
thrust: the normalized desired thrust level on [0, 1]
"""
# NOTE: for the crazyflie, the attitude commands are in degrees, so will need to adjust accordingly
# TODO: crazyflie takes in roll/pitch/yaw, should figure out the impact of making this function correct
# to the definition of cmd_attitude... not sure why commanding yaw rate here
# NOTE: thrust is also a bit weird for the crazyflie, it's a value between 10001 and 60000
# with hover thrust being around 36850.0
roll_deg = np.degrees(roll)
pitch_deg = -np.degrees(
pitch
) # crazyflie is in an XYZ frame, so pitch direction is reversed
yaw_deg = np.degrees(yaw)
# map the thrust from [0, 1] to the crazyflie accepted [10000, 65000]
thrust = thrust * 55000 + 10000
# thrust needs to be an int
thrust = int(thrust)
# NOTE: again no delay time as that is not used when sending commands at this level
self._out_msg_queue.put(
CrazyflieCommand(CrazyflieCommand.CMD_TYPE_ATTITUDE_THRUST,
(roll_deg, pitch_deg, yaw_deg, thrust), None))
# self._out_msg_queue.put(CrazyflieCommand(CrazyflieCommand.CMD_TYPE_ATTITUDE_DIST,
# (roll_deg, pitch_deg, yaw_deg, 0.5),
# None))
def cmd_attitude_zdist(self, roll, pitch, yaw, altitude):
"""Command to set the desired attitude and altitude.
This is a custom crazyflie command that has the crazyflie worry about holding altitude and
attitude is controlled by the user
Args:
roll: the desired roll in [radians]
pitch: the desired pitch in [radians]
yaw: the desired yaw in [radians]
altitude: the desired altitude in [m]
"""
roll_deg = np.degrees(roll)
pitch_deg = np.degrees(pitch)
yaw_deg = np.degrees(yaw)
# no time delay here
# create the attitude zdistance command
self._out_msg_queue.put(
CrazyflieCommand(CrazyflieCommand.CMD_TYPE_ATTITUDE_DIST,
(roll_deg, pitch_deg, yaw_deg, altitude), None))
def cmd_attitude_rate(self, roll_rate, pitch_rate, yaw_rate, thrust):
"""Command to set the desired attitude rates and thrust
Args:
yaw_rate: the desired yaw rate in radians/second
pitch_rate: the desired pitch rate in radians/second
roll_rate: the desired roll rate in radians/second
thrust: the normalized desired thrust level on [0, 1]
"""
pass
def cmd_moment(self, roll_moment, pitch_moment, yaw_moment, thrust):
"""Command to set the desired moments and thrust
Args:
roll_moment: the desired roll moment in Newton*meter
yaw_moment: the desired yaw moment in Newton*meter
pitch_moment: the desired pitch moment in Newton*meter
thrust: the normalized desired thrust level in Newton
"""
pass
def cmd_velocity(self, vn, ve, vd, heading):
"""Command to set the desired velocity (NED frame) and heading
Note: For the crazyflie, NED is defined when the crazyflie starts, not aligned with world NED
Args:
vn: desired north velocity component in meters/second
ve: desired east velocity component in meters/second
vd: desired down velocity component in meters/second (note: positive down!)
heading: desired drone heading in radians
"""
# crazyflie works in an XYZ "world" frame, so need to convert from NED to XYZ
vx = vn
vy = -ve
vz = -vd
# TODO: crazyflie takes yaw_rate here, need to handle this correctly
# for now, ignore all heading commands
# for a velocity command the idea of a delay time doesn't exist, it's up to the user to make sure
# that velocity commands keep getting sent
delay_time = None
self._out_msg_queue.put(
CrazyflieCommand(CrazyflieCommand.CMD_TYPE_VELOCITY,
(vx, vy, vz, 0.0), delay_time))
def cmd_motors(self, motor1, motor2, motor3, motor4):
"""Command the thrust levels for each motor on a quadcopter
Args:
motor1: normalized thrust level for motor 1 on [0, 1]
motor2: normalized thrust level for motor 2 on [0, 1]
motor3: normalized thrust level for motor 3 on [0, 1]
motor4: normalized thrust level for motor 4 on [0, 1]
"""
pass
def cmd_position(self, n, e, d, heading):
"""Command to set the desired position ("NED" frame) and heading
Note: For the crazyflie, NED is really the body XYZ frame fixed
unpon startup of the crazyflie to be a world frame
Args:
n: desired north position in meters
e: desired east position in meters
d: desired down position in meters (note: positive down!)
heading: desired drone heading in radians
"""
# consider the waypoint as reached, so command the cf to stop
current_height = self._cmd_position_xyz[2]
stop_moving_cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_HOVER,
(0.0, 0.0, 0.0, current_height))
self._out_msg_queue.put(stop_moving_cmd)
# need to know the current position: for now going to simply map NED to XYZ!!!
# x is forward
# y is left
# z is up
# also completely ignoring heading for now
# send a position command - this will allow the write loop
# to use the most up to date information for generating the
# corresponding velocity command
cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_POSITION,
(n, -e, -d, heading))
self._out_msg_queue.put(cmd)
# # need to covert the commanded position to the crazyflie's
# # "world" frame
# cmd_pos_cf_xyz = self._convert_to_cf_xyz(cmd_pos_xyz)
# # DEBUG - position info
# print("current positions:")
# print("\tvehicle: ({}, {}, {})".format(
# self._current_position_xyz[0],
# self._current_position_xyz[1],
# self._current_position_xyz[2]))
# print("\thome: ({}, {}, {})".format(
# self._home_position_xyz[0],
# self._home_position_xyz[1],
# self._home_position_xyz[2]))
# print("\tdynamic: ({}, {}, {})".format(
# self._dynamic_home_xyz[0],
# self._dynamic_home_xyz[1],
# self._dynamic_home_xyz[2]))
# # DEBUG - command info
# print("command detailed:")
# print("\tuser xyz frame: ({}, {}, {})".format(n, -e, -d))
# print("\tcf frame: ({}, {}, {})".format(
# cmd_pos_cf_xyz[0], cmd_pos_cf_xyz[1], cmd_pos_cf_xyz[2]))
# # calculate the change vector needed
# # note the slight oddity that happens in converting NED to XYZ
# # as things are used as XYZ internally for the crazyflie
# dx = cmd_pos_cf_xyz[0] - self._current_position_xyz[0]
# dy = cmd_pos_cf_xyz[1] - self._current_position_xyz[1]
# z = cmd_pos_cf_xyz[2] # holding a specific altitude, so just pass altitude through directly
# # DEBUG
# # print("move vector: ({}, {}) at height {}".format(dx, dy, z))
# # command the relative position
# self.cmd_relative_position(dx, dy, z, heading)
def cmd_relative_position(self, dx, dy, dz, heading):
print("move vector: ({}, {}, {})".format(dx, dy, dz))
# update the commanded position information
# want to be able to keep track of the desired "world frame"
# coordinates to be able to catch estimator errors.
self._cmd_position_xyz = self._current_position_xyz + np.array(
[dx, dy, dz])
# use the helper function for this
cmd = self._create_velocity_cmd(dx, dy, self._cmd_position_xyz[2],
heading)
self._out_msg_queue.put(cmd)
# distance = math.sqrt(dx * dx + dy * dy)
# delay_time = distance / self._velocity
# print("the delay time for the move command: {}".format(delay_time))
# # need to now calculate the velocity vector -> need to have a magnitude of default velocity
# vx = self._velocity * dx / distance
# vy = self._velocity * dy / distance
# print("vel vector: ({}, {})".format(vx, vy))
# # create and send the command
# # TODO: determine if would want to use the hover command instead of the velocity command....
# # TODO: problem with the hover command is have no feedback on the current altitude!!
# cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_HOVER, (vx, vy, 0.0, z), delay_time)
# self._out_msg_queue.put(cmd)
def takeoff(self, n, e, d):
"""Command the drone to takeoff.
Note some autopilots need a full position for takeoff
and since this class is not aware of current position.`n` and `e`
must be passed along with `d` for this command.
Args:
n: current north position in meters
e: current east position in meters
altitde: desired altitude
"""
# first step: reset the estimator to make sure all is good, this will take a variable amount of time
# as it waits for the filter to converge before returning
# self._reset_position_estimator()
# set the command position
self._cmd_position_xyz = np.copy(self._current_position_xyz)
self._cmd_position_xyz[2] = -d
# add to queue a command with 0 x,y vel, 0 yawrate, and the desired height off the ground
cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_HOVER,
(0.0, 0.0, 0.0, d))
self._out_msg_queue.put(cmd)
def land(self, n, e):
"""Command the drone to land.
Note some autopilots need a full position for landing
and since this class is not aware of current position.`n` and `e`
must be passed along with `d` for this command.
Args:
n: current north position in meters
e: current east position in meters
"""
# set the command position
self._cmd_position_xyz = np.copy(self._current_position_xyz)
self._cmd_position_xyz[2] = 0
# need to know the current height here...
current_height = self._current_position_xyz[2]
decent_velocity = -self._velocity / 2 # [m/s]
# calculate how long that command should be executed for
# we aren't going to go all the way down before then sending a stop command
# TODO: figure out a way to do this without sleeping!!
delay_time = (current_height - 0.02) / (-1 * decent_velocity
) # the wait time in seconds
# DEBUG
# print("current height: {}, delay time: {}".format(current_height, delay_time))
# make sure delay time is always positive and non-zero
if delay_time < 0:
delay_time = 0.1
cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_VELOCITY,
(0.0, 0.0, decent_velocity, 0.0), delay_time)
self._out_msg_queue.put(cmd)
# wait the desired amount of time and then send a stop command to kill the motors
time.sleep(delay_time)
self._out_msg_queue.put(
CrazyflieCommand(CrazyflieCommand.CMD_TYPE_STOP, None))
def set_home_position(self, lat, lon, alt):
"""Command to change the home position of the drone.
Note: for the crazyflie, there is no global position coordinates.
Therefore when this command is called, the current local position
of the crazyflie will be used as the home position.
**This will therefore ignore all input arguments!**
Args:
lat: desired home latitude in decimal degrees
lon: desired home longitude in decimal degrees
alt: desired home altitude in meters (AMSL)
"""
# NOTE: for the crazyflie, this takes the current local position
# and sets that value to home.
# Therefore all of the inputs are ignored!
# update the home position to be the current position
# this will be added to all the waypoint commands to get the
# proper coordinate to command
self._home_position_xyz = self._current_position_xyz
self._home_position_xyz[2] = 0.0 # for now keep this at 0
# want to reset the dynamic home adjustment at this point, since resetting the home position
self._dynamic_home_xyz = np.array([0.0, 0.0, 0.0])
