diff --git a/rosys/hardware/__init__.py b/rosys/hardware/__init__.py index 6715f5ef..9029db6f 100644 --- 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 index 7d713375..3f6676ae 100644 --- a/rosys/hardware/gnss.py +++ b/rosys/hardware/gnss.py @@ -5,7 +5,6 @@ from abc import ABC from dataclasses import dataclass from enum import IntEnum -from typing import TYPE_CHECKING import numpy as np import serial @@ -13,13 +12,11 @@ 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 index 87244131..f24b830a 100644 --- 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')