From 8d57a0e85210dd1e12358fd50badd7df801e3b68 Mon Sep 17 00:00:00 2001 From: LukasBaecker <65940705+LukasBaecker@users.noreply.github.com> Date: Wed, 25 Sep 2024 14:07:50 +0200 Subject: [PATCH] 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 --- field_friend/automations/field_provider.py | 8 ---- .../automations/navigation/navigation.py | 3 ++ .../interface/components/field_planner.py | 2 +- .../interface/components/status_dev.py | 5 ++- field_friend/interface/pages/main_page.py | 1 + field_friend/localization/gnss.py | 44 +++++++++++++++---- 6 files changed, 44 insertions(+), 19 deletions(-) diff --git a/field_friend/automations/field_provider.py b/field_friend/automations/field_provider.py index 33fcd2b3..ef724d75 100644 --- a/field_friend/automations/field_provider.py +++ b/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) diff --git a/field_friend/automations/navigation/navigation.py b/field_friend/automations/navigation/navigation.py index dab5b836..ea0d9a46 100644 --- a/field_friend/automations/navigation/navigation.py +++ b/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() diff --git a/field_friend/interface/components/field_planner.py b/field_friend/interface/components/field_planner.py index 429af05f..86faf8aa 100644 --- a/field_friend/interface/components/field_planner.py +++ b/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() diff --git a/field_friend/interface/components/status_dev.py b/field_friend/interface/components/status_dev.py index b8c27b79..dbc503fb 100644 --- a/field_friend/interface/components/status_dev.py +++ b/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) diff --git a/field_friend/interface/pages/main_page.py b/field_friend/interface/pages/main_page.py index b1a6d102..d7bc5886 100644 --- a/field_friend/interface/pages/main_page.py +++ b/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) diff --git a/field_friend/localization/gnss.py b/field_friend/localization/gnss.py index cc7cc552..36c8657a 100644 --- a/field_friend/localization/gnss.py +++ b/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,7 +25,7 @@ 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 @@ -30,6 +33,7 @@ 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