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")