From c761640114e1797cf1685765556dcef3915754c0 Mon Sep 17 00:00:00 2001
From: Felix Agner <felix.agner@control.lth.se>
Date: Sat, 29 Oct 2022 15:32:13 +0200
Subject: [PATCH] Added a new, cleaner framework for the servers that I want to
work into a proper python package
---
code/src/omnibot_package/CrazyFlieLogger.py | 124 ++++++++++++++++++++
code/src/omnibot_package/__init__.py | 2 +
2 files changed, 126 insertions(+)
create mode 100644 code/src/omnibot_package/CrazyFlieLogger.py
create mode 100644 code/src/omnibot_package/__init__.py
diff --git a/code/src/omnibot_package/CrazyFlieLogger.py b/code/src/omnibot_package/CrazyFlieLogger.py
new file mode 100644
index 0000000..aa449c3
--- /dev/null
+++ b/code/src/omnibot_package/CrazyFlieLogger.py
@@ -0,0 +1,124 @@
+# numpy
+import numpy as np
+
+# Crazyflie packages
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+
+
+class CrazyFlieLogger:
+
+ """
+ Simple logging class that logs the Crazyflie Stabilizer from a supplied
+ link uri
+ """
+ # Define which states to log:
+ namelink = {'stateEstimate.x': 0, 'stateEstimate.y': 1,'stateEstimate.z': 2, 'stabilizer.yaw':3}
+ #_Offset for crazyflie position on robot to center
+ position_offset = 0.11
+
+ def __init__(self, link_uri):
+ """ Initialize and run the example with the specified link_uri """
+
+ self._cf = Crazyflie(rw_cache='./cache')
+ self._state = np.array([0.0,0.0,0.0,0.0])
+
+ # 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)
+
+ # The definition of the logconfig can be made before connecting
+ self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=100)
+ self._lg_stab.add_variable('stateEstimate.x', 'float')
+ self._lg_stab.add_variable('stateEstimate.y', 'float')
+ self._lg_stab.add_variable('stateEstimate.z', 'float')
+ self._lg_stab.add_variable('stabilizer.yaw', 'float')
+
+ # The fetch-as argument can be set to FP16 to save space in the log packet
+ #self._lg_stab.add_variable('pm.vbat', 'FP16')
+
+ # Adding the configuration cannot be done until a Crazyflie is
+ # connected, since we need to check that the variables we
+ # would like to log are in the TOC.
+ try:
+ self._cf.log.add_config(self._lg_stab)
+ # This callback will receive the data
+ self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
+ # This callback will be called on errors
+ self._lg_stab.error_cb.add_callback(self._stab_log_error)
+ # Start the logging
+ self._lg_stab.start()
+ except KeyError as e:
+ print('Could not start log configuration,'
+ '{} not found in TOC'.format(str(e)))
+ except AttributeError:
+ print('Could not add Stabilizer log config, bad configuration.')
+
+
+ def _stab_log_error(self, logconf, msg):
+ """Callback from the log API when an error occurs"""
+ print('Error when logging %s: %s' % (logconf.name, msg))
+
+ def _stab_log_data(self, timestamp, data, logconf):
+ """Callback from a the log API when data arrives"""
+ #print(f'[{timestamp}][{logconf.name}]: ', end='')
+ for name, value in data.items():
+ self._state[self._namelink[name]] = value
+ #print(f'{name}: {value:3.3f} ', end='\n')
+ #print()
+
+ def _connection_failed(self, link_uri, msg):
+ """Callback when connection initial connection fails (i.e no Crazyflie
+ at the specified address)"""
+ print('Connection to %s failed: %s' % (link_uri, msg))
+ self.is_connected = False
+
+ 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))
+
+ 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
+
+ def x(self):
+ """ Return x-position of robot center"""
+ # Note that the offset of the crazyflie mount position is included
+ return self._state[0] - np.sin(np.deg2rad(self._state[3]))*self._position_offset
+
+ def y(self):
+ """ Return y-position of robot center"""
+ # Note that the offset of the crazyflie mount position is included
+ return self._state[1] + np.cos(np.deg2rad(self._state[3]))*self._position_offset
+
+ def z(self):
+ """ Return z-position of robot center"""
+ return self._state[2]
+
+ def theta(self):
+ """ Return direction of robot center, measured in radians between -pi and pi."""
+ # Note that the offset of the crazyflie mount position is included
+ return np.mod(self._state[3]*np.pi/180 + 11*np.pi/6,2*np.pi)-np.pi
+
+ def states(self):
+ return self._state
+
+ def close(self):
+ self._cf.close_link()
diff --git a/code/src/omnibot_package/__init__.py b/code/src/omnibot_package/__init__.py
new file mode 100644
index 0000000..14410f4
--- /dev/null
+++ b/code/src/omnibot_package/__init__.py
@@ -0,0 +1,2 @@
+from . import crazyflie_logger
+from . import Omnibot
--
GitLab