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 CrazyflieControl:
def __init__(self, name, crazyflie):
# Instantiate motion commander
self._cf = crazyflie
self._name = name
self._mc = MotionCommander(self._cf)
# Topic Publishers
self._velocity_setpoint_pub = rospy.Publisher(self._name +
'/velocity_setpoint',
Vector3,
queue_size=10)
"""
Services hosted for this crazyflie controller
"""
self._reset_position_estimator_srv = rospy.Service(
self._name + '/reset_position_estimator', ResetPositionEstimator,
self._reset_position_estimator_cb)
self._send_hover_setpoint_srv = rospy.Service(
self._name + '/send_hover_setpoint', SendHoverSetpoint,
self._send_hover_setpoint_cb)
self._set_param_srv = rospy.Service(self._name + '/set_param',
SetParam, self._set_param_cb)
self._velocity_control_srv = rospy.Service(
self._name + '/velocity_control', VelocityControl,
self._velocity_control_cb)
"""
Action servers for this crazyflie controller
"""
self._position_control_as = actionlib.SimpleActionServer(
self._name + '/position_control', PositionControlAction,
self._position_control_cb, False)
self._position_control_as.start()
"""
Service Callbacks
"""
def _reset_position_estimator_cb(self, req):
pass
def _send_hover_setpoint_cb(self, req):
vx = req.vx
vy = req.vy
z = req.z
yaw_rate = req.yaw_rate
self._cf.commander.send_hover_setpoint(vx, vy, yaw_rate, z)
return []
def _set_param_cb(self, req):
self._cf.param.set_value(req.param, req.value)
print("set %s to %s" % (req.param, req.value))
return SetParamResponse()
def _velocity_control_cb(self, req):
try:
obj = pickle.loads(req.pickle)
print(self._mc)
if isinstance(obj, SetVelSetpoint):
self._mc._set_vel_setpoint(obj.vx, obj.vy, obj.vz,
obj.rate_yaw)
elif isinstance(obj, StartBack):
self._mc.start_back(velocity=obj.velocity)
elif isinstance(obj, StartCircleLeft):
self._mc.start_circle_left(obj.radius_m, velocity=obj.velocity)
elif isinstance(obj, StartCircleRight):
self._mc.start_turn_right(obj.radius_m, velocity=obj.velocity)
elif isinstance(obj, StartDown):
self._mc.start_down(velocity=obj.velocity)
elif isinstance(obj, StartForward):
self._mc.start_forward(velocity=obj.velocity)
elif isinstance(obj, StartLeft):
self._mc.start_left(velocity=obj.velocity)
elif isinstance(obj, StartLinearMotion):
self._mc.start_linear_motion(obj.vx, obj.vy, obj.vz)
elif isinstance(obj, StartRight):
self._mc.start_right(velocity=obj.velocity)
elif isinstance(obj, StartTurnLeft):
self._mc.start_turn_left(rate=obj.rate)
elif isinstance(obj, StartTurnRight):
self._mc.start_turn_right(rate=obj.rate)
elif isinstance(obj, StartUp):
self._mc.start_up(velocity=obj.velocity)
elif isinstance(obj, Stop):
self._mc.stop()
else:
return 'Object is not a valid velocity command'
except Exception as e:
print(str(e))
raise e
return 'ok'
"""
Action Implementations
"""
def _position_control_cb(self, goal):
try:
obj = pickle.loads(goal.pickle)
if isinstance(obj, Back):
self._mc.back(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, CircleLeft):
self._mc.circle_left(obj.radius_m,
velocity=obj.velocity,
angle_degrees=obj.angle_degrees)
elif isinstance(obj, CircleRight):
self._mc.circle_right(obj.radius_m,
velocity=obj.velocity,
angle_degrees=obj.angle_degrees)
elif isinstance(obj, Down):
self._mc.down(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, Forward):
self._mc.forward(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, Land):
self._mc.land(velocity=obj.velocity)
elif isinstance(obj, Left):
self._mc.left(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, MoveDistance):
self._mc.move_distance(obj.x,
obj.y,
obj.z,
velocity=obj.velocity)
elif isinstance(obj, Right):
self._mc.right(obj.distance_m, velocity=obj.velocity)
elif isinstance(obj, TakeOff):
self._mc.take_off(height=obj.height, velocity=obj.velocity)
elif isinstance(obj, TurnLeft):
self._mc.turn_left(obj.angle_degrees, rate=obj.rate)
elif isinstance(obj, TurnRight):
self._mc.turn_right(obj.angle_degrees, rate=obj.rate)
elif isinstance(obj, Up):
self._mc.up(obj.distance_m, velocity=obj.velocity)
except Exception as e:
print('Exception in action server %s' % self._name +
'/position_control')
print(str(e))
print('Action aborted')
self._position_control_as.set_aborted()
return
self._position_control_as.set_succeeded()
def _takeoff(self, goal):
try:
self._mc.take_off(height=goal.height)
time.sleep(5)
except BaseException as e:
self._takeoff_as.set_aborted()
print(e)
return
self._takeoff_as.set_succeeded(TakeOffResult(True))
def _land(self, goal):
try:
self._mc.land(velocity=goal.velocity)
except BaseException as e:
self._land_as.set_aborted()
print(e)
return
self._land_as.set_succeeded(LandResult(True))
def _move_distance(self, goal):
try:
x = goal.x
y = goal.y
z = goal.z
velocity = goal.velocity
dist = np.sqrt(x**2 + y**2 + z**2)
vx = x / dist * velocity
vy = y / dist * velocity
vz = z / dist * velocity
# self._velocity_setpoint_pub.publish(Vector3(vx, vy, vz))
self._mc.move_distance(x, y, z, velocity=velocity)
# self._velocity_setpoint_pub.publish(Vector3(vx, vy, vz))
except BaseException as e:
self._move_distance_as.set_aborted()
print(e)
return
self._move_distance_as.set_succeeded()
class FlightCtrl:
#initialize variables for attitude tracking
yawInit = 0
yawCurr = 0
theta = 0
rtCoef = [0, -1]
lfCoef = [0, 1]
fwCoef = [1, 0]
bkCoef = [-1, 0]
lvSpeed = 0.3
def __init__(self, _scf):
self.mc = MotionCommander(_scf, default_height=0.7)
self.scf = _scf
self.scf.open_link()
def perform_gesture(self, g_id):
global consecDoubleTaps
global inMotion
d = 0.3
if g_id[0] == DOUBLE_TAP:
if self.mc._is_flying:
print("Hovering...")
#mc.stop()
self.resetYawInit()
else:
consecDoubleTaps = 0
print("Taking off...")
inMotion = True
self.mc.take_off()
inMotion = False
self.resetYawInit()
threadUpdate = Thread(target=self._updateYaw,
args=(self.scf, ))
threadUpdate.start()
elif g_id[0] == NO_POSE and g_id[1] == yaw_id:
print("Roll...")
inMotion = True
self.mc.move_distance(0, math.copysign(d, g_id[2]), 0)
inMotion = False
"""if (g_id[2] > 0):
#turn left
print("turning left")
inMotion = True
self.mc.move_distance(self.lvSpeed * self.lfCoef[0], self.lvSpeed * self.lfCoef[1], 0)
inMotion = False
else:
#turn right
print("turning right")
inMotion = True
self.mc.move_distance(self.lvSpeed * self.rtCoef[0], self.lvSpeed * self.rtCoef[1], 0)
inMotion = False
"""
elif g_id[0] == NO_POSE and g_id[1] == pitch_id:
print("Pitch...")
inMotion = True
self.mc.move_distance(-math.copysign(d, g_id[2]), 0, 0)
inMotion = False
"""if (g_id[2] < 0):
#move forward
print("moving forward")
inMotion = True
self.mc.move_distance(self.lvSpeed * self.fwCoef[0], self.lvSpeed * self.fwCoef[1], 0)
inMotion = False
else:
#move backward
print("moving backward")
inMotion = True
self.mc.move_distance(self.lvSpeed * self.bkCoef[0], self.lvSpeed * self.bkCoef[1], 0)
inMotion = False
"""
elif g_id[0] == FINGERS_SPREAD and g_id[1] == pitch_id:
if g_id[2] > 0:
if self.mc._thread.get_height() + d < self.mc.max_height:
print("Up...")
inMotion = True
self.mc.up(d)
inMotion = False
else:
print("Max. height %.2fm reached: requested height: %.2f"
) % (self.mc.max_height,
self.mc._thread.get_height() + d)
else:
if self.mc._thread.get_height() - d < self.mc.min_height:
print("Down...")
inMotion = True
self.mc.down(d)
inMotion = False
else:
print("Max. height %.2fm reached: requested height: %.2f"
) % (self.mc.max_height,
self.mc._thread.get_height() + d)
elif g_id[0] == FIST and g_id[1] == yaw_id:
print('Yaw...')
if g_id[2] > 0:
inMotion = True
self.mc.turn_left(30)
inMotion = False
else:
inMotion = True
self.mc.turn_right(30)
inMotion = False
elif g_id[0] == LAND:
print("Landing...")
inMotion = True
self.mc.land()
inMotion = False
else: #rest behaviour
if self.mc._is_flying:
inMotion = True
self.mc.stop()
inMotion = False
"""Functions to update attitude by reading storage text file"""
def updateCoef(self):
try:
self.theta = (self.yawCurr - self.yawInit) * (3.1415926 / 180)
self.rtCoef = [-sin(self.theta), -cos(self.theta)]
self.lfCoef = [sin(self.theta), cos(self.theta)]
self.fwCoef = [cos(self.theta), -sin(self.theta)]
self.bkCoef = [-cos(self.theta), sin(self.theta)]
except Exception, e:
print str(e)
print("Update failed")
