Produce more accurate odometer fixes by averaging multiple GNSS poses while standing

field_friend/automations/automation_watcher.py

@@ -35,9 +35,11 @@ def __init__(self, system: 'System') -> None:
         self.bumper_watch_active: bool = False
         self.gnss_watch_active: bool = False
         self.field_watch_active: bool = False
+        self.last_robot_pose = self.odometer.prediction
 
         rosys.on_repeat(self.try_resume, 0.1)
         rosys.on_repeat(self.check_field_bounds, 1.0)
+        rosys.on_repeat(self.ensure_robot_pose_updates_when_not_in_automation, 5.0)
         if self.field_friend.bumper:
             self.field_friend.bumper.BUMPER_TRIGGERED.register(lambda name: self.pause(f'Bumper {name} was triggered'))
         self.gnss.GNSS_CONNECTION_LOST.register(lambda: self.pause('GNSS connection lost'))
@@ -120,3 +122,15 @@ def check_field_bounds(self) -> None:
             if self.automator.is_running:
                 self.stop('robot is outside of field boundaries')
                 self.field_watch_active = False
+
+    async def ensure_robot_pose_updates_when_not_in_automation(self) -> None:
+        if self.automator.is_running:
+            return
+        if self.gnss.is_paused:
+            self.log.warning('GNSS is paused, this should not happen outside of an automation')
+            return
+        if self.last_robot_pose.distance(self.odometer.prediction) == 0:  # if robot stands still
+            await self.gnss.update_robot_pose()
+        else:
+            self.gnss.observed_poses.clear()
+        self.last_robot_pose = self.odometer.prediction

field_friend/automations/navigation/navigation.py

@@ -1,10 +1,8 @@
 import abc
-import asyncio
 import logging
 from typing import TYPE_CHECKING, Any
 
 import rosys
-from nicegui import ui
 
 from ..implements import Implement
 
@@ -25,6 +23,7 @@ def __init__(self, system: 'System', implement: Implement) -> None:
         self.log = logging.getLogger('field_friend.navigation')
         self.driver = system.driver
         self.odometer = system.odometer
+        self.gnss = system.gnss
         self.kpi_provider = system.kpi_provider
         self.plant_provider = system.plant_provider
         self.puncher = system.puncher
@@ -47,11 +46,15 @@ async def start(self) -> None:
             if isinstance(self.driver.wheels, rosys.hardware.WheelsSimulation) and not rosys.is_test:
                 self.create_simulation()
             while not self._should_finish():
+                await self.gnss.update_robot_pose()
+                self.gnss.is_paused = True
                 distance = await self.implement.get_stretch(self.MAX_STRETCH_DISTANCE)
                 if distance > self.MAX_STRETCH_DISTANCE:  # we do not want to drive to long without observing
                     await self._drive(self.DEFAULT_DRIVE_DISTANCE)
+                    self.gnss.is_paused = False
                     continue
                 await self._drive(distance)
+                self.gnss.is_paused = False
                 await self.implement.start_workflow()
                 await self.implement.stop_workflow()
         except WorkflowException as e:
@@ -75,6 +78,7 @@ async def prepare(self) -> bool:
 
     async def finish(self) -> None:
         """Executed after the navigation is done"""
+        self.gnss.is_paused = False
 
     @abc.abstractmethod
     async def _drive(self, distance: float) -> None:

field_friend/localization/gnss.py

@@ -49,9 +49,13 @@ def __init__(self, odometer: rosys.driving.Odometer, antenna_offset: float) -> N
         self.device: str | None = None
         self._reference: Optional[GeoPoint] = None
         self.antenna_offset = antenna_offset
+        self.is_paused = False
+        self.observed_poses: list[rosys.geometry.Pose] = []
+        self.last_pose_update = rosys.time()
+        self.min_seconds_between_updates = 10.0
 
         self.needs_backup = False
-        rosys.on_repeat(self.update, 0.01)
+        rosys.on_repeat(self.check_gnss, 0.01)
         rosys.on_repeat(self.try_connection, 3.0)
 
     @property
@@ -83,7 +87,9 @@ def distance(self, point: GeoPoint) -> Optional[float]:
             return None
         return point.distance(point)
 
-    async def update(self) -> None:
+    async def check_gnss(self) -> None:
+        if self.is_paused:
+            return
         previous = deepcopy(self.current)
         try:
             self.current = await self._create_new_record()
@@ -127,12 +133,23 @@ def _on_rtk_fix(self) -> None:
         # correct the gnss coordinate by antenna offset
         self.current.location = get_new_position(self.current.location, self.antenna_offset, yaw+np.pi/2)
         cartesian_coordinates = self.current.location.cartesian(self.reference)
-        distance = self.odometer.prediction.point.distance(cartesian_coordinates)
-        if distance > 1:
-            self.log.warning(f'GNSS distance to prediction too high: {distance:.2f}m!!')
         pose = rosys.geometry.Pose(
             x=cartesian_coordinates.x,
             y=cartesian_coordinates.y,
             yaw=yaw,
             time=self.current.timestamp)
+        self.observed_poses.append(pose)
+
+    async def update_robot_pose(self) -> None:
+        assert not self.is_paused
+        while len(self.observed_poses) < 10:
+            if rosys.time() - self.last_pose_update < self.min_seconds_between_updates:
+                return
+            await rosys.sleep(0.1)
+        x = np.mean([pose.point.x for pose in self.observed_poses])
+        y = np.mean([pose.point.y for pose in self.observed_poses])
+        yaw = np.mean([pose.yaw for pose in self.observed_poses])
+        pose = rosys.geometry.Pose(x=float(x), y=float(y), yaw=float(yaw), time=rosys.time())
         self.ROBOT_POSE_LOCATED.emit(pose)
+        self.last_pose_update = rosys.time()
+        self.observed_poses.clear()

tests/test_navigation.py

@@ -107,9 +107,9 @@ async def test_follow_crops_with_slippage(system: System, detector: rosys.vision
         p = rosys.geometry.Point3d(x=x, y=(x/3) ** 3, z=0)
         p = system.odometer.prediction.transform3d(p)
         detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='maize', position=p))
-        print(p)
-    assert isinstance(system.field_friend.wheels, rosys.hardware.WheelsSimulation)
+    system.gnss.min_seconds_between_updates = 1
     system.current_navigation = system.follow_crops_navigation
+    assert isinstance(system.field_friend.wheels, rosys.hardware.WheelsSimulation)
     system.field_friend.wheels.slip_factor_right = 0.05
     system.automator.start()
     await forward(until=lambda: system.automator.is_running)