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)
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 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 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()
def key_ctrl(self, scf): mc = MotionCommander(scf) mc._reset_position_estimator() print 'Spacebar to start' raw_input() pygame.display.set_mode((400, 300)) print 'WASD for throttle & yaw; arrow keys for left/right/forward/backward' print 'Spacebar to land' vel_x = 0 vel_y = 0 vel_z = 0 yaw_rate = 0 try: mc.take_off() #set inital Yaw value with open('SensorMaster.txt', 'r') as stbFile: stbLines = stbFile.readlines() while len(stbLines) == 0: with open('SensorMaster.txt', 'r') as stbFile: stbLines = stbFile.readlines() currAttitude = stbLines[len(stbLines) - 1] need, to, currentYaw, test = currAttitude.split(',') self.yawCurr = float(currentYaw) Thread(target=self._updateYaw, args=(scf, )).start() while True: for event in pygame.event.get(): if event.type == pygame.KEYDOWN: if event.key == pygame.K_w: vel_z = 0.3 if event.key == pygame.K_SPACE: print 'Space pressed, landing' mc.land() time.sleep(2) print 'Closing link...' scf.close_link() print 'Link closed' pygame.quit() sys.exit(0) if event.key == pygame.K_a: yaw_rate = -45 if event.key == pygame.K_s: vel_z = -0.3 if event.key == pygame.K_d: yaw_rate = 45 if event.key == pygame.K_UP: #vel_x = 0.3 vel_x = 0.2 * self.fwCoef[0] vel_y = 0.2 * self.fwCoef[1] print('vel_x = %.2f, vel_y = %.2f' % (vel_x * self.fwCoef[0], vel_y * self.fwCoef[1])) print("the current yaw is:") print(self.yawCurr) if event.key == pygame.K_DOWN: #vel_x = -0.3 vel_x = 0.2 * self.bkCoef[0] vel_y = 0.2 * self.bkCoef[1] print('vel_x = %.2f, vel_y = %.2f' % (vel_x * self.bkCoef[0], vel_y * self.bkCoef[1])) print("the current yaw is:") print(self.yawCurr) if event.key == pygame.K_LEFT: #vel_y = 0.3 vel_x = 0.2 * self.lfCoef[0] vel_y = 0.2 * self.lfCoef[1] print('vel_x = %.2f, vel_y = %.2f' % (vel_x * self.bkCoef[0], vel_y * self.bkCoef[1])) print("the current yaw is:") print(self.yawCurr) if event.key == pygame.K_RIGHT: #vel_y = -0.3 vel_x = 0.2 * self.rtCoef[0] vel_y = 0.2 * self.rtCoef[1] print('vel_x = %.2f, vel_y = %.2f' % (vel_x * self.bkCoef[0], vel_y * self.bkCoef[1])) print("the current yaw is:") print(self.yawCurr) if event.key == pygame.K_r: #set recalibrate initial Yaw value with open('SensorMaster.txt', 'r') as stbFile: stbLines = stbFile.readlines() while len(stbLines) == 0: with open('SensorMaster.txt', 'r') as stbFile: stbLines = stbFile.readlines() print("yaw angle recalibrated :") newInitAttitude = stbLines[len(stbLines) - 1] need, to, initYaw, test = newInitAttitude.split( ',') print(initYaw) self.yawInit = float(initYaw) if event.key == pygame.K_RCTRL: mc.stop() if event.type == pygame.KEYUP: if event.key == pygame.K_w or event.key == pygame.K_s: vel_z = 0 if event.key == pygame.K_a or event.key == pygame.K_d: yaw_rate = 0 if event.key == pygame.K_UP or event.key == pygame.K_DOWN: vel_x = 0 if event.key == pygame.K_LEFT or event.key == pygame.K_RIGHT: vel_y = 0 mc._set_vel_setpoint(vel_x, vel_y, vel_z, yaw_rate) except KeyboardInterrupt: print('\nShutting down...') scf.close_link() except Exception, e: print str(e) scf.close_link()
class CrazyDrone(Drone): def __init__(self, link_uri): super().__init__() cache = "./cache" if getattr(sys, 'frozen', False): cache = sys._MEIPASS + os.path.sep + "cache" self._cf = Crazyflie(rw_cache=cache) self.motion_commander = None self.multiranger = None # maximum speeds self.max_vert_speed = 1 self.max_horiz_speed = 1 self.max_rotation_speed = 90 self.logger = None # Connect some callbacks from the Crazyflie API self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) print('Connecting to %s' % link_uri) # Try to connect to the Crazyflie self._cf.open_link(link_uri) # Variable used to keep main loop occupied until disconnect self.is_connected = True def _connected(self, link_uri): """ This callback is called form the Crazyflie API when a Crazyflie has been connected and the TOCs have been downloaded.""" print('Connected to %s' % link_uri) self.connection.emit("progress") # The definition of the logconfig can be made before connecting self.logger = LogConfig("Battery", 1000) # delay self.logger.add_variable("pm.vbat", "float") try: self._cf.log.add_config(self.logger) self.logger.data_received_cb.add_callback( lambda e, f, g: self.batteryValue.emit(float(f['pm.vbat']))) # self.logger.error_cb.add_callback(lambda: print('error')) self.logger.start() except KeyError as e: print(e) self.connection.emit("on") self.motion_commander = MotionCommander(self._cf, 0.5) self.multiranger = Multiranger(self._cf, rate_ms=50) self.multiranger.start() def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the speficied address)""" print('Connection to %s failed: %s' % (link_uri, msg)) self.is_connected = False self.connection.emit("off") def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" print('Connection to %s lost: %s' % (link_uri, msg)) self.connection.emit("off") def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print('Disconnected from %s' % link_uri) self.is_connected = False self.connection.emit("off") def take_off(self): if self._cf.is_connected( ) and self.motion_commander and not self.motion_commander._is_flying: self.motion_commander.take_off() self.is_flying_signal.emit(True) def land(self): if self._cf.is_connected( ) and self.motion_commander and self.motion_commander._is_flying: self.motion_commander.land() self.is_flying_signal.emit(False) def stop(self): if not (self.logger is None): self.logger.stop() if self.motion_commander: self.motion_commander.land() if self.multiranger: self.multiranger.stop() self._cf.close_link() def is_flying(self): if self._cf.is_connected() and self.motion_commander: return self.motion_commander._is_flying return False def process_motion(self, _up, _rotate, _front, _right): if self.motion_commander: # WARNING FOR CRAZYFLY # positive X is forward, # positive Y is left # positive Z is up velocity_z = _up * self.max_vert_speed velocity_yaw = _rotate * self.max_rotation_speed velocity_x = _front * self.max_horiz_speed velocity_y = -_right * self.max_horiz_speed # print("PRE", velocity_x, velocity_y, velocity_z, velocity_yaw) # protect against collision by reducing speed depending on distance ranger = self.multiranger if ranger.front and ranger.front < ANTI_COLLISION_DISTANCE and velocity_x > 0: velocity_x = velocity_x * ( ranger.front - ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE velocity_x = max(0, velocity_x) if ranger.back and ranger.back < ANTI_COLLISION_DISTANCE and velocity_x < 0: velocity_x = velocity_x * ( ranger.back - ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE velocity_x = min(0, velocity_x) if ranger.left and ranger.left < ANTI_COLLISION_DISTANCE and velocity_y > 0: velocity_y = velocity_y * ( ranger.left - ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE velocity_y = max(0, velocity_y) if ranger.right and ranger.right < ANTI_COLLISION_DISTANCE and velocity_y < 0: velocity_y = velocity_y * ( ranger.left - ANTI_COLLISION_MIN_DIST) / ANTI_COLLISION_DISTANCE velocity_y = min(0, velocity_y) if ranger.up and ranger.up < ANTI_COLLISION_DISTANCE and velocity_z > 0: velocity_z = velocity_z * (ranger.up - ANTI_COLLISION_MIN_DIST ) / ANTI_COLLISION_DISTANCE velocity_z = max(0, velocity_z) # print("POST", velocity_x, velocity_y, velocity_z, velocity_yaw) if self.motion_commander._is_flying: self.motion_commander._set_vel_setpoint( velocity_x, velocity_y, velocity_z, velocity_yaw)
class 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()
else: velocity_x = -0.5 # velocity_z = pid_height.update(vertical_distance) if (x_distance < 50): if calculator.alpha > 0: velocity_y = 0.3 else: velocity_y = -0.3 if (calculator.horizontal_distance > 0): yaw = 30 else: yaw = -30 mc._set_vel_setpoint(velocity_x, velocity_y, 0, yaw) mc.setIsRunning(True) else: # Drone interrompe o movimento somente se não encontrar o marcador # em 3 frames consecutivos notFoundCount += 1 if ((3 <= notFoundCount <= 20) and mc.isRunning): mc.stop() mc.setIsRunning(False) elif (notFoundCount > 20): mc._set_vel_setpoint(0.0, 0.0, 0.0, 100) mc.setIsRunning(True) calculator.writeDistance(frame) cv2.imshow('frame', frame)