Expand the current IMU implementation (#250)

* wip: remove offset for now * Expand current IMU * Use default rotation * Refactor ImuMeasurement * Rework developer ui * Add gyro calibration to measurement * Add docstrings * code review * improve `rotation` and `angular_velocity` attributes * Fix developer_ui --------- Co-authored-by: Falko Schindler <[email protected]>
zauberzeug · Jan 23, 2025 · f637e6a · f637e6a
1 parent e2d3281
commit f637e6a
Show file tree

Hide file tree

Showing 3 changed files with 103 additions and 41 deletions.
diff --git a/rosys/hardware/__init__.py b/rosys/hardware/__init__.py
@@ -7,7 +7,7 @@
 from .estop import EStop, EStopHardware, EStopSimulation
 from .expander import ExpanderHardware
 from .gnss import Gnss, GnssHardware, GnssMeasurement, GnssSimulation
-from .imu import Imu, ImuHardware, ImuSimulation
+from .imu import Imu, ImuHardware, ImuMeasurement, ImuSimulation
 from .module import Module, ModuleHardware, ModuleSimulation
 from .robot import Robot, RobotHardware, RobotSimulation
 from .robot_brain import RobotBrain
@@ -35,6 +35,7 @@
     'GnssSimulation',
     'Imu',
     'ImuHardware',
+    'ImuMeasurement',
     'ImuSimulation',
     'Module',
     'ModuleHardware',

diff --git a/rosys/hardware/gnss.py b/rosys/hardware/gnss.py
@@ -5,21 +5,18 @@
 from abc import ABC
 from dataclasses import dataclass
 from enum import IntEnum
-from typing import TYPE_CHECKING
 
 import numpy as np
 import serial
 from nicegui import ui
 from serial.tools import list_ports
 
 from .. import rosys
+from ..driving.driver import PoseProvider
 from ..event import Event
 from ..geometry import GeoPoint, GeoPose, Pose
 from ..run import io_bound
 
-if TYPE_CHECKING:
-    from ..hardware import WheelsSimulation
-
 
 class GpsQuality(IntEnum):
     INVALID = 0
@@ -33,7 +30,7 @@ class GpsQuality(IntEnum):
     SIMULATION = 8
 
 
-@dataclass
+@dataclass(slots=True, kw_only=True)
 class GnssMeasurement:
     time: float
     pose: GeoPose
@@ -238,7 +235,7 @@ class GnssSimulation(Gnss):
     """Simulation of a GNSS receiver."""
 
     def __init__(self, *,
-                 wheels: WheelsSimulation,
+                 wheels: PoseProvider,
                  lat_std_dev: float = 0.01,
                  lon_std_dev: float = 0.01,
                  heading_std_dev: float = 0.01,

diff --git a/rosys/hardware/imu.py b/rosys/hardware/imu.py
@@ -1,41 +1,80 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+from nicegui import ui
+
+from .. import rosys
+from ..driving.driver import PoseProvider
 from ..event import Event
 from ..geometry import Rotation
 from .module import Module, ModuleHardware, ModuleSimulation
 from .robot_brain import RobotBrain
 
 
+@dataclass(slots=True, kw_only=True)
+class ImuMeasurement:
+    """Imu measurement data with corrected and uncorrected angles and angular velocities in radians."""
+    time: float
+    gyro_calibration: float
+    rotation: Rotation
+    angular_velocity: Rotation
+
+
 class Imu(Module):
+    """A module that provides measurements from an IMU."""
 
-    def __init__(self, offset_rotation: Rotation, **kwargs) -> None:
+    def __init__(self, offset_rotation: Rotation | None = None, **kwargs) -> None:
         super().__init__(**kwargs)
-        self.offset_rotation = offset_rotation
-        self.rotation: Rotation | None = None
-        self.gyro_calibration: float = 0.0
+        self.offset_rotation = offset_rotation or Rotation.zero()
+        self.last_measurement: ImuMeasurement | None = None
 
         self.NEW_MEASUREMENT = Event()
-        """a new measurement has been received (argument: Tuple[Roll, Pitch, Yaw])"""
-
-    @property
-    def roll(self) -> float | None:
-        return self.rotation.roll if self.rotation else None
-
-    @property
-    def pitch(self) -> float | None:
-        return self.rotation.pitch if self.rotation else None
-
-    @property
-    def yaw(self) -> float | None:
-        return self.rotation.yaw if self.rotation else None
-
-    @property
-    def euler(self) -> tuple[float, float, float] | None:
-        return self.rotation.euler if self.rotation else None
-
-    def emit_measurement(self) -> None:
-        self.NEW_MEASUREMENT.emit(self.euler)
+        """a new measurement has been received (argument: ImuMeasurement)"""
+
+    def _emit_measurement(self, gyro_calibration: float, raw_rotation: Rotation, time: float) -> None:
+        new_measurement = ImuMeasurement(
+            time=time,
+            gyro_calibration=gyro_calibration,
+            rotation=raw_rotation * self.offset_rotation.T,
+            angular_velocity=Rotation.zero(),
+        )
+        if self.last_measurement is not None:
+            d_rotation = new_measurement.rotation * self.last_measurement.rotation.T
+            d_roll, d_pitch, d_yaw = d_rotation.euler
+            d_t = time - self.last_measurement.time
+            new_measurement.angular_velocity = Rotation.from_euler(d_roll / d_t, d_pitch / d_t, d_yaw / d_t)
+        self.last_measurement = new_measurement
+        self.NEW_MEASUREMENT.emit(new_measurement)
+
+    def developer_ui(self) -> None:
+        ui.label('IMU').classes('text-center text-bold')
+        with ui.row().classes('gap-0 w-56'):
+            with ui.column().classes('gap-y-0 w-1/3'):
+                ui.label('Roll:')
+                ui.label().bind_text_from(self, 'last_measurement',
+                                          lambda m: f'{np.rad2deg(m.rotation.roll):.2f}°' if m is not None else 'N/A')
+                ui.label().bind_text_from(self, 'last_measurement',
+                                          lambda m: f'{np.rad2deg(m.angular_velocity.roll):.2f}°/s' if m is not None and m.angular_velocity.roll is not None else 'N/A')
+            with ui.column().classes('gap-y-0 w-1/3'):
+                ui.label('Pitch:')
+                ui.label().bind_text_from(self, 'last_measurement',
+                                          lambda m: f'{np.rad2deg(m.rotation.pitch):.2f}°' if m is not None else 'N/A')
+                ui.label().bind_text_from(self, 'last_measurement',
+                                          lambda m: f'{np.rad2deg(m.angular_velocity.pitch):.2f}°/s' if m is not None and m.angular_velocity.pitch is not None else 'N/A')
+            with ui.column().classes('gap-y-0 w-1/3'):
+                ui.label('Yaw:')
+                ui.label().bind_text_from(self, 'last_measurement',
+                                          lambda m: f'{np.rad2deg(m.rotation.yaw):.2f}°' if m is not None else 'N/A')
+                ui.label().bind_text_from(self, 'last_measurement',
+                                          lambda m: f'{np.rad2deg(m.angular_velocity.yaw):.2f}°/s' if m is not None and m.angular_velocity.yaw is not None else 'N/A')
+        ui.label().bind_text_from(self, 'last_measurement',
+                                  lambda m: f'Gyro-Calibration: {m.gyro_calibration:.0f}' if m is not None else 'N/A')
 
 
 class ImuHardware(Imu, ModuleHardware):
+    """A hardware module that handles the communication with an IMU."""
 
     def __init__(self, robot_brain: RobotBrain, name: str = 'imu', **kwargs) -> None:
         self.name = name
@@ -50,23 +89,48 @@ def __init__(self, robot_brain: RobotBrain, name: str = 'imu', **kwargs) -> None
         super().__init__(robot_brain=robot_brain, lizard_code=self.lizard_code, core_message_fields=self.core_message_fields, **kwargs)
 
     def handle_core_output(self, time: float, words: list[str]) -> None:
-        self.gyro_calibration = float(words.pop(0))
-        rotation = Rotation.from_quaternion(
+        gyro_calibration = float(words.pop(0))
+        raw_rotation = Rotation.from_quaternion(
             float(words.pop(0)),
             float(words.pop(0)),
             float(words.pop(0)),
             float(words.pop(0)),
         )
 
-        if self.gyro_calibration < 1.0:
+        if gyro_calibration < 1.0:
             return
-
-        self.rotation = rotation * self.offset_rotation.T
-        self.emit_measurement()
+        self._emit_measurement(gyro_calibration, raw_rotation, time)
 
 
 class ImuSimulation(Imu, ModuleSimulation):
-
-    def simulate_measurement(self, rotation: Rotation) -> None:
-        self.rotation = rotation * self.offset_rotation.T
-        self.emit_measurement()
+    """Simulation of an IMU."""
+
+    def __init__(self, *,
+                 wheels: PoseProvider,
+                 interval: float = 0.1,
+                 roll_noise: float = 0.0,
+                 pitch_noise: float = 0.0,
+                 yaw_noise: float = 0.0,
+                 **kwargs) -> None:
+        super().__init__(**kwargs)
+        self.wheels = wheels
+        self._roll_noise = roll_noise
+        self._pitch_noise = pitch_noise
+        self._yaw_noise = yaw_noise
+        rosys.on_repeat(self.simulate, interval)
+
+    def simulate(self) -> None:
+        roll = np.random.normal(0, self._roll_noise)
+        pitch = np.random.normal(0, self._pitch_noise)
+        yaw = np.random.normal(self.wheels.pose.yaw, self._yaw_noise)
+        self._emit_measurement(3.0, Rotation.from_euler(roll, pitch, yaw), rosys.time())
+
+    def developer_ui(self) -> None:
+        super().developer_ui()
+        with ui.column().classes('gap-y-0'):
+            ui.number(label='Roll Noise', format='%.3f', prefix='± ', suffix='°') \
+                .bind_value(self, '_roll_noise', forward=np.deg2rad, backward=np.rad2deg).classes('w-4/5')
+            ui.number(label='Pitch Noise', format='%.3f', prefix='± ', suffix='°') \
+                .bind_value(self, '_pitch_noise', forward=np.deg2rad, backward=np.rad2deg).classes('w-4/5')
+            ui.number(label='Yaw Noise', format='%.3f', prefix='± ', suffix='°') \
+                .bind_value(self, '_yaw_noise', forward=np.deg2rad, backward=np.rad2deg).classes('w-4/5')