Distanced Reference Warning Dialog (#172)

To prevent a robot that changes its location and does not receive a
current reference from having errors/inaccuracies in its work, it should
be checked at the start of an automation whether the reference is x km
away from the field.

The user is informed by a pop-up that the reference should be reset.

---------

Co-authored-by: Pascal Schade <[email protected]>

field_friend/automations/field_provider.py

@@ -1,5 +1,4 @@
 import logging
-import os
 import uuid
 from typing import Any, Optional
 
@@ -71,13 +70,6 @@ def clear_fields(self) -> None:
         self.FIELDS_CHANGED.emit()
         self.invalidate()
 
-    def update_reference(self) -> None:
-        if self.gnss.current is None:
-            rosys.notify('No GNSS position available.')
-            return
-        localization.reference = self.gnss.current.location
-        os.utime('main.py')
-
     def create_obstacle(self, field: Field, points: list[GeoPoint] = []) -> FieldObstacle:
         obstacle = FieldObstacle(id=f'{str(uuid.uuid4())}', name=f'obstacle_{len(field.obstacles)+1}', points=points)
         field.obstacles.append(obstacle)

field_friend/automations/navigation/navigation.py

@@ -46,6 +46,8 @@ async def start(self) -> None:
                 self.log.error('Preparation failed')
                 return
             await self.gnss.update_robot_pose()
+            if self.gnss.check_distance_to_reference():
+                raise WorkflowException('reference to far away from robot')
             self.start_position = self.odometer.prediction.point
             if isinstance(self.driver.wheels, rosys.hardware.WheelsSimulation) and not rosys.is_test:
                 self.create_simulation()
@@ -65,6 +67,7 @@ async def start(self) -> None:
         except WorkflowException as e:
             self.kpi_provider.increment_weeding_kpi('automation_stopped')
             self.log.error(f'WorkflowException: {e}')
+            rosys.notify(f'An exception occurred during automation: {e}', 'negative')
         finally:
             self.kpi_provider.increment_weeding_kpi('weeding_completed')
             await self.implement.finish()

field_friend/interface/components/field_planner.py

@@ -54,7 +54,7 @@ def __init__(self, system: 'System', leaflet: leaflet_map) -> None:
                 with ui.row():
                     ui.button('Clear fields', on_click=self.clear_field_dialog.open).props("outline color=warning") \
                         .tooltip("Delete all fields").classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;")
-                    ui.button("Update reference", on_click=self.field_provider.update_reference).props("outline color=warning") \
+                    ui.button("Update reference", on_click=self.gnss.update_reference).props("outline color=warning") \
                         .tooltip("Set current position as geo reference and restart the system").classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;")
             self.show_field_settings()
             self.show_object_settings()

field_friend/interface/components/status_dev.py

@@ -330,8 +330,9 @@ def update_status() -> None:
             m2_status.text = 'Error in m2' if robot.mower.m2_error else 'No error'
 
         gnss_device_label.text = 'No connection' if system.gnss.device is None else 'Connected'
-        reference_position_label.text = 'No reference' if localization.reference is None else 'Set'
-        gnss_label.text = str(system.gnss.current.location) if system.gnss.current is not None else 'No position'
+        reference_position_label.text = 'No reference' if localization.reference is None or (
+            localization.reference.lat == 0 and localization.reference.long == 0) else str(localization.reference)
+        gnss_label.text = 'No position' if system.gnss.current is None else str(system.gnss.current.location)
         heading_label.text = f'{system.gnss.current.heading:.2f}° {direction_flag}' if system.gnss.current is not None and system.gnss.current.heading is not None else 'No heading'
         rtk_fix_label.text = f'gps_qual: {system.gnss.current.gps_qual}, mode: {system.gnss.current.mode}' if system.gnss.current is not None else 'No fix'
         gnss_paused_label.text = str(system.gnss.is_paused)

field_friend/interface/pages/main_page.py

@@ -21,6 +21,7 @@ def page() -> None:
     def content(self, devmode) -> None:
         page_height = '50vh' if devmode else 'calc(100vh - 170px)'
         ui.colors(primary='#6E93D6', secondary='#53B689', accent='#111B1E', positive='#53B689')
+        self.system.gnss.reference_warning_dialog()
         with ui.row().style(f'height:{page_height}; width: calc(100vw - 2rem); flex-wrap: nowrap;'):
             with ui.column().classes('h-full w-1/2 p-2'):
                 leaflet = leaflet_map(self.system, False)

field_friend/localization/gnss.py

@@ -1,13 +1,16 @@
 from __future__ import annotations
 
 import logging
+import os
 from abc import ABC, abstractmethod
 from copy import deepcopy
 from dataclasses import dataclass
-from typing import Any, Optional
+from typing import Any
 
 import numpy as np
 import rosys
+from nicegui import ui
+from rosys.persistence.registry import backup
 
 from .. import localization
 from .geo_point import GeoPoint
@@ -22,14 +25,15 @@ class GNSSRecord:
     gps_qual: int = 0
     altitude: float = 0.0
     separation: float = 0.0
-    heading: Optional[float] = None
+    heading: float | None = None
     speed_kmh: float = 0.0
 
 
 class Gnss(rosys.persistence.PersistentModule, ABC):
     NEEDED_POSES: int = 10
     MIN_SECONDS_BETWEEN_UPDATES: float = 10.0
     ENSURE_GNSS: bool = False
+    MAX_DISTANCE_TO_REFERENCE: float = 5000.0
 
     def __init__(self, odometer: rosys.driving.Odometer, antenna_offset: float) -> None:
         super().__init__()
@@ -48,7 +52,7 @@ def __init__(self, odometer: rosys.driving.Odometer, antenna_offset: float) -> N
         self.GNSS_CONNECTION_LOST = rosys.event.Event()
         """the GNSS connection was lost"""
 
-        self.current: Optional[GNSSRecord] = None
+        self.current: GNSSRecord | None = None
         self.device: str | None = None
         self.antenna_offset = antenna_offset
         self.is_paused = False
@@ -57,6 +61,8 @@ def __init__(self, odometer: rosys.driving.Odometer, antenna_offset: float) -> N
         self.needed_poses: int = self.NEEDED_POSES
         self.min_seconds_between_updates: float = self.MIN_SECONDS_BETWEEN_UPDATES
         self.ensure_gnss: bool = self.ENSURE_GNSS
+        self.max_distance_to_reference: float = self.MAX_DISTANCE_TO_REFERENCE
+        self.reference_alert_dialog: ui.dialog
 
         self.needs_backup = False
         rosys.on_repeat(self.check_gnss, 0.01)
@@ -88,20 +94,19 @@ async def check_gnss(self) -> None:
             # TODO also do antenna_offset correction for this event
             self.ROBOT_GNSS_POSITION_CHANGED.emit(self.current.location)
             if not self.is_paused and ("R" in self.current.mode or self.current.mode == "SSSS"):
-                self._on_rtk_fix()
+                await self._on_rtk_fix()
         except Exception:
             self.log.exception('gnss record could not be applied')
             self.current = None
 
     @abstractmethod
-    async def _create_new_record(self) -> Optional[GNSSRecord]:
+    async def _create_new_record(self) -> GNSSRecord | None:
         pass
 
-    def _on_rtk_fix(self) -> None:
+    async def _on_rtk_fix(self) -> None:
         assert self.current is not None
         if localization.reference.lat == 0 and localization.reference.long == 0:
-            self.log.info(f'GNSS reference set to {self.current.location}')
-            localization.reference = deepcopy(self.current.location)
+            await self.update_reference()
         if self.current.heading is not None:
             yaw = np.deg2rad(-self.current.heading)
         else:
@@ -146,3 +151,26 @@ def restore(self, data: dict[str, Any]) -> None:
         self.needed_poses = data.get('needed_poses', self.needed_poses)
         self.min_seconds_between_updates = data.get('min_seconds_between_updates', self.min_seconds_between_updates)
         self.ensure_gnss = data.get('ensure_gnss', self.ensure_gnss)
+
+    async def update_reference(self) -> None:
+        if self.current is None:
+            self.log.warning('No GNSS position available')
+            return
+        localization.reference = self.current.location
+        await backup(force=True)
+        self.log.info('GNSS reference set to %s', self.current.location)
+        os.utime('main.py')
+
+    def reference_warning_dialog(self) -> None:
+        with ui.dialog() as self.reference_alert_dialog, ui.card():
+            ui.label('The reference is to far away from the current position which would lead to issues in the navigation. Do you want to set it now?')
+            with ui.row():
+                ui.button("Update reference", on_click=self.update_reference).props("outline color=warning") \
+                    .tooltip("Set current position as geo reference and restart the system").classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;")
+                ui.button('Cancel', on_click=self.reference_alert_dialog.close)
+
+    def check_distance_to_reference(self) -> bool:
+        if self.current is not None and self.current.location.distance(localization.reference) > self.max_distance_to_reference:
+            self.reference_alert_dialog.open()
+            return True
+        return False