def test_construction_with_sync_crazyflie_instance(self): # Fixture scf_mock = MagicMock(spec=SyncCrazyflie) scf_mock.cf = self.cf_mock # Test sut = SyncLogger(scf_mock, self.log_config_mock) sut.connect()
class _LoggingThread(Thread): def __init__(self, scf, log_vars, period_in_ms=10): Thread.__init__(self) log_config = LogConfig('GenericLogConfig', period_in_ms) log_vars = set(log_vars) log_vars |= { 'kalman.stateX', 'kalman.stateY', 'kalman.stateZ', 'controller.yaw' } self.data = dict() for var in log_vars: log_config.add_variable(var) self.data[var] = [] self.data['timestamp'] = [] self.sync_logger = SyncLogger(scf, log_config) self.terminate = False def run(self): self.sync_logger.connect() for log_item in self.sync_logger: if self.terminate: return entry = log_item[1] entry['timestamp'] = log_item[0] for var, data_list in self.data.items(): data_list.append(entry[var]) def stop(self): """ Stop the thread and wait for it to terminate :return: """ time.sleep(3) # enough time to flush out the rest of the data self.terminate = True self.join() self.sync_logger.disconnect()
def main(): # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') 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 example') else: #create log config if (small_log): logConfig_stab = LogConfig(name='Stabilizer', period_in_ms=log_period) logConfig_stab.add_variable('stabilizer.roll', 'float') logConfig_stab.add_variable('stabilizer.pitch', 'float') logConfig_stab.add_variable('stabilizer.yaw', 'float') logConfig_stab.add_variable('stabilizer.thrust', 'float') logConfig_state = LogConfig(name='State', period_in_ms=log_period) logConfig_state.add_variable('stateEstimate.x', 'float') logConfig_state.add_variable('stateEstimate.y', 'float') logConfig_state.add_variable('stateEstimate.z', 'float') """ logConfig_target = LogConfig(name='ControlTarget', period_in_ms=log_period) logConfig_target.add_variable('ctrltarget.roll', 'float') logConfig_target.add_variable('ctrltarget.pitch', 'float') logConfig_target.add_variable('ctrltarget.yaw', 'float') """ logConfig_mpow = LogConfig(name='MotorPower', period_in_ms=log_period) logConfig_mpow.add_variable('motor.m1', 'int32_t') logConfig_mpow.add_variable('motor.m2', 'int32_t') logConfig_mpow.add_variable('motor.m3', 'int32_t') logConfig_mpow.add_variable('motor.m4', 'int32_t') if (big_log): logConfig_all = LogConfig(name='all', period_in_ms=log_period) logConfig_all.add_variable('stabilizer.thrust', 'float') logConfig_all.add_variable('motor.m1', 'int32_t') logConfig_all.add_variable('motor.m2', 'int32_t') logConfig_all.add_variable('motor.m3', 'int32_t') logConfig_all.add_variable('motor.m4', 'int32_t') with SyncCrazyflie(URI) as scf: cf = scf.cf #create logger if (small_log): logger_stab = SyncLogger(scf, logConfig_stab) logger_state = SyncLogger(scf, logConfig_state) logger_mpow = SyncLogger(scf, logConfig_mpow) logger_stab.connect() logger_state.connect() logger_mpow.connect() #logger_target = SyncLogger(scf, logConfig_target) #logger_target.connect() if (big_log): logger_all = SyncLogger(scf, logConfig_all) logger_all.connect() cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) cf.param.set_value('kalman.resetEstimation', '0') time.sleep(2) for y in range(10): cf.commander.send_hover_setpoint(0, 0, 0, y / 20) time.sleep(0.1) #start point logging if (small_log): log_start = logger_stab.qsize() else: log_start = logger_all.qsize() for i in range(50): print_message("Going " + str(i)) cf.commander.send_hover_setpoint(0, 0, 0, 0.3) time.sleep(0.1) #end point logging if (small_log): log_end = logger_stab.qsize() else: log_end = logger_all.qsize() log_count = log_end - log_start print_message("Start:{}, end:{}".format(log_start, log_end)) print_message("To log {} lines.".format(log_end - log_start + 1)) for _ in range(10): cf.commander.send_hover_setpoint(0, 0, 0, 0.2) time.sleep(0.1) cf.commander.send_hover_setpoint(0, 0, 0, 0.1) time.sleep(0.2) cf.commander.send_stop_setpoint() #land time.sleep(1.5) print_message("Landed, start logging...") #write log files if (small_log): with open(log_file_name(logConfig_stab), "w") as f: log_count = write_out_log(logger_stab, f, log_start, log_end) print_message("logged stabilizer") with open(log_file_name(logConfig_state), "w") as f: log_count = write_out_log(logger_state, f, log_start, log_end) print_message("logged state") """ with open(log_file_name(logConfig_target), "w") as f: log_count = write_out_log(logger_target, f, log_start, log_end) print_message("logged control target") """ with open(log_file_name(logConfig_mpow), "w") as f: log_count = write_out_log(logger_mpow, f, log_start, log_end) print_message("logged motor power") logger_stab.disconnect() logger_state.disconnect() logger_mpow.disconnect() #logger_target.disconnect() if (big_log): with open(log_file_name(logConfig_all), "w") as f: log_count = write_out_log(logger_all, f, log_start, log_end) logger_all.disconnect() print_message("Done logging")
class SyncLoggerTest(unittest.TestCase): def setUp(self): self.cf_mock = MagicMock(spec=Crazyflie) self.cf_mock.disconnected = Caller() self.log_mock = MagicMock(spec=Log) self.cf_mock.log = self.log_mock self.log_config_mock = MagicMock(spec=LogConfig) self.log_config_mock.data_received_cb = Caller() self.sut = SyncLogger(self.cf_mock, self.log_config_mock) def test_that_log_configuration_is_added_on_connect(self): # Fixture # Test self.sut.connect() # Assert self.log_mock.add_config.assert_called_once_with(self.log_config_mock) def test_that_logging_is_started_on_connect(self): # Fixture # Test self.sut.connect() # Assert self.log_config_mock.start.assert_called_once_with() def test_connection_status_after_connect(self): # Fixture self.sut.connect() # Test actual = self.sut.is_connected() # Assert self.assertTrue(actual) def test_that_callbacks_are_removed_on_disconnect(self): # Fixture # Test self.sut.connect() self.sut.disconnect() # Assert self.assertEqual(0, len(self.cf_mock.disconnected.callbacks)) self.assertEqual(0, len(self.log_config_mock.data_received_cb.callbacks)) def test_that_log_config_is_stopped_on_disconnect(self): # Fixture self.sut.connect() # Test self.sut.disconnect() # Assert self.log_config_mock.stop.assert_called_once_with() self.log_config_mock.delete.assert_called_once_with() def test_that_data_is_received(self): # Fixture self.sut.connect() expected = ('Some ts', 'Some data', 'Some logblock') AsyncCallbackCaller(cb=self.log_config_mock.data_received_cb, args=[expected[0], expected[1], expected[2]] ).trigger() # Test actual = None for log_block in self.sut: actual = log_block break # Assert self.assertEqual(expected, actual) def test_connection_status_after_disconnect(self): # Fixture self.sut.connect() self.sut.disconnect() # Test actual = self.sut.is_connected() # Assert self.assertFalse(actual) def test_that_connect_to_connected_instance_raises_exception(self): # Fixture self.sut.connect() # Test # Assert with self.assertRaises(Exception): self.sut.connect() def test_connect_to_disconnected_instance(self): # Fixture self.sut.connect() self.sut.disconnect() # Test self.sut.connect() # Assert # Nothing here. Just not expecting an exception def test_disconnect_from_disconnected_instance(self): # Fixture # Test self.sut.disconnect() # Assert # Nothing here. Just not expecting an exception def test_connect_disconnect_with_context_management(self): # Fixture # Test with(SyncLogger(self.cf_mock, self.log_config_mock)) as sut: # Assert self.assertTrue(sut.is_connected()) self.assertFalse(sut.is_connected()) def test_that_iterator_is_implemented(self): # Fixture # Test actual = self.sut.__iter__() # Assert self.assertEqual(self.sut, actual) def test_construction_with_sync_crazyflie_instance(self): # Fixture scf_mock = MagicMock(spec=SyncCrazyflie) scf_mock.cf = self.cf_mock # Test sut = SyncLogger(scf_mock, self.log_config_mock) sut.connect() # Assert # Nothing here. Just not expecting an exception def test_getting_data_without_conection_raises_exception(self): # Fixture # Test with self.assertRaises(StopIteration): self.sut.__next__() # Assert def test_lost_connection_in_crazyflie_disconnects(self): # Fixture self.sut.connect() # Test AsyncCallbackCaller(cb=self.cf_mock.disconnected, args=['Some uri'] ).call_and_wait() # Assert self.assertFalse(self.sut.is_connected()) def test_lost_connection_in_crazyflie_raises_exception_in_iterator(self): # Fixture self.sut.connect() # Note this is not foolproof, the disconnected callback may be called # before we are waiting on data. It will raise the same exception # though and will pass AsyncCallbackCaller(cb=self.cf_mock.disconnected, delay=0.5, args=['Some uri'] ).trigger() # Test # Assert with self.assertRaises(StopIteration): self.sut.__next__()
def main(): # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) # Scan for Crazyflies and use the first one found print('Scanning interfaces for Crazyflies...') 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 example') else: if (small_log): logConfig_stab = LogConfig(name='AttitudeBeforePosCtrl', period_in_ms=log_intervall) logConfig_stab.add_variable('bposatt.roll', 'float') logConfig_stab.add_variable('bposatt.pitch', 'float') logConfig_stab.add_variable('bposatt.yaw', 'float') logConfig_stab.add_variable('bposatt.actuatorThrust', 'float') logConfig_state = LogConfig(name='AttitudeAfterPosCtrl', period_in_ms=log_intervall) logConfig_state.add_variable('aposatt.roll', 'float') logConfig_state.add_variable('aposatt.pitch', 'float') logConfig_state.add_variable('aposatt.yaw', 'float') logConfig_state.add_variable('aposatt.actuatorThrust', 'float') logConfig_all = LogConfig(name='PositionLog', period_in_ms=log_intervall) logConfig_all.add_variable('stateEstimate.x', 'float') logConfig_all.add_variable('stateEstimate.y', 'float') logConfig_all.add_variable('stateEstimate.z', 'float') with SyncCrazyflie(URI) as scf: cf = scf.cf if (small_log): logger_stab = SyncLogger(scf, logConfig_stab) logger_state = SyncLogger(scf, logConfig_state) logger_stab.connect() logger_state.connect() logger_all = SyncLogger(scf, logConfig_all) logger_all.connect() cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) cf.param.set_value('kalman.resetEstimation', '0') time.sleep(2) for y in range(maneuver_iterations): cf.commander.send_hover_setpoint(0, 0, 0, y * maneuver_step) time.sleep(time_step) for i in range(balance_iterations): print_message("Balancing out " + str(i)) cf.commander.send_hover_setpoint(0, 0, 0, height) time.sleep(time_step) log_start = logger_all._queue.qsize() for i in range(log_iterations): print_message("Logging " + str(i)) cf.commander.send_hover_setpoint(0, 0, 0, height) time.sleep(time_step) log_end = logger_all._queue.qsize() log_count = log_end - log_start print_message("Start:{}, end:{}".format(log_start, log_end)) print_message("To log {} lines.".format(log_end - log_start + 1)) for y in range(maneuver_iterations - 1): cf.commander.send_hover_setpoint(0, 0, 0, height - (y * maneuver_step)) time.sleep(time_step) cf.commander.send_hover_setpoint(0, 0, 0, 0.1) time.sleep(time_step) time.sleep(time_step) cf.commander.send_stop_setpoint() #land time.sleep(1.5) print_message("Landed, writing out log...") if (small_log): with open(log_file_name(logConfig_stab), "w") as f: log_count = write_out_log(logger_stab, f, log_start, log_end) print_message("logged stabilizer") with open(log_file_name(logConfig_state), "w") as f: log_count = write_out_log(logger_state, f, log_start, log_end) print_message("logged state") logger_stab.disconnect() logger_state.disconnect() with open(log_file_name(logConfig_all), "w") as f: log_count = write_out_log(logger_all, f, log_start, log_end) logger_all.disconnect() print_message("Done logging.")
class SyncLoggerTest(unittest.TestCase): def setUp(self): self.cf_mock = MagicMock(spec=Crazyflie) self.cf_mock.disconnected = Caller() self.log_mock = MagicMock(spec=Log) self.cf_mock.log = self.log_mock self.log_config_mock = MagicMock(spec=LogConfig) self.log_config_mock.data_received_cb = Caller() self.log_config_mock2 = MagicMock(spec=LogConfig) self.log_config_mock2.data_received_cb = Caller() self.sut = SyncLogger(self.cf_mock, self.log_config_mock) self.sut_multi = SyncLogger( self.cf_mock, [self.log_config_mock, self.log_config_mock2]) def test_that_log_configuration_is_added_on_connect(self): # Fixture # Test self.sut.connect() # Assert self.log_mock.add_config.assert_called_once_with(self.log_config_mock) def test_that_multiple_log_configurations_are_added_on_connect(self): # Fixture # Test self.sut_multi.connect() # Assert self.log_mock.add_config.assert_has_calls( [call(self.log_config_mock), call(self.log_config_mock2)]) def test_that_logging_is_started_on_connect(self): # Fixture # Test self.sut.connect() # Assert self.log_config_mock.start.assert_called_once_with() def test_that_logging_is_started_on_connect_for_multiple_log_confs(self): # Fixture # Test self.sut_multi.connect() # Assert self.log_config_mock.start.assert_called_once_with() self.log_config_mock2.start.assert_called_once_with() def test_connection_status_after_connect(self): # Fixture self.sut.connect() # Test actual = self.sut.is_connected() # Assert self.assertTrue(actual) def test_that_callbacks_are_removed_on_disconnect(self): # Fixture # Test self.sut.connect() self.sut.disconnect() # Assert self.assertEqual(0, len(self.cf_mock.disconnected.callbacks)) self.assertEqual(0, len(self.log_config_mock.data_received_cb.callbacks)) def test_that_log_config_is_stopped_on_disconnect(self): # Fixture self.sut.connect() # Test self.sut.disconnect() # Assert self.log_config_mock.stop.assert_called_once_with() self.log_config_mock.delete.assert_called_once_with() def test_that_multiple_log_configs_are_stopped_on_disconnect(self): # Fixture self.sut_multi.connect() # Test self.sut_multi.disconnect() # Assert self.log_config_mock.stop.assert_called_once_with() self.log_config_mock.delete.assert_called_once_with() self.log_config_mock2.stop.assert_called_once_with() self.log_config_mock2.delete.assert_called_once_with() def test_that_data_is_received(self): # Fixture self.sut.connect() expected = ('Some ts', 'Some data', 'Some logblock') AsyncCallbackCaller(cb=self.log_config_mock.data_received_cb, args=[expected[0], expected[1], expected[2]]).trigger() # Test actual = None for log_block in self.sut: actual = log_block break # Assert self.assertEqual(expected, actual) def test_connection_status_after_disconnect(self): # Fixture self.sut.connect() self.sut.disconnect() # Test actual = self.sut.is_connected() # Assert self.assertFalse(actual) def test_that_connect_to_connected_instance_raises_exception(self): # Fixture self.sut.connect() # Test # Assert with self.assertRaises(Exception): self.sut.connect() def test_connect_to_disconnected_instance(self): # Fixture self.sut.connect() self.sut.disconnect() # Test self.sut.connect() # Assert # Nothing here. Just not expecting an exception def test_disconnect_from_disconnected_instance(self): # Fixture # Test self.sut.disconnect() # Assert # Nothing here. Just not expecting an exception def test_connect_disconnect_with_context_management(self): # Fixture # Test with (SyncLogger(self.cf_mock, self.log_config_mock)) as sut: # Assert self.assertTrue(sut.is_connected()) self.assertFalse(sut.is_connected()) def test_that_iterator_is_implemented(self): # Fixture # Test actual = self.sut.__iter__() # Assert self.assertEqual(self.sut, actual) def test_construction_with_sync_crazyflie_instance(self): # Fixture scf_mock = MagicMock(spec=SyncCrazyflie) scf_mock.cf = self.cf_mock # Test sut = SyncLogger(scf_mock, self.log_config_mock) sut.connect() # Assert # Nothing here. Just not expecting an exception def test_getting_data_without_conection_raises_exception(self): # Fixture # Test with self.assertRaises(StopIteration): self.sut.__next__() # Assert def test_lost_connection_in_crazyflie_disconnects(self): # Fixture self.sut.connect() # Test AsyncCallbackCaller(cb=self.cf_mock.disconnected, args=['Some uri']).call_and_wait() # Assert self.assertFalse(self.sut.is_connected()) def test_lost_connection_in_crazyflie_raises_exception_in_iterator(self): # Fixture self.sut.connect() # Note this is not foolproof, the disconnected callback may be called # before we are waiting on data. It will raise the same exception # though and will pass AsyncCallbackCaller(cb=self.cf_mock.disconnected, delay=0.5, args=['Some uri']).trigger() # Test # Assert with self.assertRaises(StopIteration): self.sut.__next__()
class MyCrazyFlieClient: def __init__(self): cflib.crtp.init_drivers(enable_debug_driver=False) cf = Crazyflie(rw_cache='./cache') self.scf = SyncCrazyflie(URI, cf=cf) self.scf.open_link() self.client = None log_config = LogConfig(name='kalman', period_in_ms=100) log_config.add_variable('kalman.stateX', 'float') log_config.add_variable('kalman.stateY', 'float') self.logger_pos = SyncLogger(self.scf, log_config) log_config_2 = LogConfig(name='stabilizer', period_in_ms=100) log_config_2.add_variable('stabilizer.yaw', 'float') self.logger_orientation = SyncLogger(self.scf, log_config_2) self.home_pos = self.get_position() print('Home = %s ' % self.home_pos) self.orientation = self.get_orientation() self.client = MotionCommander(self.scf) self.client.take_off(height=0.6) def land(self): self.client.land() self.scf.close_link() def check_if_in_target(self, goal): pos = self.get_position() distance = np.sqrt( np.power((goal[0] - pos[0]), 2) + np.power((goal[1] - pos[1]), 2)) if distance < 1: self.land() return True return False def get_position(self): self.logger_pos.connect() data = self.logger_pos.next()[1] self.logger_pos.disconnect() self.logger_pos._queue.empty() return [data['kalman.stateX'], -data['kalman.stateY']] def get_orientation(self): self.logger_orientation.connect() data = self.logger_orientation.next()[1] self.logger_orientation.disconnect() self.logger_orientation._queue.empty() return -data['stabilizer.yaw'] def straight(self, speed): self.client.forward(0.25, speed) start = time.time() return start def yaw_right(self): self.client.turn_right(30, 72) start = time.time() return start def yaw_left(self): self.client.turn_left(30, 72) start = time.time() return start def take_action(self, action): start = time.time() collided = False if action == 0: start = self.straight(0.25) if action == 1: start = self.yaw_right() if action == 2: start = self.yaw_left() # print(start) return collided def goal_direction(self, goal, pos): yaw = self.get_orientation() print('yaw now %s' % yaw) pos_angle = math.atan2(goal[1] - pos[1], goal[0] - pos[0]) pos_angle = math.degrees(pos_angle) % 360 print('pos angle %s' % pos_angle) track = pos_angle - yaw track = ((track - 180) % 360) - 180 print('Track = %s ' % track) return track def observe(self, goal): position_now = self.get_position() track = self.goal_direction(goal, position_now) distance = np.sqrt( np.power((goal[0] - position_now[0]), 2) + np.power((goal[1] - position_now[1]), 2)) distance_sensor = Multiranger(self.scf) distance_sensor.start() front_distance_sensor = distance_sensor.front while front_distance_sensor is None: time.sleep(0.01) front_distance_sensor = distance_sensor.front distance_sensor.stop() print('Position now = %s ' % position_now) # print('Track = %s ' % track) print('Distance to target: %s' % distance) print('Front distance: %s' % front_distance_sensor) return position_now[0], position_now[ 1], track, distance, front_distance_sensor