def line_sequence(velocity): global pos_fixed #先记录固定点的坐标用于计算距离 with SyncCrazyflie(uri_fixed, cf=Crazyflie(rw_cache='./cache')) as scf: lpos = LogConfig(name='position', period_in_ms=100) lpos.add_variable('stateEstimate.x') lpos.add_variable('stateEstimate.y') lpos.add_variable('stateEstimate.z') scf.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(pos_data_fixed) lpos.start() time.sleep(2) lpos.stop() pos_fixed = np.mean(fixed_dt, axis=0).tolist() print(pos_fixed) with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: lpos = LogConfig(name='position', period_in_ms=100) lpos.add_variable('stateEstimate.x') lpos.add_variable('stateEstimate.y') lpos.add_variable('stateEstimate.z') lpos.add_variable(distance_mapper[uri_fixed]) scf.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(pos_data) with PositionHlCommander(scf, default_velocity=velocity) as pc: time.sleep(2) pc.go_to(2.0, 0.0, 0.5) lpos.start() pc.go_to(-0.5, 0, 0.5) lpos.stop() df = pd.DataFrame( moving_dt, columns=['x', 'y', 'z', 'distance_UWB', 'distance_lighthouse']) df.to_csv('./antenna_' + str(velocity) + '_154.65_8' + '.csv', index=False)
def simple_sequence(): with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: scf.cf.param.add_update_callback(group="deck", name="bcLighthouse4", cb=param_deck_flow) time.sleep(1) logconf = LogConfig(name='Parameters', period_in_ms=SAMPLE_PERIOD_MS) for param in log_parameters: logconf.add_variable(param[0], param[1]) scf.cf.log.add_config(logconf) logconf.data_received_cb.add_callback(logconf_callback) if not is_deck_attached: return logconf.start() with PositionHlCommander( scf, x=0.0, y=0.0, z=0.0, default_velocity=VELOCITY, default_height=DEFAULT_HEIGHT, controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc: for position in SEQUENCE: pc.go_to(position[0], position[1]) time.sleep(5) logconf.stop() write_log_history()
def consumer(queue): with SyncCrazyflie(uriDron, cf=Crazyflie(rw_cache='./cache2')) as scf2: fileQueue = open(filenameQueue,'w') lg_stab = LogConfig(name='Stabilizer', period_in_ms=100) lg_stab.add_variable('stateEstimate.x', 'float') lg_stab.add_variable('stateEstimate.y', 'float') lg_stab.add_variable('stateEstimate.z', 'float') scf2.cf.log.add_config(lg_stab) lg_stab.data_received_cb.add_callback(log_stab_callback) lg_stab.start() scf2.cf.commander.send_position_setpoint(0, 0, 0.4, 0) message = queue.get() p = [0, 0] q = [message[0], message[1]] d = math.dist(p, q) x = message[0] y = message[1] z = 0.5 if d > 0.15: n = int(d/0.1) for g in range(1, n): x_i = x*(g/n) y_i = y*(g/n) s = '{}, {}, {}\n'.format{x_i, y_i, z} fileQueue.write(s) print('Step {} ({}, {}, {})'.format(g, x_i, y_i, z)) scf2.cf.commander.send_position_setpoint(x_i, y_i, z, 0) time.sleep(1) else: print('Dron Inital pos ({}, {}, {})'.format(x, y, z)) scf2.cf.commander.send_position_setpoint(x, y, z, 0) s = '{}, {}, {}\n'.format{x, y, z} fileQueue.write(s) time.sleep(1) while (is_finished == False) or (not(queue.empty())): message = queue.get() x = message[0] y = message[1] z = 0.5 s = '{}, {}, {}\n'.format(x, y, z) fileQueue.write(s) print('Move dron to ({}, {}, {})'.format(x, y, z)) scf2.cf.commander.send_position_setpoint(x, y, z, 0) time.sleep(0.2) scf2.cf.commander.send_stop_setpoint() time.sleep(1) lg_stab.stop() fileQueue.close() with open(filenameDron, 'w') as f: for q in dronPositions: a = '{}, {}, {}\n'.format(q[0], q[1], q[2]) f.write(q) print('Consumer ended')
def start_position_printing(scf): log_conf = LogConfig(name='Position', period_in_ms=500) log_conf.add_variable('kalman.stateX', 'float') log_conf.add_variable('kalman.stateY', 'float') log_conf.add_variable('kalman.stateZ', 'float') scf.cf.log.add_config(log_conf) log_conf.data_received_cb.add_callback(position_callback) log_conf.start() time.sleep(5) log_conf.stop()
class CFLog(): def __init__(self): self.status = CFStatus() def start(self, cf): self._lc_stab = LogConfig(name="Log-Stab", period_in_ms=50) self._lc_stab.add_variable("stabilizer.roll", "float") self._lc_stab.add_variable("stabilizer.pitch", "float") self._lc_stab.add_variable("stabilizer.yaw", "float") self._lc_stab.add_variable("stabilizer.thrust", "float") self._lc_motor = LogConfig(name="Log-Motor", period_in_ms=50) self._lc_motor.add_variable("pm.vbat", "float") self._lc_motor.add_variable("motor.m1", "float") # Front (green) self._lc_motor.add_variable("motor.m2", "float") # Right self._lc_motor.add_variable("motor.m3", "float") # Back (red) self._lc_motor.add_variable("motor.m4", "float") # Left cf.log.add_config(self._lc_stab) cf.log.add_config(self._lc_motor) if self._lc_stab.valid and self._lc_motor.valid: self._lc_stab.data_received_cb.add_callback(self._log_data) self._lc_stab.error_cb.add_callback(self._log_error) self._lc_stab.start() self._lc_motor.data_received_cb.add_callback(self._log_data) self._lc_motor.error_cb.add_callback(self._log_error) self._lc_motor.start() logger.info("Starting CFLog") else: logger.error( "Could not add logconfig since some variables are not in TOC") def stop(self): self._lc_stab.stop() def _log_error(self, logconf, msg): """Callback from the log API when an error occurs""" logger.info("Error when logging %s: %s" % (logconf.name, msg)) def _log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" if logconf.name == "Log-Stab": self.status.pitch = data["stabilizer.pitch"] self.status.roll = data["stabilizer.roll"] self.status.yaw = data["stabilizer.yaw"] self.status.thrust = data["stabilizer.thrust"] else: self.status.bat = data["pm.vbat"] self.status.motor_1 = data["motor.m1"] self.status.motor_2 = data["motor.m2"] self.status.motor_3 = data["motor.m3"] self.status.motor_4 = data["motor.m4"] logger.info("%s" % self.status)
def run_sequence(scf, sequence): try: cf = scf.cf lpos = LogConfig(name='position', period_in_ms=60) lpos.add_variable('stateEstimate.x') lpos.add_variable('stateEstimate.y') lpos.add_variable('stateEstimate.z') # 配置 if scf.cf.link_uri == URI: lpos.add_variable(distance_mapper[URI_fixed]) else: pass scf.cf.log.add_config(lpos) # 回调 if scf.cf.link_uri == URI: lpos.data_received_cb.add_callback(pos_data) else: lpos.data_received_cb.add_callback(pos_data_fixed) # lpos.start() # time.sleep(500) # lpos.stop() with PositionHlCommander(scf) as pc: if scf.cf.link_uri == URI: # 等待二号无人机飞到指定地点后开始做事情 pc.go_to(sequence[0][0], sequence[0][1], sequence[0][2]) time.sleep(2) for i in range(cyc): for idx, ele in enumerate(sequence): if idx == 0: pass elif idx == 1: pc.set_default_velocity(0.5) pc.go_to(ele[0], ele[1], ele[2]) else: pc.set_default_velocity(vel) lpos.start() pc.go_to(ele[0], ele[1], ele[2]) lpos.stop() else: for ele in sequence: pc.go_to(ele[0], ele[1], ele[2]) time.sleep(ele[3]) if scf.cf.link_uri == URI: df = pd.DataFrame( moving_dt, columns=['x', 'y', 'z', 'distance_UWB', 'distance_lighthouse']) df.to_csv('./VelocitySwarm_50ms_' + str(vel) + '_far.csv', index=False) except Exception as e: print(e)
class CFLog: def __init__(self): self.status = CFStatus() def start(self, cf): self._lc_stab = LogConfig(name="Log-Stab", period_in_ms=50) self._lc_stab.add_variable("stabilizer.roll", "float") self._lc_stab.add_variable("stabilizer.pitch", "float") self._lc_stab.add_variable("stabilizer.yaw", "float") self._lc_stab.add_variable("stabilizer.thrust", "float") self._lc_motor = LogConfig(name="Log-Motor", period_in_ms=50) self._lc_motor.add_variable("pm.vbat", "float") self._lc_motor.add_variable("motor.m1", "float") # Front (green) self._lc_motor.add_variable("motor.m2", "float") # Right self._lc_motor.add_variable("motor.m3", "float") # Back (red) self._lc_motor.add_variable("motor.m4", "float") # Left cf.log.add_config(self._lc_stab) cf.log.add_config(self._lc_motor) if self._lc_stab.valid and self._lc_motor.valid: self._lc_stab.data_received_cb.add_callback(self._log_data) self._lc_stab.error_cb.add_callback(self._log_error) self._lc_stab.start() self._lc_motor.data_received_cb.add_callback(self._log_data) self._lc_motor.error_cb.add_callback(self._log_error) self._lc_motor.start() logger.info("Starting CFLog") else: logger.error("Could not add logconfig since some variables are not in TOC") def stop(self): self._lc_stab.stop() def _log_error(self, logconf, msg): """Callback from the log API when an error occurs""" logger.info("Error when logging %s: %s" % (logconf.name, msg)) def _log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" if logconf.name == "Log-Stab": self.status.pitch = data["stabilizer.pitch"] self.status.roll = data["stabilizer.roll"] self.status.yaw = data["stabilizer.yaw"] self.status.thrust = data["stabilizer.thrust"] else: self.status.bat = data["pm.vbat"] self.status.motor_1 = data["motor.m1"] self.status.motor_2 = data["motor.m2"] self.status.motor_3 = data["motor.m3"] self.status.motor_4 = data["motor.m4"] logger.info("%s" % self.status)
def testeBateria(self, cf): time.sleep(0.5) lg_stab = LogConfig(name='Battery', period_in_ms=100) lg_stab.add_variable('pm.vbat', 'float') try: cf.log.add_config(lg_stab) #This callback will receive the data lg_stab.data_received_cb.add_callback(self.stab_log_data) # Start the logging lg_stab.start() except AttributeError: print('Could not add Stabilizer log config, bad configuration.') time.sleep(0.3) lg_stab.stop() time.sleep(0.3)
class MyLog: def __init__(self): self.reload() def reload(self): self._lc_stab = LogConfig(name="Magnetometer", period_in_ms=100) # self._lc_stab.add_variable("stabilizer.roll", "float") # self._lc_stab.add_variable("stabilizer.pitch", "float") # self._lc_stab.add_variable("stabilizer.yaw", "float") # self._lc_stab.add_variable("stabilizer.thrust", "float") # self._lc_stab.add_variable("pm.vbat", "float") # self._lc_stab.add_variable("motor.m1", "float") #Front (green) # self._lc_stab.add_variable("motor.m2", "float") # Right # self._lc_stab.add_variable("motor.m3", "float") # Back (red) # self._lc_stab.add_variable("motor.m4", "float") # Left def getLogConfig(self): return self._lc_stab def start(self): if self._lc_stab.valid: # This callback will receive the data self._lc_stab.data_received_cb.add_callback(self._log_data) # This callback will be called on errors self._lc_stab.error_cb.add_callback(self._log_error) # Start the logging self._lc_stab.start() else: print "Could not add logconfig since some variables are not in TOC" def stop(self): self._lc_stab.stop() def _log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print "Error when logging %s: %s" % (logconf.name, msg) def _log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" print "%s: %s" % (logconf.name, data)
def distance_measurement(): for uri in uris: with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: lpos = LogConfig(name='rxCNT', period_in_ms=500) lpos.add_variable('rxCNT.rxcnt10') lpos.add_variable('rxCNT.rxcnt20') lpos.add_variable('rxCNT.rxcnt30') lpos.add_variable('rxCNT.rxcnt40') scf.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(pos_data) lpos.start() time.sleep(2) lpos.stop() df = pd.DataFrame( dt, columns=['uri', 'rxcnt10', 'rxcnt20', 'rxcnt30', 'rxcnt40']) df.to_csv('./rxcnt.csv', index=False) dt.clear() dt_mapper.clear() for uri in uris: with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: lpos = LogConfig(name='rxCNT', period_in_ms=500) lpos.add_variable('rangingCNT.rangingcnt10') lpos.add_variable('rangingCNT.rangingcnt20') lpos.add_variable('rangingCNT.rangingcnt30') lpos.add_variable('rangingCNT.rangingcnt40') scf.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(pos_data2) lpos.start() time.sleep(2) lpos.stop() df = pd.DataFrame(dt, columns=[ 'uri', 'rangingcnt10', 'rangingcnt20', 'rangingcnt30', 'rangingcnt40' ]) df.to_csv('./rangingcnt.csv', index=False) print("done") return
else: is_deck_attached = False print('Deck is NOT attached!') if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: scf.cf.param.add_update_callback(group="deck", name="bcLighthouse4", cb=param_deck_flow) time.sleep(1) logconf = LogConfig(name='Parameters', period_in_ms=SAMPLE_PERIOD_MS) for param in log_parameters: logconf.add_variable(param[0], param[1]) scf.cf.log.add_config(logconf) logconf.data_received_cb.add_callback(logconf_callback) if is_deck_attached: logconf.start() take_off_simple(scf) logconf.stop() write_log_history()
ax, line, )) if args.manual_control: _thread.start_new_thread(manual_control, (scf, )) if args.log_pos: time.sleep(5) logconf_pos.start() plt.show() if not args.manual_control and not args.live_plot: while (1): try: time.sleep(0.001) except KeyboardInterrupt: break # Stop logging and end if args.log_pos: logconf_pos.stop() if args.write_to_file: pos_file_handler.close() if args.log_temp: logconf_temp.stop() if args.write_to_file: temp_file_handler.close() if args.log_range: logconf_range.stop() if args.write_to_file: range_file_handler.close()
baduanjin_sound_thread_p2 = threading.Thread( name='P2-Baduanjin-Sound-Thread', target=open_baduanjin_sound_p2, args=()) # Starting threads for movement no.2 baduanjin_sound_thread_p2.start() # Perform the movement move_baduanjin_mc_p2(scf_1) # Threads join baduanjin_sound_thread_p2.join() # # # Movement no.3 (MotionCommander) # # Declaring feedback threads for movement no.3 # baduanjin_sound_thread_p3 = threading.Thread(name='P2-Baduanjin-Sound-Thread', target=open_baduanjin_sound_p3, args=()) # # Starting threads for movement no.3 # baduanjin_sound_thread_p3.start() # # Perform the movement # move_baduanjin_mc_p3(scf_1) # # Threads join # baduanjin_sound_thread_p3.join() time.sleep(3) logconf_1.stop()
class CrazyRadioClient: """ CrazyRadio client that recieves the commands from the controller and sends them in a CRTP package to the crazyflie with the specified address. """ def __init__(self): # Setpoints to be sent to the CrazyFlie self.roll = 0.0 self.pitch = 0.0 self.yaw = 0.0 self.motor1cmd = 0.0 self.motor2cmd = 0.0 self.motor3cmd = 0.0 self.motor4cmd = 0.0 self._status = DISCONNECTED self.link_uri = "" # self.status_pub = rospy.Publisher(node_name + '/CrazyRadioStatus', Int32, queue_size=1) # self.FlyingAgentClient_command_pub = rospy.Publisher('FlyingAgentClient/Command', Int32, queue_size=1) time.sleep(1.0) # Initialize the CrazyFlie and add callbacks self._init_cf() # Connect to the Crazyflie self.connect() def _init_cf(self): self._cf = Crazyflie() # Add callbacks that get executed depending on the connection _status. 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) def get_status(self): return self._status def update_link_uri(self): self.link_uri = "radio://0/79/2M" def connect(self): # update link from ros params self.update_link_uri() print "Connecting to %s" % self.link_uri self._cf.open_link(self.link_uri) def disconnect(self): print "Motors OFF" self._send_to_commander(0, 0, 0, 0, 0, 0, 0, 0, CONTROLLER_MOTOR) print "Disconnecting from %s" % self.link_uri self._cf.close_link() def _data_received_callback(self, timestamp, data, logconf): #print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data) batteryVolt = Float32() stabilizerYaw = Float32() stabilizerPitch = Float32() stabilizerRoll = Float32() batteryVolt.data = data["pm.vbat"] stabilizerYaw.data = data["stabilizer.yaw"] stabilizerPitch.data = data["stabilizer.pitch"] def _logging_error(self, logconf, msg): print "Error when logging %s" % logconf.name # def _init_logging(self): def _start_logging(self): self.logconf = LogConfig("LoggingTest", 100) # second variable is freq in ms self.logconf.add_variable("stabilizer.roll", "float") self.logconf.add_variable("stabilizer.pitch", "float") self.logconf.add_variable("stabilizer.yaw", "float") self.logconf.add_variable("pm.vbat", "float") self._cf.log.add_config(self.logconf) if self.logconf.valid: self.logconf.data_received_cb.add_callback( self._data_received_callback) self.logconf.error_cb.add_callback(self._logging_error) print "logconf valid" else: print "logconf invalid" self.logconf.start() print "logconf start" def _connected(self, link_uri): """ This callback is executed as soon as the connection to the quadrotor is established. """ # cf_client._send_to_commander(15000, 15000, 15000, 15000); # cf_client._send_to_commander_rate(1000, 0, 1000, 0, 1, 1, 1) cf_client._send_to_commander_angle(1000, 0, 1000, 0, 1, 1, 1) print "sent command to commander" # Config for Logging self._start_logging() def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the specified address)""" rospy.logerr("Connection to %s failed: %s" % (link_uri, msg)) def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" rospy.logerr("Connection to %s lost: %s" % (link_uri, msg)) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" self.logconf.stop() rospy.loginfo("logconf stopped") self.logconf.delete() rospy.loginfo("logconf deleted") def _send_to_commander_motor(self, cmd1, cmd2, cmd3, cmd4): pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC pk.data = struct.pack('<BHHHH', CF_COMMAND_TYPE_MOTORS, cmd1, cmd2, cmd3, cmd4) self._cf.send_packet(pk) def _send_to_commander_rate(self, cmd1, cmd2, cmd3, cmd4, roll_rate, pitch_rate, yaw_rate): pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_RATE, cmd1, cmd2, cmd3, cmd4, roll_rate, pitch_rate, yaw_rate) self._cf.send_packet(pk) def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw): pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_ANGLE, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw) self._cf.send_packet(pk)
class Drone: """Represents a CrazyFlie drone.""" def __init__(self, drone_id: str, arena: Arena, radio_id: int = 0, channel: int = 80, address: str = "E7E7E7E7E7", data_rate: str = "2M"): """ Initializes the drone with the given uri.""" # Initialize public variables self.id: str = drone_id self.var_x: float = 0 self.var_y: float = 0 self.var_z: float = 0 self.pos_x: float = 0 self.pos_y: float = 0 self.pos_z: float = 0 self.pitch: float = 0 self.roll: float = 0 self.yaw: float = 0 self.battery_voltage: float = 0 self.is_connected: bool = False self.status: DroneState = DroneState.OFFLINE self.link_uri: str = "radio://" + str(radio_id) + "/" + str( channel) + "/" + data_rate + "/" + address # Initialize limits self._max_velocity: float = 1.0 self._min_duration: float = 1.0 self._max_yaw_rotations: float = 1.0 self._arena = arena # Event to asynchronously wait for the connection self._connect_event = threading.Event() # Initialize the crazyflie self._cf = Crazyflie(rw_cache='./cache') # Initialize the callbacks 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) # Initialize events self.drone_lost = Caller() # Define the log configuration self._log_config_1 = LogConfig(name='DroneLog_1', period_in_ms=500) self._log_config_1.add_variable('kalman.varPX', 'float') self._log_config_1.add_variable('kalman.varPY', 'float') self._log_config_1.add_variable('kalman.varPZ', 'float') self._log_config_1.add_variable('pm.vbat', 'float') self._log_config_2 = LogConfig(name='DroneLog_2', period_in_ms=100) self._log_config_2.add_variable('kalman.stateX', 'float') self._log_config_2.add_variable('kalman.stateY', 'float') self._log_config_2.add_variable('kalman.stateZ', 'float') self._log_config_3 = LogConfig(name='DroneLog_3', period_in_ms=500) self._log_config_3.add_variable('stabilizer.pitch', 'float') self._log_config_3.add_variable('stabilizer.roll', 'float') self._log_config_3.add_variable('stabilizer.yaw', 'float') def connect(self, synchronous: bool = False): """Connects to the Crazyflie.""" self._connect_crazyflie() if synchronous: self._connect_event.wait() def disconnect(self): """Disconnects from the Crazyflie and stops all logging.""" self._disconnect_crazyflie() def enable_high_level_commander(self): """Enables the drones high level commander.""" self._cf.param.set_value('commander.enHighLevel', '1') time.sleep(0.1) def disable_motion_tracking(self): """Disables to motion control (x/y) from the flow-deck.""" self._cf.param.set_value('motion.disable', '1') time.sleep(0.1) def get_status(self) -> str: """Gets various information of the drone.""" return { "id": self.id, "var_x": self.var_x, "var_y": self.var_y, "var_z": self.var_z, "x": self._arena.transform_x_inverse(self.pos_x), "y": self._arena.transform_y_inverse(self.pos_y), "z": self.pos_z, "pitch": self.pitch, "roll": self.roll, "yaw": self.yaw, "status": self.status.name, "battery_voltage": self.battery_voltage, "battery_percentage:": (self.battery_voltage - 3.4) / (4.18 - 3.4) * 100 } def reset_estimator(self) -> bool: """Resets the position estimates.""" self._cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) self._cf.param.set_value('kalman.resetEstimation', '0') time.sleep(2.0) # TODO: wait_for_position_estimator(cf) return True def takeoff(self, absolute_height: float, velocity: float, synchronous: bool = False) -> float: absolute_height = self._sanitize_z(absolute_height, False) self.reset_estimator() duration = self._convert_velocity_to_time(absolute_height, velocity) self._cf.high_level_commander.takeoff(absolute_height, duration) self.status = DroneState.STARTING if synchronous: time.sleep(duration) return {"duration": duration, "target_z": absolute_height} def land(self, absolute_height: float, velocity: float, synchronous: bool = False) -> float: absolute_height = self._sanitize_z(absolute_height, False) duration = self._convert_velocity_to_time(absolute_height, velocity) self._cf.high_level_commander.land(absolute_height, duration) self.status = DroneState.LANDING if synchronous: time.sleep(duration) return {"duration": duration, "target_z": absolute_height} def go_to(self, x: float, y: float, z: float, yaw: float, velocity: float, relative: bool = False, synchronous: bool = False) -> float: x = self._sanitize_x(x, relative) y = self._sanitize_y(y, relative) z = self._sanitize_z(z, relative) yaw = self._sanitize_yaw(yaw) distance = self._calculate_distance(x, y, z, relative) duration = self._convert_velocity_to_time(distance, velocity) self._cf.high_level_commander.go_to(x, y, z, yaw, duration, relative) self.status = DroneState.NAVIGATING if synchronous: time.sleep(duration) return { "duration": duration, "target_x": self._arena.transform_x_inverse(x), "target_y": self._arena.transform_y_inverse(y), "target_z": z, "target_yaw": yaw, "relative": relative } def stop(self): self._cf.high_level_commander.stop() self.status = DroneState.IDLE def _connect_crazyflie(self): print('Connecting to %s' % self.link_uri) self._cf.open_link(self.link_uri) def _disconnect_crazyflie(self): print('Disconnecting from %s' % self.link_uri) # Stop the loggers self._log_config_1.stop() self._log_config_2.stop() self._log_config_3.stop() # Shutdown the rotors self._shutdown() # Disconnect self._cf.close_link() def _connected(self, link_uri): """This callback is called when the Crazyflie has been connected and the TOCs have been downloaded.""" print('Connected to %s' % link_uri) # Setup parameters self.disable_motion_tracking() # Add the logger self._cf.log.add_config(self._log_config_1) self._cf.log.add_config(self._log_config_2) self._cf.log.add_config(self._log_config_3) # This callback will receive the data self._log_config_1.data_received_cb.add_callback( self._log_config_1_data) self._log_config_2.data_received_cb.add_callback( self._log_config_2_data) self._log_config_3.data_received_cb.add_callback( self._log_config_3_data) # This callback will be called on errors self._log_config_1.error_cb.add_callback(self._log_config_error) self._log_config_2.error_cb.add_callback(self._log_config_error) self._log_config_3.error_cb.add_callback(self._log_config_error) # Start the logging self._log_config_1.start() self._log_config_2.start() self._log_config_3.start() # Set the connected event self._connect_event.set() self.is_connected = True self.status = DroneState.IDLE def _connection_failed(self, link_uri, msg): """Callback when the initial connection fails.""" print('Connection to %s failed: %s' % (link_uri, msg)) # Set the connected event self._connect_event.set() def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected.""" print('Disconnected from %s' % link_uri) self.is_connected = False self.status = DroneState.OFFLINE def _connection_lost(self, link_uri, msg): """Callback when the connection is lost after a connection has been made.""" print('Connection to %s lost: %s' % (link_uri, msg)) self.drone_lost.call(self) self._connect_event.set() self.is_connected = False self.status = DroneState.OFFLINE def _log_config_error(self, logconf, msg): """Callback from the log API when an error occurs.""" print('Error when logging %s: %s' % (logconf.name, msg)) def _log_config_1_data(self, timestamp, data, logconf): """Callback from the log API when data arrives.""" self.var_x = data['kalman.varPX'] self.var_y = data['kalman.varPY'] self.var_z = data['kalman.varPZ'] self.battery_voltage = data['pm.vbat'] def _log_config_2_data(self, timestamp, data, logconf): """Callback from the log API when data arrives.""" self.pos_x = data['kalman.stateX'] self.pos_y = data['kalman.stateY'] self.pos_z = data['kalman.stateZ'] def _log_config_3_data(self, timestamp, data, logconf): self.pitch = data['stabilizer.pitch'] self.roll = data['stabilizer.roll'] self.yaw = data['stabilizer.yaw'] def _unlock(self): # Unlock startup thrust protection (only needed for low lewel commands) self._cf.commander.send_setpoint(0, 0, 0, 0) def _shutdown(self): self._cf.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) def _keep_setpoint(self, roll, pitch, yawrate, thrust, keeptime): """Keeps the drone at the given setpoint for the given amount of time.""" while keeptime > 0: self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) keeptime -= 0.1 time.sleep(0.1) def _convert_velocity_to_time(self, distance: float, velocity: float) -> float: """Converts a distance and a velocity to a time.""" duration = distance / self._sanitize_velocity(velocity) return self._sanitize_duration(duration) def _calculate_distance(self, x: float, y: float, z: float, relative: bool = False) -> float: """Calculates the distance from the drone or the zero position (relative) to a given point in space.""" start_x = 0 if relative else self.pos_x start_y = 0 if relative else self.pos_y start_z = 0 if relative else self.pos_z return np.sqrt((x - start_x)**2 + (y - start_y)**2 + (z - start_z)**2) def _sanitize_velocity(self, velocity: float) -> float: return min(velocity, self._max_velocity) def _sanitize_duration(self, duration: float) -> float: return max(duration, self._min_duration) def _sanitize_yaw(self, yaw: float) -> float: return yaw % (2 * self._max_yaw_rotations * math.pi) def _sanitize_x(self, x: float, relative: bool) -> float: target_x = (self.pos_x + x) if relative else x sanitized_x = self._sanitize_number(target_x, self._arena.min_x, self._arena.max_x) return self._arena.transform_x(sanitized_x) def _sanitize_y(self, y: float, relative: bool) -> float: target_y = (self.pos_y + y) if relative else y sanitized_y = self._sanitize_number(target_y, self._arena.min_y, self._arena.max_y) return self._arena.transform_y(sanitized_y) def _sanitize_z(self, z: float, relative: bool) -> float: return self._sanitize_number(z, self._arena.min_z, self._arena.max_z) def _sanitize_number(self, value: float, min_value: float, max_value: float) -> float: return min(max(value, min_value), max_value)
class UAVController(): def __init__(self, targetURI=None): """ Function: __init__ Purpose: Initialize all necessary UAV functionality Inputs: none Outputs: none Description: An initializer function that finds a Crazyflie, can be particular target or not, and sets up all necessary data values for logging data values from the Crazyflie. """ #Enable the CrazyRadio PA drivers to communicate with the UAV. cflib.crtp.init_drivers() self.timeout = True self.available = [] self.UAV = Crazyflie() self.param = None self.airborne = False self._recentDataPacket = None self._receivingDataPacket = False #Attempt to locate UAV by scanning available interface for _ in range(0, 500): if (len(self.available) > 0): self.timeout = False break #If a UAV is found via scanning, break out of this loop else: self.available = cflib.crtp.scan_interfaces() #If there are Crazyflies available, connect to one of them if (len(self.available) > 0): #If there is a specific target, parse through the returned array to locate specific Crazyflie if (targetURI != None): for i in range(len(self.available)): if (self.available[i][0] == targetURI): #If found, open a link to the Crazyflie self.UAV.open_link(self.available[i][0]) self.connectedToTargetUAV = True else: self.connectedToTargetUAV = False else: #If there is not a specific target, simply connect to the first Crazyflie available self.UAV.open_link(self.available[0][0]) #While the Crazyflie is not connected, wait until the connection occurs to allow for Motion Commander initialization while (self.UAV.is_connected() == False): time.sleep(0.1) #Pass the connected Crazyflie to the Motion Commander class self.MC = MotionCommander(self.UAV) #Create desired logging parameters self.UAVLogConfig = LogConfig(name="UAVLog", period_in_ms=100) self.UAVLogConfig.add_variable('pm.vbat', 'float') self.UAVLogConfig.add_variable('stateEstimate.x', 'float') self.UAVLogConfig.add_variable('stateEstimate.y', 'float') self.UAVLogConfig.add_variable('stateEstimate.z', 'float') self.UAVLogConfig.add_variable('pm.chargeCurrent', 'float') #Add more variables here for logging as desired #Add the configured logger to the Crazyflie to begin grabbing data packets self.UAV.log.add_config(self.UAVLogConfig) if (self.UAVLogConfig.valid): self.UAVLogConfig.data_received_cb.add_callback( self._getUAVDataPacket) self.UAVLogConfig.start() else: logger.warning("Could not setup log configuration") #End of function def done(self): """ Function: done Purpose: Close connection to UAV to terminate all threads running Inputs: none Outputs: none Description: See purpose. """ self.UAVLogConfig.stop() self.UAV.close_link() self.airborne = False return def launch(self): """ Function: launch Purpose: Instruct the UAV to takeoff from current position to the default height Inputs: none Outputs: none Description: A wrapper function that calls the Crazyflie Motion Commander take_off function. See Bitcraze Crazyflie documentation for further details. """ self.airborne = True self.MC.take_off() #End of function return def land(self): """ Function: launch Purpose: Instruct the UAV to land on the ground at current position Inputs: none Outputs: none Description: A function that calls the Crazyflie Motion Commander Land function. See Bitcraze Crazyflie documentation for further details. """ self.airborne = False self.MC.land() return def move(self, distanceX, distanceY, distanceZ, velocity): """ Function: move Purpose: A wrapper function to instruct an UAV to move <x, y, z> distance from current point Inputs: distanceX - a floating point value that represents the distance to move the in the X-dimension, measured in meters. distanceY - a floating point value that represents the distance to move the in the Y-dimension, measured in meters. distanceZ - a floating point value that represents the distance to move the in the Z-dimension, measured in meters. velocity - a floating point value that represents the velocity of the UAV during movement, measured in meters per second Outputs: none Description: See purpose. """ if (self.airborne == False): self.launch() self.MC.move_distance(distanceX, distanceY, distanceZ, velocity) #End of function return def rotate(self, degree): """ Function: rotate Purpose: A wrapper function to instruct an UAV to rotate Inputs: degree - a floating point value in degrees Outputs: none Description: Currently this function is not in use as rotating the Crazyflie regularly introduces positional errors. In particular, the Crazyflie will rotate on a motor, not the center of the drone. """ if (self.airborne == False): self.launch() if (degree < 0): print("UC: rotate - Going Right") locDeg = 0 #self.MC.turn_right(abs(degree)) for _ in range(1, int(abs(degree) / 1)): locDeg += 1 self.MC.turn_right(1) self.MC.turn_right(abs(degree) - locDeg) else: print("UC: rotate - Going Left") self.MC.turn_left(abs(degree)) #Delay by 1 second, to allow for total rotation time time.sleep(1) return def getBatteryLevel(self): """ Function: getBatteryLevel Purpose: A function that reads the UAV battery level from an IOStream Inputs: none Outputs: retVal - a floating point value that represents the battery voltage of the UAV. Description: See purpose. """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["pm.vbat"] return retVal def getHeight(self): """ Function: getCurrentHeight Purpose: A function that reads the UAV height from a IOStream. Inputs: none Outputs: retVal - a floating point value that represents the onboard height detection of the UAV. Description: See purpose. """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["stateEstimate.z"] return retVal def isCharging(self): """ Function: getCurrentHeight Purpose: A function that reads the UAV charge current from a IOStream Inputs: none Outputs: retVal - a floating point value that represents the charge current of the UAV. Description: See purpose. """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["pm.chargeCurrent"] return retVal def _getUAVDataPacket(self, ident, data, logconfig): """ Function: getUAVDataPacket Purpose: A callback function to process a data packet received from the UAV Inputs: ident - identifier of the UAV data - data from the UAV logconfig - log configuration from the UAV Outputs: None Description: A user should NEVER call this function. """ self._receivingDataPacket = True self._recentDataPacket = data self._receivingDataPacket = False
while intensity < max_intensity: try: if (rangeLeft > obst_dist_thresh and rangeRight > obst_dist_thresh and rangeFront > obst_dist_thresh and rangeBack > obst_dist_thresh): print("intensity={}, last_intensity={}".format( intensity, last_intensity)) if intensity > last_intensity: run(mc) else: tumble(mc) else: avoid_obst(mc) except KeyboardInterrupt: break # end while loop mc.stop() mc.land() # Stop logging and end logconf_pos.stop() pos_file_handler.close() logconf_intensity.stop() intensity_file_handler.close() logconf_range.stop() range_file_handler.close() action_file_handler.close() sys.exit(0)
scf.cf.log.add_config(log_att_estimate) log_att_estimate.data_received_cb.add_callback( attitude_estimate_callback) set_initial_position(scf, initial_x, initial_y, initial_z, initial_yaw) reset_estimator(scf) log_pos_estimate.start() log_vel_estimate.start() log_att_estimate.start() run_sequence(scf, sequence, initial_x, initial_y, initial_z, initial_yaw) log_pos_estimate.stop() log_vel_estimate.stop() log_att_estimate.stop() fig, axs = plt.subplots(3, 3) axs[0, 0].plot(POSITION_X) axs[0, 0].plot(X_VAR) axs[0, 0].grid() axs[0, 0].legend(["est", "var"]) axs[0, 0].set_title("Position X") axs[1, 0].plot(POSITION_Y) axs[1, 0].plot(Y_VAR) axs[1, 0].grid() axs[1, 0].legend(["est", "var"]) axs[1, 0].set_title("Position Y") axs[2, 0].plot(POSITION_Z)
class DroneController: COLOR_BALL_TRACKED = (255, 0, 0) COLOR_BALL_UNTRACKED = (0, 0, 255) COLOR_LINE_TRACKED = (255, 0, 0) COLOR_LINE_UNTRACKED = (0, 0, 255) COLOR_TARGET_TRACKED = (0, 255, 0) COLOR_TARGET_UNTRACKED = (0, 0, 255) FIGURE_NAME = "Output" SETTINGS_ENVIRONMENT = "Very bright bedroom" CAMERA_SETTINGS_FILE = "config/cam_settings/Camera settings - USB 2.0 Camera - {}.txt".format(SETTINGS_ENVIRONMENT) COLOR_THRESH_SETTINGS_FILE = "config/color_thresh/Color threshold settings - {}.txt".format(SETTINGS_ENVIRONMENT) BLOB_DETECTOR_SETTINGS_FILE = "config/blob_detector/Blob detector settings.txt" SETTINGS_SEPARATOR = UvcCapture.SETTINGS_SEPARATOR # We save files in a csv type of way ASK_FOR_TARGET_YAW = False TAKEOFF_THRUST = 44000 def __init__(self): self.t_start = self.t_frame = self.t_last_frame = datetime.now() self.t_events = [] self.EXPERIMENT_START_DATETIME = str(self.t_start)[:-7].replace(':', '-') self.VIDEO_FOLDER = "img-ns/{}".format(self.EXPERIMENT_START_DATETIME) self.experiment_log = plot_tools.ExperimentLog(self.EXPERIMENT_START_DATETIME, {"Roll": "piv", "Pitch": "piv", "Yaw": "pid", "Thrust": "piv", "Estimated_Z": "log", "Velocity_Z": "log"}) self.window_for_kb_input = None self.video_capture = None self.cv_HSV_thresh_min = np.array([ 0, 0, 0], dtype=np.uint8) self.cv_HSV_thresh_max = np.array([255, 255, 255], dtype=np.uint8) self.cv_blob_detect_params = None self.cv_cam_frame = None self.cv_filtered_HSV_mask = None self.cv_frame_out = None self.crazyflie = None self.cf_log = None self.cf_radio_connecting = True self.cf_radio_connected = False self.cf_ignore_camera = False self.cf_pos_tracked = False self.cf_taking_off = True self.cf_str_status = "TAKING OFF" self.cf_roll = self.cf_pitch = self.cf_yaw = self.cf_estimated_z = self.cf_vel_z = 0 self.cf_curr_pos = np.array([0, 0, 0]) self.cf_PID_roll = PID.PIDposAndVel(posP=0.5, velP=0.05, velI=0.01, vel_offs=0, pos_out_max=300, vel_out_max=30, vel_invert_error=True) self.cf_PID_pitch = PID.PIDposAndVel(posP=0.7, velP=0.3, velI=0.002, vel_offs=0, pos_out_max=30, vel_out_max=30) self.cf_PID_yaw = PID.PID(P=0.5, I=0.3, D=0, offs=0, out_max=20, invert_error=True, error_in_degrees=True) self.cf_PID_thrust = PID.PIDposAndVel(posP=1, velP=35, velI=25, vel_offs=43000, pos_out_max=300, vel_out_max=7000, pos_invert_error=True, vel_invert_input=True) def init_video_cam_and_cv_algorithm(self, create_video_folder=True): """ Initializes camera: connects to it, loads settings from config file (if available), loads color threshold and blob detector settings (if available), creates a folder to save cv output images... :param create_video_folder: True to create folder specified by VIDEO_FOLDER (where output frames will be saved) """ self.video_capture = UvcCapture.new_from_settings(self.CAMERA_SETTINGS_FILE) # Connect to device specified by settings, and load its desired param values if self.video_capture is None: # If unable to connect to device specified by settings, open first available camera self.video_capture = UvcCapture(0) if self.video_capture is None: # If still unable to connect, raise an exception raise Exception("Couldn't open camera! :(") # If we're here, we couldn't connect to device specified by settings but were able to open 1st available cam if not self.video_capture.load_settings(self.CAMERA_SETTINGS_FILE): # Try to load frame size & rate from settings self.video_capture.select_best_frame_mode(60) # If loading settings failed, choose best frame size with fps >= 60 logging.info("Camera opened! :)") # Initialize cv algorithm too: load settings for color thresholding and blob detector self.load_color_thresh_settings() self.load_blob_detector_settings() # Sometimes, first couple frames take a long time to be obtained, do it before quad goes in flying mode self.video_capture.get_frame_robust() self.video_capture.get_frame_robust() # Initialize PID setpoints and initial input value to the center of the frame self.cf_PID_roll.setSetPoint(self.video_capture.frame_size[0] / 2) self.cf_PID_thrust.setSetPoint(self.video_capture.frame_size[1] / 2) self.cf_PID_pitch.setSetPoint(40) self.cf_PID_roll.PIDpos.curr_input = self.cf_PID_roll.getSetPoint() self.cf_PID_thrust.PIDpos.curr_input = self.cf_PID_thrust.getSetPoint() self.cf_PID_pitch.PIDpos.curr_input = self.cf_PID_pitch.getSetPoint() # Prepare the folder self.VIDEO_FOLDER so we can store each frame we processed (for debugging) if create_video_folder: shutil.rmtree(self.VIDEO_FOLDER, ignore_errors=True) # Delete the folder and its contents, if it exists (ignore errors if it doesn't) os.makedirs(self.VIDEO_FOLDER) # Now create the folder, which won't throw any exceptions as we made sure it didn't already exist def init_UI_window(self): """ Initializes the appropriate user interface depending on experiment's purpose (see is_input_for_drone_commands) :param is_input_for_drone_commands: True if we have communication with the CF and need input to send it commands False if we're just debugging the computer vision side (camera settings, color threshold, etc.) and need input to debug pixel information """ # Open an SDL2 window to track keyboard keydown events and use it to send commands to the CF sdl2.ext.init() self.window_for_kb_input = sdl2.ext.Window("Window to receive keyboard input", size=(400, 300)) self.window_for_kb_input.show() def save_color_thresh_settings(self, HSV_min, HSV_max, file_name=COLOR_THRESH_SETTINGS_FILE, sep=SETTINGS_SEPARATOR): """ Saves specified color threshold settings to a file. :param HSV_min: np.array (1x3) containing minimum H, S and V values for the color thresholding CF detection :param HSV_max: np.array (1x3) containing maximum H, S and V values for the color thresholding CF detection :param file_name: Name of the file to use when saving the settings :param sep: String that will be used to separate parameters (in a csv fashion). Eg: '\t', ',', etc. :return: True if everything went well; False if settings couldn't be saved """ try: logging.debug("Saving current color threshold settings to file '{}'".format(file_name)) with open(file_name, 'w') as f: # Open file for output f.write("{}\n".format(sep.join(HSV_min.astype(str)))) # Store HSV_min f.write("{}\n".format(sep.join(HSV_max.astype(str)))) # Store HSV_max except: logging.exception("Something went wrong while saving current color threshold settings to '{}'.".format(file_name)) return False return True def load_color_thresh_settings(self, file_name=COLOR_THRESH_SETTINGS_FILE, sep=SETTINGS_SEPARATOR): """ Loads color threshold settings from a file. :param file_name: Path to the file to load the settings from :param sep: String that was used to separate parameters (in a csv fashion). Eg: '\t', ',', etc. :return: True if everything went well; False if settings couldn't be loaded """ try: logging.debug("Loading color threshold settings from file '{}'".format(file_name)) with open(file_name, 'r') as f: # Open file for input self.cv_HSV_thresh_min = np.array(f.readline().rstrip('\r\n').split(sep), dtype=np.uint8) self.cv_HSV_thresh_max = np.array(f.readline().rstrip('\r\n').split(sep), dtype=np.uint8) logging.debug("\tLoaded color threshold settings: HSV_min={}; HSV_max={}".format(self.cv_HSV_thresh_min, self.cv_HSV_thresh_max)) except: logging.exception("Something went wrong while loading color threshold settings from '{}'.".format(file_name)) return False return True def save_blob_detector_settings(self, detector_params, file_name=BLOB_DETECTOR_SETTINGS_FILE, sep=SETTINGS_SEPARATOR): """ Saves specified blob detector settings to a file. :param detector_params: cv2.SimpleBlobDetector_Params object containing the params which want to be saved :param file_name: Name of the file to use when saving the settings :param sep: String that will be used to separate parameters (in a csv fashion). Eg: '\t', ',', etc. :return: True if everything went well; False if settings couldn't be saved """ try: logging.debug("Saving current blob detector settings to file '{}'".format(file_name)) with open(file_name, 'w') as f: # Open file for output for m in dir(detector_params): # Traverse all methods and properties of detector_params if m[0] != '_': # For every property that's not "built-in" (ie: doesn't start by '_') f.write("{}{}{}\n".format(m, sep, getattr(detector_params, m))) # Store the property name and value except: logging.exception("Something went wrong while saving current blob detector settings to '{}'.".format(file_name)) return False return True def load_blob_detector_settings(self, file_name=BLOB_DETECTOR_SETTINGS_FILE, sep=SETTINGS_SEPARATOR): """ Loads blob detector settings from a file. Leave file_name empty to only load default params. cv2 will use these params to detect the drone from a binary image mask (the output of the color thresholding step). :param file_name: Path to the file to load the settings from :param sep: String that was used to separate parameters (in a csv fashion). Eg: '\t', ',', etc. :return: True if everything went well; False if settings couldn't be loaded """ detector_params = cv2.SimpleBlobDetector_Params() self.cv_blob_detect_params = detector_params # Filter by color detector_params.filterByColor = False detector_params.blobColor = 255 # Change thresholds detector_params.minThreshold = 254 detector_params.maxThreshold = 255 # Filter by Area. detector_params.filterByArea = True detector_params.minArea = 30 detector_params.maxArea = 40000 # Filter by Circularity detector_params.filterByCircularity = False detector_params.minCircularity = 0.7 detector_params.maxCircularity = 1.0 # Filter by Convexity detector_params.filterByConvexity = False detector_params.minConvexity = 0.7 detector_params.maxConvexity = 1.0 # Filter by Inertia detector_params.filterByInertia = False detector_params.minInertiaRatio = 0.5 detector_params.maxInertiaRatio = 1.0 detector_params.minRepeatability = 1 detector_params.minDistBetweenBlobs = 3000 try: logging.debug("Loading blob detector settings from file '{}'".format(file_name)) with open(file_name, 'r') as f: # Open file for input for line in f: # Every line contains one property: name + sep + value name, value = line.split(sep) setattr(detector_params, name, eval(value)) # Use eval to cast to right type (eg: False instead of "False") logging.debug("\tLoaded blob detector setting: '{}' = {}".format(name, value)) except: logging.exception("Something went wrong while loading blob detector settings from '{}'.".format(file_name)) return False return True def connect_to_cf(self, retry_after=10, max_timeout=20): """ Initializes radio drivers, looks for available CrazyRadios, and attempts to connect to the first available one. Doesn't return anything, but raises exceptions if anything goes wrong. :param retry_after: Time in seconds after which we should abort current connection and restart the attempt. :param max_timeout: Time in seconds after which we should give up if we haven't been able to establish comm. yet """ logging.info("Initializing drivers.") crtp.init_drivers(enable_debug_driver=False) logging.info("Setting up the communication link. Looking for available CrazyRadios in range.") available_links = crtp.scan_interfaces() if len(available_links) == 0: raise Exception("Error, no Crazyflies found. Exiting.") else: logging.info("CrazyFlies found:") # For debugging purposes, show info about available links for i in available_links: logging.info("\t" + i[0]) link_uri = available_links[0][0] # Choose first available link logging.info("Initializing CrazyFlie (connecting to first available interface: '{}').".format(link_uri)) self.crazyflie = Crazyflie(ro_cache="cachero", rw_cache="cacherw") # Create an instance of Crazyflie self.crazyflie.connected.add_callback(self.on_cf_radio_connected) # Set up callback functions for communication feedback self.crazyflie.disconnected.add_callback(self.on_cf_radio_disconnected) self.crazyflie.connection_failed.add_callback(self.on_cf_radio_conn_failed) self.crazyflie.connection_lost.add_callback(self.on_cf_radio_conn_lost) cnt = 0 # Initialize a time counter while self.cf_radio_connecting and cnt < max_timeout: if cnt % retry_after == 0: if cnt > 0: # Only show warning after first failed attempt logging.warning("Unable to establish communication with Crazyflie ({}) after {}s. Retrying...".format(link_uri, retry_after)) self.crazyflie.close_link() # Closing the link will call on_disconnect, which will set cf_radio_connecting to False self.cf_radio_connecting = True # Reset cf_radio_connecting back to True self.crazyflie.open_link(link_uri) # Connect/Reconnect to the CrazyRadio through the selected interface time.sleep(1) # Sleep for a second (give time for callback functions to detect whether we are connected) cnt += 1 # Increase the "waiting seconds" counter if cnt >= max_timeout: self.crazyflie.close_link() raise Exception("Unable to establish communication with CrazyFlie after {}s. Given up :(".format(max_timeout)) elif not self.cf_radio_connected: raise Exception("Something failed while attempting to connect to the CrazyFlie, exiting.") # self.crazyflie.commander.send_setpoint(0, 0, 0, 0) # If we successfully connected to the CF, send thrust=0 (new firmware initializes thrustLock=True, only way to unlock it so it executes commands is by setting thrust=0) def setup_cf(self): """ Sets up the CrazyFlie before running the experiment. This includes configuring some params to default values and requesting the CrazyFlie to log certain variables back at constant intervals. Doesn't return anything, but raises exceptions if anything goes wrong. """ try: # Send some default values for CF params self.crazyflie.param.set_value('controller.tiltComp', '{:d}'.format(True)) self.crazyflie.param.set_value('flightmode.poshold', '{:d}'.format(False)) # Disable poshold and althold by default self.crazyflie.param.set_value('flightmode.althold', '{:d}'.format(False)) self.crazyflie.param.set_value('flightmode.posSet', '{:d}'.format(False)) self.crazyflie.param.set_value('flightmode.yawMode', '0') self.crazyflie.param.set_value('flightmode.timeoutStab', '{:d}'.format(1000*60*10)) # Stabilize (rpy=0) CF if doesn't receive a radio command in 10min self.crazyflie.param.set_value('flightmode.timeoutShut', '{:d}'.format(1000*60*20)) # Shutdown CF if doesn't receive a radio command in 20min self.crazyflie.param.set_value('posCtlPid.thrustBase', '{}'.format(self.TAKEOFF_THRUST)) self.crazyflie.param.set_value("ring.effect", "1") # Turn off LED ring self.crazyflie.param.set_value("ring.headlightEnable", "0") # Turn off LED headlight except Exception as e: raise Exception("Couldn't initialize CrazyFlie params to their desired values. Details: {}".format(e.message)) # Create a log configuration and include all variables that want to be logged self.cf_log = LogConfig(name="cf_log", period_in_ms=10) self.cf_log.add_variable("stabilizer.roll", "float") self.cf_log.add_variable("stabilizer.pitch", "float") self.cf_log.add_variable("stabilizer.yaw", "float") self.cf_log.add_variable("posEstimatorAlt.estimatedZ", "float") self.cf_log.add_variable("posEstimatorAlt.velocityZ", "float") try: self.crazyflie.log.add_config(self.cf_log) # Validate the log configuration and attach it to our CF except Exception as e: raise Exception("Couldn't attach the log config to the CrazyFlie, bad configuration. Details: {}".format(e.message)) self.cf_log.data_received_cb.add_callback(self.on_cf_log_new_data) # Register appropriate callbacks self.cf_log.error_cb.add_callback(self.on_cf_log_error) self.cf_log.start() # Start logging! # Get the CF's initial yaw (should be facing the camera) so we can have PID_yaw maintain that orientation if self.ASK_FOR_TARGET_YAW: # Either ask the user to press Enter to indicate the CF's orientation is ready raw_input("\nRotate the drone so it faces the camera, press Enter when you're ready...\n") else: # Or automatically detect the first yaw log packet and set the current orientation as the desired yaw while abs(self.cf_yaw) < 0.01: # Wait until first cf_yaw value is received (cf_yaw=0 by default) time.sleep(0.1) self.cf_PID_yaw.SetPoint = self.cf_yaw print "Target yaw set at {:.2f}.".format(self.cf_yaw) self.crazyflie.add_port_callback(CRTPPort.CONSOLE, self.print_cf_console) self.crazyflie.commander.send_setpoint(0, 0, 0, 0) # New firmware version requires to send thrust=0 at least once to "unlock thrust" def fly_cf(self): """ Provides the structure to make the drone fly (actual flight control is done in hover() though). hover() is called until user stops the experiment, then hover is called for a few more secs to record more data (usually includes information about the CF crashing...) :return: Whether or not to keep (store) the logs, based on whether the CF ever actually took off and flew """ print "Giving you 5s to prepare for take-off..." time.sleep(5) # t = Timer(20, self.m_CrazyFlie.close_link) # Start a timer to automatically disconnect in 20s # t.start() # Prepare for take off: clear PIDs, log start time... self.cf_str_status = "TAKING OFF" self.t_start = datetime.now() self.cf_PID_roll.clear() self.cf_PID_pitch.clear() self.cf_PID_yaw.clear() self.cf_PID_thrust.clear() # Alright, let's fly! tStop = None while tStop is None: # tStop will remain None while everything's fine tStop = self.hover() # hover returns None while everything's fine; the time to end the experiment otherwise # If we get here, either the user stopped the experiment or the code detected something went wrong print "AT t={}, A KEY WAS PRESSED -> STOPPING!".format(datetime.now().strftime("%H:%M:%S.%f")[:-3]) save_logs = (self.cf_str_status != "TAKING OFF") # Only store the logs if the drone ever started flying (not just took off) self.cf_str_status = "STOPPED" # Updated new drone status while datetime.now() < tStop: # Keep calling hover until tStop, so data is still logged self.hover() return save_logs # Return whether or not to keep the logs def cleanup_experiment(self, save_logs=True): """ "Cleans up" the experiment: closes any open windows, saves logs, disconnects camera and drone, etc. """ # If we get here, either the user ended the experiment, or an exception occurred. Same action regardless: if self.cf_log is not None: # If we were logging data from the CF, stop it (will reconnect faster next time) self.cf_log.stop() self.cf_log.delete() if self.crazyflie is not None: # If the CF was ever initialized, make sure the communication link is closed self.crazyflie.close_link() self.video_capture = None # Destroy the video capture object (this takes care of closing the camera etc.) cv2.destroyAllWindows() # Close all UI windows that could be open if self.window_for_kb_input is not None: self.window_for_kb_input.hide() sdl2.ext.quit() if save_logs: # If the experiment didn't crash before starting (the CF ever took off), plot and save the logs self.experiment_log.plot(False) self.experiment_log.save() else: # Otherwise just delete the folder (and its contents) where cam frames would have been/were saved shutil.rmtree(self.VIDEO_FOLDER, ignore_errors=False) def run_experiment(self): """ DroneController's main method: initializes all components (vision, communication, drone params, etc.) then runs the experiment. """ logging.disable(logging.DEBUG) # Seems to work better than .basicConfig(INFO), especially if logging has already been initialized -> Only show messages of level INFO or higher save_logs = True # Use this auxiliary variable to prevent saving logs if the drone never took off try: self.connect_to_cf() # Connect to the CrazyFlie self.setup_cf() # Can't initialize LogConfig until we're connected, because it needs to check that the variables we'd like to add are in the TOC. So this function must be called after connect_to_cf() self.init_video_cam_and_cv_algorithm() # Connect to the first available camera, load default settings, etc. self.init_UI_window() # Open a window to receive user input to control the CF save_logs = self.fly_cf() # And finally fly the CF except: logging.exception("Shutting down due to an exception =( See details below:") # If we got here, either the user ended the experiment, or an exception occurred. Same action regardless: self.cleanup_experiment(save_logs) # "Cleanup" the experiment: close windows, save logs, etc. def get_cf_target_pos(self): """ :return: np.array containing the current (estimated) 3D position of the CrazyFlie """ return np.array([int(round(x)) for x in [self.cf_PID_roll.getSetPoint(), self.cf_PID_thrust.getSetPoint(), self.cf_PID_pitch.getSetPoint()]]) def get_cf_curr_pos(self): """ :return: np.array containing the current (estimated) 3D position of the CrazyFlie """ return np.array([self.cf_PID_roll.getCurrPos(), self.cf_PID_thrust.getCurrPos(), self.cf_PID_pitch.getCurrPos()]) def get_cf_curr_vel(self): """ :return: np.array containing the current (estimated) 3D velocity of the CrazyFlie """ return np.array([self.cf_PID_roll.getCurrVel(), self.cf_PID_thrust.getCurrVel(), self.cf_PID_pitch.getCurrVel()]) def on_cf_radio_connected(self, link_uri): logging.info("Successfully connected to Crazyflie at '{}'!".format(link_uri)) self.cf_radio_connecting = False self.cf_radio_connected = True def on_cf_radio_conn_failed(self, link_uri, msg): logging.error("Connection to '{}' failed: {}.".format(link_uri, msg)) # Initial connection fails (i.e no Crazyflie at the speficied address) self.cf_radio_connecting = False self.cf_radio_connected = False def on_cf_radio_conn_lost(self, link_uri, msg): logging.warning("Connection to '{}' lost: {}.".format(link_uri, msg)) # Disconnected after a connection has been made (i.e Crazyflie moves out of range) def on_cf_radio_disconnected(self, link_uri): logging.error("Disconnected from '{}'.".format(link_uri)) # Crazyflie is disconnected (called in all cases) self.cf_radio_connecting = False self.cf_radio_connected = False def on_cf_log_error(self, logconf, msg): logging.error("Error when logging %s: %s." % (logconf.name, msg)) def on_cf_log_new_data(self, timestamp, data, logconf): logging.debug("[%d][%s]: %s" % (timestamp, logconf.name, data)) self.cf_roll = data['stabilizer.roll'] self.cf_pitch = data['stabilizer.pitch'] self.cf_yaw = data['stabilizer.yaw'] self.cf_estimated_z = data['posEstimatorAlt.estimatedZ'] self.cf_vel_z = data['posEstimatorAlt.velocityZ'] print "\rCurrent yaw: {:.2f}deg".format(self.cf_yaw), def print_cf_console(self, packet): console_text = packet.data.decode('UTF-8') print("Console: {}".format(console_text)) def send_cf_param(self, complete_name, value): """ Modified version of crazyflie.param.set_value that sends the packet immediately (instead of using a Thread+Queue """ element = self.crazyflie.param.toc.get_element_by_complete_name(complete_name) if not element: raise KeyError("Couldn't set {}={}, param is not in the TOC!".format(complete_name, value)) elif element.access == ParamTocElement.RO_ACCESS: raise AttributeError("Couldn't set {}={}, param is read-only!".format(complete_name, value)) else: varid = element.ident pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, WRITE_CHANNEL) pk.data = struct.pack('<B', varid) pk.data += struct.pack(element.pytype, eval(value)) self.crazyflie.send_packet(pk, expected_reply=(tuple(pk.data[0:2]))) def process_kb_input(self): """ Processes all keydown events, and takes the corresponding action depending on which key was pressed. :return: Boolean indicating whether the experiment shall go on: True while everything's fine, False to stop it """ events = sdl2.ext.get_events() # Fetch any new input event for event in events: # Iterate through all of them if event.type == sdl2.SDL_KEYDOWN: # And only process keydown events key_orig = event.key.keysym.sym # Fetch the key that was pressed down try: key = chr(key_orig) # Try to convert the key to char except: # Usually, this exeption occurs when a key can't be converted to char (arrows, Esc, etc.) if key_orig == sdl2.SDLK_UP: key = "Up" elif key_orig == sdl2.SDLK_DOWN: key = "Down" elif key_orig == sdl2.SDLK_ESCAPE: key = "Esc" else: key = hex(key_orig) logging.info("Key: '{}'".format(key)) self.window_for_kb_input.title = "Last key pressed: '{}' at t={}".format(key, str(datetime.now().time())[:-3]) key = key.lower() # Convert to lowercase so we don't have to worry about different cases if key == 'a': # Move left self.cf_PID_roll.setSetPoint(self.cf_PID_roll.getSetPoint() + 20) elif key == 's': # Move back self.cf_PID_pitch.setSetPoint(max(self.cf_PID_pitch.getSetPoint() - 2, 1)) # Size (ball radius) can't be negative! Make sure depth value is at least 1px elif key == 'd': # Move right self.cf_PID_roll.setSetPoint(self.cf_PID_roll.getSetPoint() - 20) elif key == 'w': # Move forward self.cf_PID_pitch.setSetPoint(self.cf_PID_pitch.getSetPoint() + 2) elif key == 'u': # Move up self.cf_PID_thrust.setSetPoint(self.cf_PID_thrust.getSetPoint() - 20) elif key == 'h': # Move down self.cf_PID_thrust.setSetPoint(self.cf_PID_thrust.getSetPoint() + 20) elif key == 'f': # Toggle altitude hold mode if self.cf_ignore_camera: self.cf_PID_thrust.clear() # If we were ignoring the camera for Z, thrust PID will have a wrong I component self.cf_ignore_camera = not self.cf_ignore_camera self.cf_str_status = "NO CAM for Z" if self.cf_ignore_camera else "FULL CAM" # self.crazyflie.param.set_value('flightmode.althold', '{:d}'.format(self.cf_ignore_camera)) # while not self.crazyflie.param.param_updater.request_queue.empty(): # Wait for the packet to be sent # time.sleep(0.01) self.send_cf_param('flightmode.althold', '{:d}'.format(self.cf_ignore_camera)) elif key == 'e': # Stop taking off and start flying (hover at current position) self.cf_taking_off = False self.cf_str_status = "FLYING" self.cf_PID_roll.setSetPoint(self.cf_PID_roll.getCurrPos()) self.cf_PID_pitch.setSetPoint(self.cf_PID_pitch.getCurrPos() + 2) self.cf_PID_thrust.setSetPoint(self.cf_PID_thrust.getCurrPos()) self.cf_PID_roll.clear() self.cf_PID_pitch.clear() self.cf_PID_thrust.clear() else: # Any other key ends the experiment # self.crazyflie.param.set_value('flightmode.althold', '{:d}'.format(False)) # Make sure we're not on althold mode, so sending a thrust 0 will kill the motors and not just descend self.send_cf_param('flightmode.althold', '{:d}'.format(False)) # Make sure we're not on althold mode, so sending a thrust 0 will kill the motors and not just descend return False return True def detect_cf_in_camera(self, frame=None, find_blob=True): """ Runs an iteration of the computer vision algorithm that estimates the CrazyFlie's position in 3D. That is, it captures a frame from the camera, converts it to HSV, filters by color, and detects a blob. Saves the 3D position in cf_curr_pos, camera frame in cv_cam_frame and color filter mask in cv_filtered_HSV_mask. """ ######################################################### # CAPTURE FRAME # ######################################################### self.t_events = [datetime.now()] self.cv_cam_frame = frame # Allow the function to receive a frame if we already got it from the camera if self.cv_cam_frame is None: # Otherwise, if frame is None (default), grab a frame from the camera try: uvc_frame = self.video_capture.get_frame_robust() # read() blocks execution until a new frame arrives! -> Obtain t AFTER grabbing the frame self.t_events.append(datetime.now()) self.t_frame = self.t_events[-1] self.cv_cam_frame = uvc_frame.bgr # .copy() self.t_events.append(datetime.now()) except Exception as e: raise Exception("Unexpected error accessing the camera frame :( Details: {}.".format(e.message)) ######################################################### # COLOR THRESHOLD # ######################################################### # self.cv_cam_frame = cv2.resize(self.cv_cam_frame, None, fx=0.5, fy=0.5) # self.t_events.append(datetime.now()) # self.cv_cam_frame = cv2.GaussianBlur(self.cv_cam_frame, (3, 3), 0) # Not needed, camera is already physically "blurring" (sharpness parameter set to 0) # self.t_events.append(datetime.now()) frame_HSV = cv2.cvtColor(self.cv_cam_frame, cv2.COLOR_BGR2HSV) self.t_events.append(datetime.now()) self.cv_filtered_HSV_mask = cv2.inRange(frame_HSV, self.cv_HSV_thresh_min, self.cv_HSV_thresh_max) self.t_events.append(datetime.now()) self.cv_filtered_HSV_mask = cv2.morphologyEx(self.cv_filtered_HSV_mask, cv2.MORPH_OPEN, cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))) self.t_events.append(datetime.now()) ######################################################### # DRONE DETECTION # ######################################################### if find_blob: keypoints = cv2.SimpleBlobDetector_create(self.cv_blob_detect_params).detect(self.cv_filtered_HSV_mask) self.t_events.append(datetime.now()) self.cf_pos_tracked = bool(keypoints) # For now we determine the CF's position is tracked if we find at least 1 blob if keypoints: # If the cv algorithm detected at least one blob keypoint = max(keypoints, key=attrgetter('size')) # Focus on the biggest blob self.cf_curr_pos = np.hstack((keypoint.pt, keypoint.size/2)) # And save the position estimated by the CV algorithm def control_cf(self): """ Sends messages to control the CrazyFlie (roll-pitch-yaw-thrust setpoints) using the current location and PID loops. :param drone_curr_pos: 3D np.array containing the current (estimated) position of the drone """ if not self.cf_pos_tracked: # If cv algorithm wasn't able to detect the drone, linearly estimate its position based on previous position and speed # Since PIDs haven't been updated with current values yet, don't have to multiply velocity by (curr_time-last_time) but rather by (t_frame-curr_time) self.cf_curr_pos = \ np.array([self.cf_PID_roll.getCurrPos(), self.cf_PID_thrust.getCurrPos(), self.cf_PID_pitch.getCurrPos()]) + \ np.array([self.cf_PID_roll.getCurrVel(), -self.cf_PID_thrust.getCurrVel(), self.cf_PID_pitch.getCurrVel()]) * \ (self.t_frame - self.cf_PID_thrust.PIDvel.curr_time).total_seconds() self.cf_curr_pos[2] = max(1, self.cf_curr_pos[2]) # Make sure 3D (size) stays positive! (Prevents errors further down the line) # Update PID loops with new position at t=self.t_frame self.cf_PID_roll.update(self.cf_curr_pos[0], self.t_frame) self.cf_PID_pitch.update(self.cf_curr_pos[2], self.t_frame) self.cf_PID_yaw.update(self.cf_yaw, self.t_frame) self.cf_PID_thrust.update(self.cf_curr_pos[1], self.t_frame) # Log all relevant variables after each iteration self.experiment_log.update(roll=self.cf_PID_roll, pitch=self.cf_PID_pitch, yaw=self.cf_PID_yaw, thrust=self.cf_PID_thrust, estimated_z=self.cf_estimated_z, velocity_z=self.cf_vel_z) # Send the appropriate roll-pitch-yaw-thrust setpoint depending on the scenario (eg: taking off, stopping, etc.) if self.cf_radio_connected: # While the experiment is running if self.cf_taking_off: # If taking off, send a constant RPYT setpoint that will make the CF go up "straight" # self.crazyflie.commander.send_setpoint(self.cf_PID_roll.PIDvel.out_offs, self.cf_PID_pitch.PIDvel.out_offs, self.cf_PID_yaw.output, self.TAKEOFF_THRUST) self.crazyflie.commander.send_setpoint(self.cf_PID_roll.PIDvel.out_offs, self.cf_PID_pitch.PIDvel.out_offs, 0, self.TAKEOFF_THRUST) else: # This condition holds ever after take off (once the user presses the key to start "flying") if self.cf_ignore_camera: # If user selected it, control the drone in althold mode (altitude is done on-board, rest is still controlled with the cam) # self.crazyflie.commander.send_setpoint(self.cf_PID_roll.getOutput(), self.cf_PID_pitch.getOutput(), self.cf_PID_yaw.output, 32767) self.crazyflie.commander.send_setpoint(self.cf_PID_roll.getOutput(), self.cf_PID_pitch.getOutput(), 0, 32767) elif not self.cf_pos_tracked: # If we couldn't find the drone, don't send any commands (not to mislead it) pass else: # Otherwise, just use the camera to control all 4 independent axes # self.crazyflie.commander.send_setpoint(self.cf_PID_roll.getOutput(), self.cf_PID_pitch.getOutput(), self.cf_PID_yaw.output, self.cf_PID_thrust.getOutput()) self.crazyflie.commander.send_setpoint(self.cf_PID_roll.getOutput(), self.cf_PID_pitch.getOutput(), 0, self.cf_PID_thrust.getOutput()) else: # If the user has decided to end the experiment, kill the motors and reset PIDs (this is not really necessary) self.crazyflie.commander.send_setpoint(0, 0, 0, 0) self.cf_PID_roll.clear() self.cf_PID_pitch.clear() self.cf_PID_yaw.clear() self.cf_PID_thrust.clear() self.t_events.append(datetime.now()) def get_OSD_text(self, t): """ Generates written debug information (OSD=on-screen display) to be displayed at the bottom of the current frame :param t: Datetime at which camera frame was obtained (through datetime.now()) :return: String containing relevant debug information (PID values, drone estimated position, etc.) """ formatNum = "{:+6.2f}" strPrint = ("ROLLp. ={:+3.0f};" + formatNum + " [" + formatNum + "," + formatNum + "," + formatNum + "]\t\t" + "ROLLv. ={:+3.0f};" + formatNum + " [" + formatNum + "," + formatNum + "," + formatNum + "]\t\t" + "PITCHp={:+3.0f};" + formatNum + " [" + formatNum + "," + formatNum + "," + formatNum + "]\t\t" + "PITCHv={:+3.0f};" + formatNum + " [" + formatNum + "," + formatNum + "," + formatNum + "]\t\t" + "YAW.. ={:+3.0f};" + formatNum + " [" + formatNum + "," + formatNum + "," + formatNum + "]\t\t" + "THRUSp={:3.0f};{:6.0f} [{:+6.0f}, {:+6.0f}, {:+6.0f}]\t\t" + "THRUSv={:3.0f};{:6.0f} [{:+6.0f}, {:+6.0f}, {:+6.0f}]\t\t" + "[x:{:4.0f}, y:{:4.0f}, z:{:4.0f}], [vx:{:4.0f}, vy:{:4.0f}, vz:{:4.0f}], " + "rpy: " + formatNum + "," + formatNum + "," + formatNum + "]\t\t" + "@{} (FPS: {:5.2f}) - " + self.cf_str_status).format( self.cf_PID_roll.PIDpos.SetPoint, self.cf_PID_roll.PIDpos.output, self.cf_PID_roll.PIDpos.PTerm, self.cf_PID_roll.PIDpos.Ki * self.cf_PID_roll.PIDpos.ITerm, self.cf_PID_roll.PIDpos.Kd * self.cf_PID_roll.PIDpos.DTerm, self.cf_PID_roll.PIDvel.SetPoint, self.cf_PID_roll.PIDvel.output, self.cf_PID_roll.PIDvel.PTerm, self.cf_PID_roll.PIDvel.Ki * self.cf_PID_roll.PIDvel.ITerm, self.cf_PID_roll.PIDvel.Kd * self.cf_PID_roll.PIDvel.DTerm, self.cf_PID_pitch.PIDpos.SetPoint, self.cf_PID_pitch.PIDpos.output, self.cf_PID_pitch.PIDpos.PTerm, self.cf_PID_pitch.PIDpos.Ki * self.cf_PID_pitch.PIDpos.ITerm, self.cf_PID_pitch.PIDpos.Kd * self.cf_PID_pitch.PIDpos.DTerm, self.cf_PID_pitch.PIDvel.SetPoint, self.cf_PID_pitch.PIDvel.output, self.cf_PID_pitch.PIDvel.PTerm, self.cf_PID_pitch.PIDvel.Ki * self.cf_PID_pitch.PIDvel.ITerm, self.cf_PID_pitch.PIDvel.Kd * self.cf_PID_pitch.PIDvel.DTerm, self.cf_PID_yaw.SetPoint, self.cf_PID_yaw.output, self.cf_PID_yaw.PTerm, self.cf_PID_yaw.Ki * self.cf_PID_yaw.ITerm, self.cf_PID_yaw.Kd * self.cf_PID_yaw.DTerm, self.cf_PID_thrust.PIDpos.SetPoint, self.cf_PID_thrust.PIDpos.output, self.cf_PID_thrust.PIDpos.PTerm, self.cf_PID_thrust.PIDpos.Ki * self.cf_PID_thrust.PIDpos.ITerm, self.cf_PID_thrust.PIDpos.Kd * self.cf_PID_thrust.PIDpos.DTerm, self.cf_PID_thrust.PIDvel.SetPoint, self.cf_PID_thrust.PIDvel.output, self.cf_PID_thrust.PIDvel.PTerm, self.cf_PID_thrust.PIDvel.Ki * self.cf_PID_thrust.PIDvel.ITerm, self.cf_PID_thrust.PIDvel.Kd * self.cf_PID_thrust.PIDvel.DTerm, self.cf_PID_roll.getCurrPos(), self.cf_PID_thrust.getCurrPos(), self.cf_PID_pitch.getCurrPos(), self.cf_PID_roll.getCurrVel(), self.cf_PID_thrust.getCurrVel(), self.cf_PID_pitch.getCurrVel(), self.cf_roll, self.cf_pitch, self.cf_yaw, str(t-self.t_start)[3:-3], 1./(t-self.t_last_frame).total_seconds()) # logging.debug(strPrint) return " SP | SENT [ P , I , D ]\t\t" + strPrint def save_algo_iteration(self, str_OSD="", newline_separator='\t\t', margin_x=25, margin_y=25, text_color=(200, 200, 200), font=cv2.FONT_HERSHEY_DUPLEX, font_scale=0.7, font_thickness=1, line_type=cv2.LINE_AA, mask_color=(255, 0, 255), img_resize_factor=0.5, save_cam_frame_before_resizing=True): if str_OSD == "": # Allow for custom OSD text, but if no text specified, print the default debug info (get_OSD_text) str_OSD = self.get_OSD_text(self.t_frame) self.t_events.append(datetime.now()) # Resize camera frame and CrazyFlie's current&target positions according to img_resize_factor curr_pos_resized = self.get_cf_curr_pos()*img_resize_factor target_pos_resized = self.get_cf_target_pos()*img_resize_factor frame_resized = cv2.resize(self.cv_cam_frame, None, fx=img_resize_factor, fy=img_resize_factor) mask_resized = cv2.resize(self.cv_filtered_HSV_mask, None, fx=img_resize_factor, fy=img_resize_factor) # Save the original camera frame to disk (for post-debugging if necessary) ###### cv2.imwrite(os.path.join(self.VIDEO_FOLDER, self.t_frame.strftime("frame_%H-%M-%S-%f.jpg")), self.cv_cam_frame if save_cam_frame_before_resizing else frame_resized) self.t_events.append(datetime.now()) # Plot OSD related to CF's current and target positions (2 circles and a connecting line) cv2.circle(frame_resized, tuple(curr_pos_resized[0:2].astype(int)), int(curr_pos_resized[2]), self.COLOR_BALL_TRACKED if self.cf_pos_tracked else self.COLOR_BALL_UNTRACKED, -1) cv2.line(frame_resized, tuple(curr_pos_resized[0:2].astype(int)), tuple(target_pos_resized[0:2].astype(int)), self.COLOR_LINE_TRACKED if self.cf_pos_tracked else self.COLOR_LINE_UNTRACKED, int(10*img_resize_factor)) cv2.circle(frame_resized, tuple(target_pos_resized[0:2].astype(int)), int(target_pos_resized[2]), self.COLOR_TARGET_TRACKED if self.cf_pos_tracked else self.COLOR_TARGET_UNTRACKED, -1) self.t_events.append(datetime.now()) # On top of that, overlay the HSV mask (so we can debug color filtering + blob detection steps) np.putmask(frame_resized, cv2.cvtColor(mask_resized, cv2.COLOR_GRAY2BGR).astype(bool), list(mask_color)) self.t_events.append(datetime.now()) # Generate the output image: upper part is the cam frame downsized according to img_resize_factor; lower part, str_OSD lines = str_OSD.split(newline_separator) self.cv_frame_out = np.zeros(((frame_resized.shape[0] + margin_y*(len(lines)+1)), frame_resized.shape[1], frame_resized.shape[2]), dtype=frame_resized.dtype) self.cv_frame_out[0:frame_resized.shape[0], :] = frame_resized self.t_events.append(datetime.now()) for cnt, l in enumerate(lines): # Add every line of text in str_OSD. Note that putText asks for bottom-left corner of text and that cnt=0 for 1st line. Therefore vertical component should be frame_resized height + OSD padding/border (0.5*margin_y) + text height (1*margin_y) cv2.putText(self.cv_frame_out, l.replace('\t', '; '), (margin_x, frame_resized.shape[0] + int(margin_y*(cnt+1.4))), font, font_scale, text_color, font_thickness, line_type) # Save the output image to disk (for post-debugging if necessary) cv2.imwrite(os.path.join(self.VIDEO_FOLDER, self.t_frame.strftime("out_%H-%M-%S-%f.jpg")), self.cv_frame_out) self.t_events.append(datetime.now()) def hover(self): try: # First, run the cv algorithm to estimate the CF's position self.detect_cf_in_camera() except: # Only way detect_cf_in_camera raises an Exception is if a camera frame couldn't be grabbed logging.exception("Couldn't grab a frame from the camera. Exiting") return datetime.now() # Need to stop now (if I wanted to stop in now+2sec and camera kept throwing exceptions, it would keep delaying the stop and never actually stop) # Now that we know where the drone currently is, send messages to control it (roll-pitch-yaw-thrust setpoints) self.control_cf() # And save the intermediate and output frames/images to disk for debugging self.save_algo_iteration() # Last, process kb input to control the experiment if not self.process_kb_input(): # Process kb input and take action if necessary (will return False when user wants to stop the experiment) self.cf_radio_connected = False # Set connected to False so next calls to this function just send thrust=0 messages return datetime.now() + timedelta(seconds=2) # For debugging purposes, it's great to have a few additional seconds of video&log after an experiment is stopped (helps see why it crashed) self.t_events.append(datetime.now()) logging.debug("DeltaT = {:5.2f}ms -> Total: {:5.2f}ms{}".format( (datetime.now() - self.t_frame).total_seconds()*1000, (self.t_frame - self.t_last_frame).total_seconds()*1000, "".join(["\t{}->{}: {}ms".format(i+1, i+2, (self.t_events[i+1]-self.t_events[i]).total_seconds()*1000) for i in range(len(self.t_events) - 1)]))) self.t_last_frame = self.t_frame # Remember the time this frame was taken so we can estimate FPS in next iteration return None # Return None to indicate that the experiment shall go on. All good. def estimate_cf_circle_depth(self): if False: t1 = datetime.now() _, contours, _ = cv2.findContours(self.cv_filtered_HSV_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) contour = contours[0] r = cv2.boundingRect(contour) c = cv2.minEnclosingCircle(contour) cv2.rectangle(self.cv_frame_out, r[0:2], (r[0] + r[2], r[1] + r[3]), (0, 255, 0), 2, cv2.LINE_AA) cv2.circle(self.cv_frame_out, tuple(int(x) for x in c[0]), int(c[1]), (0, 255, 0), 2, cv2.LINE_AA) t2 = datetime.now() print "\t\t{} ms;\t\t\tc:{}\t\tblob:{}".format((t2-t1).total_seconds()*1000, c[1], self.cf_curr_pos[2])
print('Setting position {}'.format(position)) x = position[0] + initial_x y = position[1] + initial_y z = position[2] + initial_z commander.go_to(x, y, z, 0.0, 4) time.sleep(4) #commander.go_to(initial_x, initial_y, initial_z, 0.0, 2) #time.sleep(2.0) commander.land(initial_z - 0.5, 3.0) time.sleep(3.0) commander.stop() log_vbat.stop() log_pos_estimate.stop() log_vel_estimate.stop() log_att_estimate.stop() log_pos_ctrl.stop() log_vel_ctrl.stop() log_att_ctrl.stop() fig, axs = plt.subplots(3, 3) axs[0, 0].plot(POSITION_X) axs[0, 0].plot(TARGET_X) axs[0, 0].legend(["est", "cmd"]) axs[0, 0].set_title("Position X") axs[1, 0].plot(POSITION_Y) axs[1, 0].plot(TARGET_Y) axs[1, 0].legend(["est", "cmd"])
def distance_measurement(uri): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: lpos = LogConfig(name='rxCNT', period_in_ms=100) for ele in rx_data1: lpos.add_variable(ele) scf.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(rxCNT_data) lpos.start() time.sleep(1) lpos.stop() lpos = LogConfig(name='rxCNT', period_in_ms=100) for ele in rx_data2: lpos.add_variable(ele) scf.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(rxCNT_data2) lpos.start() time.sleep(1) lpos.stop() lpos = LogConfig(name='rxCNT', period_in_ms=100) for ele in rg_data1: lpos.add_variable(ele) scf.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(ranging_data) lpos.start() time.sleep(1) lpos.stop() lpos = LogConfig(name='rxCNT', period_in_ms=100) for ele in rg_data2: lpos.add_variable(ele) scf.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(ranging_data2) lpos.start() time.sleep(1) lpos.stop() time.sleep(200) global isFinish isFinish = True #结束 lpos = LogConfig(name='rxCNT', period_in_ms=100) for ele in rx_data1: lpos.add_variable(ele) scf.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(rxCNT_data) lpos.start() time.sleep(1) lpos.stop() lpos = LogConfig(name='rxCNT', period_in_ms=100) for ele in rx_data2: lpos.add_variable(ele) scf.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(rxCNT_data2) lpos.start() time.sleep(1) lpos.stop() lpos = LogConfig(name='rxCNT', period_in_ms=100) for ele in rg_data1: lpos.add_variable(ele) scf.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(ranging_data) lpos.start() time.sleep(1) lpos.stop() lpos = LogConfig(name='rxCNT', period_in_ms=100) for ele in rg_data2: lpos.add_variable(ele) scf.cf.log.add_config(lpos) lpos.data_received_cb.add_callback(ranging_data2) lpos.start() time.sleep(1) lpos.stop() dt_col = ['uri'] dt = [uri] for k, v in mapper_finish.items(): dt_col.append(k) dt.append(v - mapper_start[k]) df = pd.DataFrame([dt], columns=dt_col) df.to_csv('./comparison_50ms_3_2.csv', index=False) return
compl_estimate_callback) scf.cf.log.add_config(log_att_estimate) log_att_estimate.data_received_cb.add_callback( attitude_estimate_callback) set_initial_position(scf, initial_x, initial_y, initial_z, initial_yaw) reset_estimator(scf) log_compl_estimate.start() log_att_estimate.start() run_sequence(scf, sequence, initial_x, initial_y, initial_z, initial_yaw) log_compl_estimate.stop() log_att_estimate.stop() fig, axs = plt.subplots(3, 2) axs[0, 0].plot(ROLL) axs[0, 0].grid() axs[0, 0].set_title("Roll Kalman") axs[1, 0].plot(PITCH) axs[1, 0].grid() axs[1, 0].set_title("Pitch Kalman") axs[2, 0].plot(YAW) axs[2, 0].grid() axs[2, 0].set_title("Yaw Kalman") axs[0, 1].plot(D0) axs[0, 1].grid() axs[0, 1].set_title("Roll complementary")
t = position[4] commander.go_to(x, y, z, yaw, t) time.sleep(t) #commander.go_to(initial_x, initial_y, initial_z, 0.0, 2) #time.sleep(2.0) commander.land(initial_z - 0.5, 3.0, yaw= landing_yaw) time.sleep(3.0) commander.stop() #time.sleep(10) log_pos_estimate.stop() log_vel_estimate.stop() log_att_estimate.stop() log_pos_ctrl.stop() log_vel_ctrl.stop() log_att_ctrl.stop() log_compl_estimate.stop() log_thrust.stop #scf.cf.param.set_value('usd.logging', '0') ''' log_att_estimate.stop() log_quaternion_estimate.stop() log_quaternionaux_estimate.stop() log_quaternionmeasured_estimate.stop()
class Crazyflie: # ID is for human readability def __init__(self, cf_id, radio_uri, data_only=False): self._id = cf_id self._uri = radio_uri self.stop_sig = False signal.signal(signal.SIGINT, self.signal_handler) self.cf_active = False self.accept_commands = False self.data_only = data_only self.data = None self.alt = 0 # self.bridge = CvBridge() cflib.crtp.init_drivers(enable_debug_driver=False) # try: # with SyncCrazyflie(self._uri) as scf: self.cf = CF(rw_cache="./cache") self.cf.connected.add_callback(self.connected) self.cf.disconnected.add_callback(self.disconnected) self.cf.connection_failed.add_callback(self.connection_lost) self.cf.connection_lost.add_callback(self.connection_failed) print('Connecting to %s' % radio_uri) self.cf.open_link(radio_uri) # self.cf.param.set_value('kalman.resetEstimation', '1') # time.sleep(0.1) # self.cf.param.set_value('kalman.resetEstimation', '0') # time.sleep(1.5) # except Exception as e: # print(type(e)) # print("Unable to connect to CF %d at URI %s" % (self._id, self._uri)) # self.scf = None # self.cf = None self.data_pub = rospy.Publisher('cf/%d/data' % self._id, CFData, queue_size=10) # self.image_pub = rospy.Publisher('cf/%d/image'%self._id, Image, queue_size=10) if not self.data_only: self.cmd_sub = rospy.Subscriber('cf/%d/command' % self._id, CFCommand, self.command_cb) self.motion_sub = rospy.Subscriber('cf/%d/motion' % self._id, CFMotion, self.motion_cb) def signal_handler(self, sig, frame): if self.cf_active: self.cmd_estop() self.stop_sig = True rospy.signal_shutdown("CtrlC") #killing os.kill(os.getpgrp(), signal.SIGKILL) ## CALLBACKS ## def connected(self, uri): print("Connected to Crazyflie at URI: %s" % uri) self.cf_active = True try: self.log_data = LogConfig(name="Data", period_in_ms=10) self.log_data.add_variable('acc.x', 'float') self.log_data.add_variable('acc.y', 'float') self.log_data.add_variable('acc.z', 'float') self.log_data.add_variable('pm.vbat', 'float') self.log_data.add_variable('stateEstimate.z', 'float') self.cf.log.add_config(self.log_data) self.log_data.data_received_cb.add_callback(self.received_data) #begins logging and publishing self.log_data.start() print("Logging Setup Complete. Starting...") except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add log config, bad configuration.') def disconnected(self, uri): self.cf_active = False print("Disconnected from Crazyflie at URI: %s" % uri) def connection_failed(self, uri, msg): self.cf_active = False print("Connection Failed") def connection_lost(self, uri, msg): self.cf_active = False print("Connection Lost") def command_cb(self, msg): print("ALT: %.3f" % self.alt) if self.accept_commands: print("RECEIVED COMMAND : %s" % cmd_type[msg.cmd]) if cmd_type[msg.cmd] == 'ESTOP': self.cmd_estop() elif cmd_type[msg.cmd] == 'LAND': self.alt = 0 self.cmd_land() elif cmd_type[msg.cmd] == 'TAKEOFF': self.alt = 0.4 self.cmd_takeoff() else: print('Invalid Command! %d' % msg.cmd) elif cmd_type[msg.cmd] == 'TAKEOFF': self.alt = 0.4 self.cmd_takeoff() else: print("Not Accepting Commands -- but one was sent!") def motion_cb(self, msg): print("ALT: %.3f" % self.alt) print(msg) if self.accept_commands: self.update_alt(msg) # switching between optical flow and roll pitch motion if msg.is_flow_motion: self.set_flow_motion(msg.x, msg.y, msg.yaw, self.alt) else: self.set_rp_motion(msg.x, msg.y, msg.yaw, self.alt) else: print("Not Accepting Motion Commands -- but one was sent!") def update_alt(self, msg): #what exactly does this do? #motion.alt = self.data.alt * 100 if self.data.alt > ALT_TOLERANCE else 0 self.alt += msg.dz if self.alt < 0: self.alt = 0 elif self.alt > MAX_ALT: self.alt = MAX_ALT def received_data(self, timestamp, data, logconf): # print("DATA RECEIVED") # print(self.data) self.data = data d = CFData() d.ID = self._id d.accel_x = float(data['acc.x']) d.accel_y = float(data['acc.y']) d.accel_z = float(data['acc.z']) d.v_batt = float(data['pm.vbat']) d.alt = float(data['stateEstimate.z']) # d.alt = float(data['posEstimatorAlt.estimatedZ']) # print(d.v_batt) self.data_pub.publish(d) ## COMMANDS ## def set_flow_motion(self, vx, vy, yaw, alt): self.cf.commander.send_hover_setpoint(vx, vy, yaw, alt) def set_rp_motion(self, roll_a, pitch_a, yaw_r, alt): self.cf.commander.send_zdistance_setpoint(roll_a, pitch_a, yaw_r, alt) def cmd_estop(self): print("---- Crazyflie %d Emergency Stopping ----" % self._id) self.cf.commander.send_stop_setpoint() self.accept_commands = False def cmd_takeoff(self, alt=0.4): for y in range(10): print("taking off") self.cf.commander.send_hover_setpoint(0, 0, 0, y / 10 * alt) time.sleep(0.1) self.accept_commands = True def cmd_land(self, alt=0.4): if self.accept_commands == False: print("cannot land right now") else: for y in range(10): self.cf.commander.send_hover_setpoint(0, 0, 0, alt - (y / 10 * alt)) time.sleep(0.1) self.cmd_estop() def run(self): print("WAITING FOR ACTIVE CONNECTION") while not self.cf_active: pass print("FOUND ACTIVE CONNECTION") #handles image reads # threading.Thread(target=self.image_thread).start() rate = rospy.Rate(25) rospy.spin() self.log_data.stop()
class UAVController(): def __init__(self, targetURI=None): """ Function: __init__ Purpose: Initialize all necessary UAV functionality Inputs: none Outputs: none """ cflib.crtp.init_drivers() #self.FD = open("logFile.txt", "w") self.timeout = True self.available = [] self.UAV = Crazyflie() self.param = None self.airborne = False self._recentDataPacket = None self._receivingDataPacket = False #Attempt to locate UAV by scanning available interface for _ in range(0, 500): if (len(self.available) > 0): self.timeout = False break #If a UAV is found via scanning, break out of this loop else: self.available = cflib.crtp.scan_interfaces() if (len(self.available) > 0): if (targetURI != None): for i in range(len(self.available)): if (self.available[i][0] == targetURI): self.UAV.open_link(self.available[i][0]) self.connectedToTargetUAV = True else: self.connectedToTargetUAV = False else: self.UAV.open_link(self.available[0][0]) while (self.UAV.is_connected() == False): time.sleep(0.1) self.MC = MotionCommander(self.UAV) #Create desired logging parameters self.UAVLogConfig = LogConfig(name="UAVLog", period_in_ms=100) #self.UAVLogConfig.add_variable('pm.batteryLevel', 'float') self.UAVLogConfig.add_variable('pm.vbat', 'float') self.UAVLogConfig.add_variable('pm.batteryLevel', 'float') #self.UAVLogConfig.add_variable('stateEstimate.x', 'float') #self.UAVLogConfig.add_variable('stateEstimate.y', 'float') self.UAVLogConfig.add_variable('stateEstimate.z', 'float') self.UAVLogConfig.add_variable('pm.chargeCurrent', 'float') self.UAVLogConfig.add_variable('pm.extCurr', 'float') self.UAVLogConfig.add_variable('pwm.m1_pwm', 'float') #self.UAVLogConfig.add_variable('baro.pressure', 'float') #self.UAVLogConfig.add_variable('extrx.thrust', 'float') #Add more variables here for logging as desired self.UAV.log.add_config(self.UAVLogConfig) if (self.UAVLogConfig.valid): self.UAVLogConfig.data_received_cb.add_callback( self._getUAVDataPacket) self.UAVLogConfig.start() else: logger.warning("Could not setup log configuration") self._startTime = time.time() #For testing purposes self._trialRun = 0 #End of function def done(self): """ Function: done Purpose: Close connection to UAV to terminate all threads running Inputs: none Outputs: none """ self.UAVLogConfig.stop() self.UAV.close_link() self.airborne = False return def launch(self): """ Function: launch Purpose: Instruct the UAV to takeoff from current position to the default height Inputs: none Outputs: none """ self.airborne = True self.MC.take_off() #End of function return def land(self): """ Function: launch Purpose: Instruct the UAV to land on the ground at current position Inputs: none Outputs: none """ self.MC.land() return def move(self, distanceX, distanceY, distanceZ, velocity): """ Function: move Purpose: A wrapper function to instruct an UAV to move <x, y, z> distance from current point Inputs: distance - a floating point value distance in meters velocity - a floating point value velocity in meters per second Outputs: none """ if (self.airborne == False): self.launch() self.MC.move_distance(distanceX, distanceY, distanceZ, velocity) #End of function return def rotate(self, degree): """ Function: rotate Purpose: A wrapper function to instruct an UAV to rotate Inputs: degree - a floating point value in degrees Outputs: none """ if (self.airborne == False): self.launch() if (degree < 0): print("UC: rotate - Going Right") locDeg = 0 #self.MC.turn_right(abs(degree)) for _ in range(1, int(abs(degree) / 1)): locDeg += 1 self.MC.turn_right(1) self.MC.turn_right(abs(degree) - locDeg) else: print("UC: rotate - Going Left") self.MC.turn_left(abs(degree)) #Delay by 1 seclond, to allow for total rotation time time.sleep(1) return def getBatteryLevel(self): """ Function: getBatteryLevel Purpose: A function that reads the UAV battery level from a IOStream Inputs: none Outputs: none Description: """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["pm.vbat"] return retVal def getHeight(self): """ Function: getCurrentHeight Purpose: A function that reads the UAV height from a IOStream Inputs: none Outputs: none Description: """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["stateEstimate.z"] return retVal def isCharging(self): """ Function: getCurrentHeight Purpose: A function that reads the UAV height from a IOStream Inputs: none Outputs: none Description: """ retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): retVal = self._recentDataPacket["pm.chargeCurrent"] return retVal def _getUAVDataPacket(self, ident, data, logconfig): """ Function: getUAVDataPacket Purpose: Process a data packet received from the UAV Inputs: ident - data - logconfig - Outputs: None Description: A user should never call this function. """ self._receivingDataPacket = True self._recentDataPacket = data self._receivingDataPacket = False def appendToLogFile(self): """ Function: appendToLogFile Purpose: append log variables to log file 'log.txt.' Inputs: none Outputs: none Description: """ #retVal = None if (self._recentDataPacket != None and self._receivingDataPacket == False): current = self._recentDataPacket["pm.chargeCurrent"] extcurrent = self._recentDataPacket["pm.extCurr"] voltage = self._recentDataPacket["pm.vbat"] bat_level = self._recentDataPacket["pm.batteryLevel"] height = self._recentDataPacket["stateEstimate.z"] #x = self._recentDataPacket["stateEstimate.y"] #y = self._recentDataPacket["stateEstimate.x"] m1_pwm = self._recentDataPacket["pwm.m1_pwm"] #extrx_thrust = self._recentDataPacket["extrx.thrust"] #baro_pressure = self._recentDataPacket["baro.pressure"] with open('log2.txt', 'a') as file: #print(batlevel) file.write(str(datetime.now()) + '\n') file.write('bat_voltage: ' + str(voltage) + '\n') file.write('bat_level: ' + str(bat_level) + '\n') file.write('crg_current: ' + str(current) + '\n') file.write('ext_current: ' + str(extcurrent) + '\n') file.write('height: ' + str(height) + '\n') #file.write('x: ' + str(x) + '\n') #file.write('y: ' + str(y) + '\n') file.write('m1_pwm: ' + str(m1_pwm) + '\n')
commander.go_to(origin[0], origin[1], origin[2] + 1.0, 0.0, 4) time.sleep(4.0) commander.go_to(origin[0] - 1.5, origin[1], origin[2] + 1.0, 0.0, 3) time.sleep(3.0) commander.go_to(origin[0], origin[1], origin[2] + 1.0, 0.0, 4) time.sleep(4.0) commander.go_to(origin[0], origin[1], origin[2] - 0.15, 0.0, 2) time.sleep(2.0) #commander.go_to(origin[0]+0.5, origin[1], origin[2]+1.5, 0.0, 1) #time.sleep(4.0) # commander.go_to(origin[0], origin[1], origin[2]+1.5, 0.0, 1) # time.sleep(4.0) # commander.go_to(origin[0], origin[1]-0.75, origin[2]+1.5, 0.0, 1) # time.sleep(4.0) # commander.go_to(origin[0], origin[1]+0.75, origin[2]+1.5, 0.0, 1) # time.sleep(4.0) # commander.go_to(origin[0], origin[1], origin[2]+1.25, 0.0, 1) # time.sleep(4.0) commander.land(0.0, 3.0) time.sleep(3.0) commander.stop() log_state.stop() log_set.stop() log_cmd.stop() log_vbat.stop() log_range1.stop() log_range2.stop() log_pos_control.stop()
target=sound_feedback, args=(e1, e2)) baduanjin_sound_thread.start() pos_state_thread.start() sound_thread.start() # # Posture 1 (MotionCommander) move_baduanjin_mc_p1(scf_1, e2) # # Posture 1 (PositioningHlCommander) # activate_high_level_commander(scf_1.cf) # move_baduanjin_hl_p1(scf_1, e2) # # Posture 3 (MotionCommander) # move_baduanjin_mc_p3(scf_1, e2) # # Posture 3 (PositioningHlCommander) # activate_high_level_commander(scf_1.cf) # move_baduanjin_hl_p3(scf_1, e2) baduanjin_sound_thread.join() pos_state_thread.join() sound_thread.join() time.sleep(3) logconf_1.stop() logconf_2.stop() logconf_3.stop()
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)