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
