add GNSS correction

field_friend/hardware/field_friend_hardware.py

@@ -40,6 +40,11 @@ def __init__(self) -> None:
         self.M_PER_TICK: float = self.WHEEL_DIAMETER * np.pi / self.MOTOR_GEAR_RATIO
         self.WHEEL_DISTANCE: float = config_params['wheel_distance']
         self.ANTENNA_OFFSET: float = config_params['antenna_offset']
+        if 'robot_height' in config_params:
+            self.ROBOT_HEIGHT: float = config_params['robot_height']
+        else:
+            # TODO: set a default value for normal robots
+            self.ROBOT_HEIGHT = 0.0
         self.WORK_X: float
         self.DRILL_RADIUS: float
         implement: str = config_params['tool']

field_friend/localization/__init__.py

@@ -2,7 +2,7 @@
 from .gnss import Gnss
 from .gnss_hardware import GnssHardware
 from .gnss_simulation import GnssSimulation
-
+from .gnss_correction_service import GnssCorrectionService
 reference: GeoPoint = GeoPoint(lat=0, long=0)
 
 __all__ = [
@@ -11,4 +11,5 @@
     'Gnss',
     'GnssHardware',
     'GnssSimulation',
+    'GnssCorrectionService',
 ]

field_friend/localization/gnss_correction_service.py

@@ -0,0 +1,29 @@
+import rosys
+import numpy as np
+from .gnss import Gnss
+
+
+class GnssCorrectionService():
+    def __init__(self, imu: rosys.hardware.Imu | None, gnss: Gnss, robot_height: float) -> None:
+        self.imu = imu
+        self.gnss = gnss
+        self.CORRECTED_ROBOT_POSE = rosys.event.Event()
+        """the robot pose corrected for the robot roll (argument: Pose)"""
+        if self.imu:
+            self.imu.NEW_MEASUREMENT.register(self.update_imu)
+        self.gnss.ROBOT_POSE_LOCATED.register(self.update_gnss)
+        self._last_offset = 0.0
+        self._robot_height = robot_height
+
+    def update_imu(self, euler: tuple[float, float, float]) -> None:
+        roll = euler[0]
+        self._last_offset = self._robot_height * np.sin(np.radians(roll))
+
+    def update_gnss(self, measured_pose: rosys.geometry.Pose) -> None:
+        corrected_pose = measured_pose.transform_pose(rosys.geometry.Pose(
+            x=0,
+            y=self._last_offset,
+            yaw=0,
+            time=measured_pose.time,
+        ))
+        self.CORRECTED_ROBOT_POSE.emit(corrected_pose)

field_friend/system.py

@@ -45,6 +45,7 @@
 from .localization.geo_point import GeoPoint
 from .localization.gnss_hardware import GnssHardware
 from .localization.gnss_simulation import GnssSimulation
+from .localization.gnss_correction_service import GnssCorrectionService
 from .vision import CalibratableUsbCameraProvider, CameraConfigurator
 from .vision.zedxmini_camera import ZedxminiCameraProvider
 
@@ -66,6 +67,7 @@ def __init__(self) -> None:
 
         self.camera_provider = self.setup_camera_provider()
         self.detector: rosys.vision.DetectorHardware | rosys.vision.DetectorSimulation
+        self.gnss: GnssHardware | GnssSimulation
         self.field_friend: FieldFriend
         if self.is_real:
             try:
@@ -78,6 +80,8 @@ def __init__(self) -> None:
             self.monitoring_detector = rosys.vision.DetectorHardware(port=8005)
             self.odometer = rosys.driving.Odometer(self.field_friend.wheels)
             self.camera_configurator = CameraConfigurator(self.camera_provider, odometer=self.odometer)
+            self.gnss = GnssHardware(self.odometer, self.field_friend.ANTENNA_OFFSET)
+            self.gnss_correction_service = GnssCorrectionService(imu=self.field_friend.imu, gnss=self.gnss, robot_height=self.field_friend.ROBOT_HEIGHT)
         else:
             self.field_friend = FieldFriendSimulation(robot_id=self.version)
             # NOTE we run this in rosys.startup to enforce setup AFTER the persistence is loaded
@@ -86,15 +90,9 @@ def __init__(self) -> None:
             self.odometer = rosys.driving.Odometer(self.field_friend.wheels)
             self.camera_configurator = CameraConfigurator(
                 self.camera_provider, odometer=self.odometer, robot_id=self.version)
+            self.gnss = GnssSimulation(self.odometer, self.field_friend.wheels)
         self.plant_provider = PlantProvider()
         self.steerer = rosys.driving.Steerer(self.field_friend.wheels, speed_scaling=0.25)
-        self.gnss: GnssHardware | GnssSimulation
-        if self.is_real:
-            assert isinstance(self.field_friend, FieldFriendHardware)
-            self.gnss = GnssHardware(self.odometer, self.field_friend.ANTENNA_OFFSET)
-        else:
-            assert isinstance(self.field_friend.wheels, rosys.hardware.WheelsSimulation)
-            self.gnss = GnssSimulation(self.odometer, self.field_friend.wheels)
         self.gnss.ROBOT_POSE_LOCATED.register(self.odometer.handle_detection)
         self.driver = rosys.driving.Driver(self.field_friend.wheels, self.odometer)
         self.driver.parameters.linear_speed_limit = 0.3