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 test_different_underlying_cf_instances(self): # Fixture # Test scf1 = SyncCrazyflie('uri 1') scf2 = SyncCrazyflie('uri 2') # Assert actual1 = scf1.cf actual2 = scf2.cf self.assertNotEqual(actual1, actual2)
def log_ranging(link_uri, period_in_ms=100, keep_time_in_s=5): cflib.crtp.init_drivers() log_data = pd.DataFrame(columns=[ 'timestamp', 'receive_from_1', 'receive_from_2', 'receive_from_3', 'receive_from_4', 'compute_from_1', 'compute_from_2', 'compute_from_3', 'compute_from_4', 'total_receive', 'total_send', 'total_compute', 'receive_error', 'compute_error' ]) with SyncCrazyflie(link_uri=link_uri, cf=Crazyflie(rw_cache="./cache")) as scf: log_ranging = LogConfig(name='TSranging', period_in_ms=period_in_ms) log_ranging.add_variable('TSranging.receive_from_1', 'uint16_t') log_ranging.add_variable('TSranging.receive_from_2', 'uint16_t') log_ranging.add_variable('TSranging.receive_from_3', 'uint16_t') log_ranging.add_variable('TSranging.receive_from_4', 'uint16_t') log_ranging.add_variable('TSranging.compute_from_1', 'uint16_t') log_ranging.add_variable('TSranging.compute_from_2', 'uint16_t') log_ranging.add_variable('TSranging.compute_from_3', 'uint16_t') log_ranging.add_variable('TSranging.compute_from_4', 'uint16_t') log_ranging.add_variable('TSranging.total_receive', 'uint16_t') log_ranging.add_variable('TSranging.total_send', 'uint16_t') log_ranging.add_variable('TSranging.total_compute', 'uint16_t') log_ranging.add_variable('TSranging.receive_error', 'uint16_t') log_ranging.add_variable('TSranging.compute_error', 'uint16_t') with SyncLogger(scf, log_ranging) as logger: end_time = time.time() + keep_time_in_s for log_entry in logger: timestamp = log_entry[0] data = log_entry[1] logconf_name = log_entry[2] temp = { 'timestamp': timestamp, 'receive_from_1': data['TSranging.receive_from_1'], 'receive_from_2': data['TSranging.receive_from_2'], 'receive_from_3': data['TSranging.receive_from_3'], 'receive_from_4': data['TSranging.receive_from_4'], 'compute_from_1': data['TSranging.compute_from_1'], 'compute_from_2': data['TSranging.compute_from_2'], 'compute_from_3': data['TSranging.compute_from_3'], 'compute_from_4': data['TSranging.compute_from_4'], 'total_receive': data['TSranging.total_receive'], 'total_send': data['TSranging.total_send'], 'total_compute': data['TSranging.total_compute'], 'receive_error': data['TSranging.receive_error'], 'compute_error': data['TSranging.compute_error'] } log_data = log_data.append(temp, ignore_index=True) # print(log_data.iloc[log_data.shape[0] - 1, :]) print(temp) if time.time() > end_time: break log_data.to_csv('../data/LAB2.csv', index=False)
def main(): cflib.crtp.init_drivers(enable_debug_driver=False) numRepeticions = int(input("Quants cops vols repetir la ruta?")) ruta = input("Quina ruta vols realitzar?") with SyncCrazyflie(URI) as scf: with MotionCommander(scf) as mq90: if ruta == rutaABC: mq90.up(1) mq90.forward(1) ... ... elif ruta == rutaBC: mq90.up(1) mq90.forward(1) ... ... elif ruta == rutaCB: mq90.up(1) mq90.forward(1) ... ... elif ruta == rutaCA: mq90.up(1) mq90.forward(1) ... ... elif ruta == rutaAC: mq90.up(1) mq90.forward(1) ... ... mq90.land()
def main(): cflib.crtp.init_drivers(enable_debug_driver=False) with SyncCrazyflie(URI) as scf: with MotionCommander(scf) as mq90: print("Take off done!") mq90.up(1)
def __init__(self, uri, geos, calibs): self.data_written = False self.result_received = False with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: mems = scf.cf.mem.get_mems(MemoryElement.TYPE_LH) count = len(mems) if count != 1: raise Exception('Unexpected nr of memories found:', count) lh_mem = mems[0] for bs, geo in geos.items(): self.data_written = False print('Write geoetry', bs, 'to RAM') lh_mem.write_geo_data(bs, geo, self._data_written, write_failed_cb=self._data_failed) while not self.data_written: time.sleep(0.1) for bs, calib in calibs.items(): self.data_written = False print('Write calibration', bs, 'to RAM') lh_mem.write_calib_data(bs, calib, self._data_written, write_failed_cb=self._data_failed) while not self.data_written: time.sleep(0.1) print('Persist data') scf.cf.loc.receivedLocationPacket.add_callback(self._data_persisted) scf.cf.loc.send_lh_persist_data_packet(list(range(16)), list(range(16))) while not self.result_received: time.sleep(0.1)
def run_flight(): cflib.crtp.init_drivers(enable_debug_driver=False) available = cflib.crtp.scan_interfaces() print('Crazyflies found:') for i in available: print(i[0]) if len(available) == 0: print('No Crazyflies found, cannot run test') return threads = [] for uri in available: scf = SyncCrazyflie(uri[0], cf=Crazyflie(rw_cache='./cache')) scfs.append(scf) t = Thread(target=run_drone, args=(scf, uri[0]), daemon=True) threads.append(t) t.start() for t in threads: t.join()
def processNatNetPacket: def init_optitrack(iface): self.optitrack = rx.mkcmdsock(ip_address=iface) msg = struct.pack("I", rx.NAT_PING) server_address = '192.168.1.205' result = optitrack.sendto(msg, (server_address, rx.PORT_COMMAND)) if __name__ == '__main__': cflib.crtp.init_drivers(enable_debug_driver=False) iface = "192.168.1.7" """ Initialize Otitrack """ init_optitrack(iface) with SyncCrazyflie(uri) as scf: reset_estimator(scf) # start_position_printing(scf) run_sequence(scf, sequence) while(1) self.processNatNetPacket()
def square(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander( scf, x=0, y=0, z=0, default_velocity=0.3, default_height=0.2, controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc: print(pc.get_position()) pc.up(.5) print(pc.get_position()) pc.right(.5) print(pc.get_position()) pc.forward(.5) print(pc.get_position()) pc.left(1) print(pc.get_position()) pc.back(1) print(pc.get_position()) pc.right(1) print(pc.get_position()) pc.forward(.5) print(pc.get_position()) pc.left(.5) print(pc.get_position()) pc.down(.5) print(pc.get_position())
def selectDrone(self): self.cfs = [] self.scfs = [] self.mcs = [] self.selected = [] message = 'Quais drones deseja controlar? (1, 2, 3, 4 ou combinacao delas):' while (len(self.selected) == 0): user_input_numbers = input(message) if (user_input_numbers.find('1') != -1): self.selected.append(1) if (user_input_numbers.find('2') != -1): self.selected.append(2) if (user_input_numbers.find('3') != -1): self.selected.append(3) if (user_input_numbers.find('4') != -1): self.selected.append(4) if (len(self.selected) == 0): print("comando invalido") try: for i in self.selected: cf = Crazyflie(rw_cache='./cache') self.cfs.append(cf) sync = SyncCrazyflie(self.uris[i - 1], cf=cf) sync.open_link() self.mcs.append(MotionCommander(sync)) self.scfs.append(sync) except: print("Exception: Too many packets lost\n") self.selected = []
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 setUp(self): self.uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') self.cf_mock = MagicMock(spec=Crazyflie) self.cf_mock.connected = Caller() self.cf_mock.connection_failed = Caller() self.cf_mock.disconnected = Caller() self.cf_mock.open_link = AsyncCallbackCaller( cb=self.cf_mock.connected, args=[self.uri], delay=0.2 ).trigger self.close_link_mock = AsyncCallbackCaller( cb=self.cf_mock.disconnected, args=[self.uri], delay=0.2 ) self.cf_mock.close_link = self.close_link_mock.trigger # Mock the behaviour that param values are updated(downloaded) after connection self.param_mock = MagicMock(spec=Param) self.param_mock.all_updated = Caller() self.cf_mock.param = self.param_mock # Register a callback to be called when connected. Use it to trigger a callback # to trigger the call to the param.all_updated() callback self.cf_mock.connected.add_callback(self._connected_callback) self.sut = SyncCrazyflie(self.uri, cf=self.cf_mock)
def __init__(self, uri, threaded=False): super().__init__(threaded) # TODO: maybe add a parameter so people can change the default velocity # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) # the connection to the crazyflie self._scf = SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) # temp (or maybe will be permanent) state variable self._is_open = False self._running = False self._send_rate = 5 # want to send messages at 5Hz NOTE: the minimum is 2 Hz self._out_msg_queue = queue.Queue( ) # a queue for sending data between threads self._write_handle = threading.Thread(target=self.command_loop) self._write_handle.daemon = True # since can only command velocities and not positions, the connection # needs some awareness of the current position to be able to do # the math necessary self._current_position_xyz = [0.0, 0.0, 0.0] # [x, y, z]
def stateInit(self): """Initialize CF (scan, open link) and logging framework""" #--- Scan for cf cflib.crtp.init_drivers(enable_debug_driver=False) print('Scanning interfaces for Crazyflies...') available = cflib.crtp.scan_interfaces() if len(available) == 0: print("No cf found, aborting cf code.") self.cf = None else: print('Crazyflies found:') for i in available: print(str(i[0])) self.URI = 'radio://0/80/2M' #available[0][0] self.cf = Crazyflie(rw_cache='./cache') if self.cf is None: self.b.wait() print('Not running cf code.') return None self.scf = SyncCrazyflie(self.URI,cf=Crazyflie(rw_cache='./cache')) self.scf.open_link() self.mc = MotionCommander(self.scf) self.file = open("ugly_log.txt","a") #-- Barrier to wait for CV thread self.b.wait() self.lgr = UglyLogger(self.URI, self.scf, self.file) self.enterTakingOff() return self.stateTakingOff
def cf_handler(self): uri = "radio://0/30" lg_block = LogConfig(name='LH', period_in_ms=50) lg_block.add_variable('lighthouse.angle0x', 'float') lg_block.add_variable('lighthouse.angle0y', 'float') lg_block.add_variable('lighthouse.angle1x', 'float') lg_block.add_variable('lighthouse.angle1y', 'float') cf = Crazyflie(rw_cache='./cache') with SyncCrazyflie(uri, cf=cf) as scf: print("Getting LH geometry") mems = scf.cf.mem.get_mems(MemoryElement.TYPE_LH) mems[0].update(self._update_geometry) with SyncLogger(scf, lg_block) as logger: for log_entry in logger: data = log_entry[1] self.angles[0][0] = data['lighthouse.angle0x'] self.angles[0][1] = data['lighthouse.angle0y'] self.angles[1][0] = data['lighthouse.angle1x'] self.angles[1][1] = data['lighthouse.angle1y'] if self.terminate: break self.update_scene()
def __init__(self, cf_id, radio_uri): self._id = cf_id self._uri = radio_uri cflib.crtp.init_drivers(enable_debug_driver=False) try: self.scf = SyncCrazyflie(URI) self.cf = self.scf.cf except 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, CFImage, queue_size=10) self.cmd_sub = rospy.Subscriber('cf/%d/command' % self._id, CFCommand, command_cb) self.motion_sub = rospy.Subscriber('cf/%d/motion' % self._id, CFMotion, motion_cb) threading.Thread(target=self.image_thread).start()
def simple_sequence(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander(scf) as pc: pc.forward(1.0) pc.left(1.0) pc.back(1.0) pc.go_to(0.0, 0.0, 1.0)
def slightly_more_complex_usage(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander( scf, x=0.0, y=0.0, z=0.0, default_velocity=0.3, default_height=0.5, controller=PositionHlCommander.CONTROLLER_PID) as pc: # Go to a coordinate pc.go_to(1.0, 1.0, 1.0) # Move relative to the current position pc.right(1.0) # Go to a coordinate and use default height pc.go_to(0.0, 0.0) # Go slowly to a coordinate pc.go_to(1.0, 1.0, velocity=0.2) # Set new default velocity and height pc.set_default_velocity(0.3) pc.set_default_height(1.0) pc.go_to(0.0, 0.0)
def run_sequence(drone, waypoints): with SyncCrazyflie(drone.uri, cf=Crazyflie(rw_cache='./cache')) as scf: drone.scf = scf reset_estimator(drone) drone.start_position_reading() # 20 Hz drone.start_battery_status_reading() # 2 Hz time.sleep(1) drone.pose_home = drone.pose print('Home position:', drone.pose_home) print('Battery status: %.2f' % drone.V_bat) # takeoff to z=0.3 m: drone.takeoff(toFly=toFly) drone.hover(1, toFly=toFly) # """ Flight mission """ for goal in waypoints: drone.goTo(goal, toFly=toFly) drone.hover(1, toFly=toFly) print('Go home before landing...') drone.goTo([drone.pose_home[0], drone.pose_home[1], 0.3, 0], toFly=toFly) drone.hover(2, toFly=toFly) drone.land(toFly=toFly) print('Battery status %.2f:' % drone.V_bat)
def main(): try: #logfile reset myfile = open('SensorMaster.txt', 'w') myfile.write myfile.close() cflib.crtp.init_drivers(enable_debug_driver=False) scf = SyncCrazyflie(URI) fc = FlightCtrl(scf) m = Myo() Thread(target=fc.gesture_ctrl, args=(scf, fc, gesture)).start() Thread(target=m.gesture_detection, args=(gesture, )).start() while True: time.sleep(0.5) except KeyboardInterrupt: print('\nClosing link...') #perhaps land gracefully here scf.close_link() print('Shutting down...') os.kill(os.getpid(), signal.SIGINT) except Exception, e: print(str(e)) print('\nShutting down...') scf.close_link() sys.exit(0)
def producer(queue): fileTag = open(filenameTag, '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') cnt = 0 with SyncCrazyflie(uriTag, cf=Crazyflie(rw_cache='./cache')) as scf: with SyncLogger(scf, lg_stab) as logger: for log_entry in logger: timestamp = log_entry[0] data = log_entry[1] logconf_name = log_entry[2] # print('[%d][%s]: %s' % (timestamp, logconf_name, data)) a = '%s, %s, %s\n' % (data["stateEstimate.x"], data["stateEstimate.y"], data["stateEstimate.z"]) x = float(data["stateEstimate.x"]) y = float(data["stateEstimate.y"]) fileTag.write(a) if cnt == 15: consumerPos = (x, y) queue.put(consumerPos) cnt = 0 else: cnt += 1 if is_finished == True: endPos = (x, y) queue.put(endPos) break fileTag.close() print('Productor ended')
def main(): cflib.crtp.init_drivers(enable_debug_driver=False) numRepeticiones = int(input("¿Cuántas veces quieres repetir la ruta?")) ruta = input("¿Qué ruta quieres realizar?") with SyncCrazyflie(URI) as scf: with MotionCommander(scf) as mq90: if ruta == rutaABC: mq90.up(1) mq90.forward(1) ... ... elif ruta == rutaBC: mq90.up(1) mq90.forward(1) ... ... elif ruta == rutaCB: mq90.up(1) mq90.forward(1) ... ... elif ruta == rutaCA: mq90.up(1) mq90.forward(1) ... ... elif ruta == rutaAC: mq90.up(1) mq90.forward(1) ... ... mq90.land()
def main(): cflib.crtp.init_drivers(enable_debug_driver=False) with SyncCrazyflie(URI) as scf: with MotionCommander(scf) as mq90: mq90.up(valueToMove) mq90.forward(valueGoForward) mq90.land()
def __init__(self, uri='radio://0/80/2M'): self.uri = uri # URI to the Crazyflie to connect to self.cf = SyncCrazyflie(self.uri, cf=Crazyflie(rw_cache='./cache')).cf self.pose = None self.pose_home = np.array([0, 0, 0]) self.orient = None self.sp = None self.battery_state = ''
def __init__(self, uri, file_name): self._event = Event() with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: writer = LighthouseConfigWriter(scf.cf) writer.write_and_store_config_from_file(self._data_written, file_name) self._event.wait()
def test_that_the_same_cf_object_can_be_connected_multiple_times(self): # Fixture self.test_rig_support.restart_devices(self.test_rig_support.all_uris) cf = Crazyflie(rw_cache='./cache') # Test for uri in self.test_rig_support.all_uris: with SyncCrazyflie(uri, cf=cf): pass
def test_that_requested_logging_is_received_properly_from_one_cf(self): # Fixture uri = self.test_rig_support.all_uris[0] self.test_rig_support.restart_devices([uri]) cf = Crazyflie(rw_cache='./cache') # Test and Assert with SyncCrazyflie(uri, cf=cf) as scf: self.assert_add_logging_and_get_non_zero_value(scf)
def land_on_elevated_surface(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander(scf, default_height=0.5, default_velocity=0.2, default_landing_height=0.35) as pc: # fly onto a landing platform at non-zero height (ex: from floor to desk, etc) pc.forward(1.0) pc.left(1.0)
def test_memory_mapping_with_one_cf(self): # Fixture uri = self.test_rig_support.all_uris[0] self.test_rig_support.restart_devices([uri]) cf = Crazyflie(rw_cache='./cache') # Test and Assert with SyncCrazyflie(uri, cf=cf) as scf: self.assert_memory_mapping(scf)
def __init__(self, uri, geos, calibs): self._event = Event() with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: writer = LighthouseConfigWriter(scf.cf, nr_of_base_stations=2) writer.write_and_store_config(self._data_written, geos=geos, calibs=calibs) self._event.wait()