Remove individual field references in favour of a simplified "global" reference for all coordinates

field_friend/automations/field.py

@@ -1,5 +1,4 @@
 from dataclasses import dataclass, field
-from typing import Optional
 
 import rosys
 from shapely.geometry import Polygon
@@ -23,23 +22,21 @@ def reversed(self):
             points=list(reversed(self.points)),
         )
 
-    def line_segment(self, reference: GeoPoint) -> rosys.geometry.LineSegment:
-        return rosys.geometry.LineSegment(point1=self.points[0].cartesian(reference),
-                                          point2=self.points[-1].cartesian(reference))
+    def line_segment(self) -> rosys.geometry.LineSegment:
+        return rosys.geometry.LineSegment(point1=self.points[0].cartesian(),
+                                          point2=self.points[-1].cartesian())
 
 
 @dataclass(slots=True, kw_only=True)
 class Field(GeoPointCollection):
-    reference: Optional[GeoPoint] = None
     visualized: bool = False
     obstacles: list[FieldObstacle] = field(default_factory=list)
     rows: list[Row] = field(default_factory=list)
     crop: str | None = None
 
     @property
     def outline(self) -> list[rosys.geometry.Point]:
-        assert self.reference is not None
-        return self.cartesian(self.reference)
+        return self.cartesian()
 
     @property
     def outline_as_tuples(self) -> list[tuple[float, float]]:

field_friend/automations/field_provider.py

@@ -1,11 +1,13 @@
 import logging
+import os
 import uuid
-from typing import Any, Literal, Optional, TypedDict, Union
+from typing import Any, Optional
 
 import rosys
 from geographiclib.geodesic import Geodesic
 from rosys.geometry import Point
 
+from .. import localization
 from ..localization import GeoPoint, Gnss
 from . import Field, FieldObstacle, Row
 
@@ -43,10 +45,6 @@ def restore(self, data: dict[str, Any]) -> None:
             outline = fields_data[i].get('outline_wgs84', [])
             for coords in outline:
                 f.points.append(GeoPoint(lat=coords[0], long=coords[1]))
-            rlat = fields_data[i].get('reference_lat', None)
-            rlong = fields_data[i].get('reference_lon', None)
-            if rlat is not None and rlong is not None:
-                f.reference = GeoPoint(lat=rlat, long=rlong)
             rows = fields_data[i].get('rows', [])
             for j, row in enumerate(rows):
                 for point in row.get('points_wgs84', []):
@@ -59,8 +57,6 @@ def invalidate(self) -> None:
     def create_field(self, points: list[GeoPoint] = []) -> Field:
         new_id = str(uuid.uuid4())
         field = Field(id=f'{new_id}', name=f'field_{len(self.fields)+1}', points=points)
-        if points:
-            self.set_reference(field, points[0])
         self.fields.append(field)
         self.invalidate()
         return field
@@ -75,9 +71,12 @@ def clear_fields(self) -> None:
         self.FIELDS_CHANGED.emit()
         self.invalidate()
 
-    def set_reference(self, field: Field, point: GeoPoint) -> None:
-        if field.reference is None:
-            field.reference = point
+    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)
@@ -141,25 +140,6 @@ def get_distance(row: Row, direction: str):
         self.fields[field_index].rows = sorted(field.rows, key=lambda row: get_distance(row, direction=direction))
         self.FIELDS_CHANGED.emit()
 
-    def ensure_field_reference(self, field: Field) -> None:
-        if self.gnss.device is None:
-            self.log.warning('not creating Reference because no GNSS device found')
-            rosys.notify('No GNSS device found', 'negative')
-            return
-        if self.gnss.current is None or self.gnss.current.gps_qual != 4:
-            self.log.warning('not creating Reference because no RTK fix available')
-            rosys.notify('No RTK fix available', 'negative')
-            return
-        if field.reference is None:
-            ref = self.gnss.reference
-            if ref is None:
-                self.log.warning('not creating Point because no reference position available')
-                rosys.notify('No reference position available')
-                return
-            field.reference = ref
-        if self.gnss.reference != field.reference:
-            self.gnss.reference = field.reference
-
     async def add_field_point(self, field: Field, point: Optional[GeoPoint] = None, new_point: Optional[GeoPoint] = None) -> None:
         assert self.gnss.current is not None
         positioning = self.gnss.current.location
@@ -172,13 +152,8 @@ async def add_field_point(self, field: Field, point: Optional[GeoPoint] = None,
         new_point = positioning
         if point is not None:
             index = field.points.index(point)
-            if index == 0:
-                self.set_reference(field, new_point)
             field.points[index] = new_point
         else:
-            if len(field.points) < 1:
-                self.set_reference(field, new_point)
-                self.gnss.reference = new_point
             field.points.append(new_point)
         self.invalidate()
 

field_friend/automations/navigation/coverage_navigation.py

@@ -84,21 +84,16 @@ async def prepare(self) -> bool:
             self.log.error('Field is not available')
             rosys.notify('No field selected', 'negative')
             return False
-        if not self.field.reference:
-            self.log.error('Field reference is not available')
-            return False
-        self.system.gnss.reference = self.field.reference
         if self.padding < self.robot_width+self.lane_distance:
             self.padding = self.robot_width+self.lane_distance
 
         self.path_planner.obstacles.clear()
         self.path_planner.areas.clear()
         assert self.field is not None
-        assert self.field.reference is not None
         for obstacle in self.field.obstacles:
             self.path_planner.obstacles[obstacle.id] = \
                 rosys.pathplanning.Obstacle(id=obstacle.id,
-                                            outline=obstacle.cartesian(self.field.reference))
+                                            outline=obstacle.cartesian())
         area = rosys.pathplanning.Area(id=f'{self.field.id}', outline=self.field.outline)
         self.path_planner.areas = {area.id: area}
         self.paths = self._generate_mowing_path()

field_friend/automations/navigation/row_on_field_navigation.py

@@ -39,9 +39,6 @@ async def prepare(self) -> bool:
         if self.field is None:
             rosys.notify('No field selected', 'negative')
             return False
-        if not self.field.reference:
-            rosys.notify('No field reference available', 'negative')
-            return False
         if not self.field.rows:
             rosys.notify('No rows available', 'negative')
             return False
@@ -55,13 +52,11 @@ async def prepare(self) -> bool:
         if self.state == self.State.FIELD_COMPLETED:
             self.clear()
         if self.state == self.State.FOLLOWING_ROW:
-            if self.current_row.line_segment(self.field.reference).distance(self.odometer.prediction.point) > 0.1:
+            if self.current_row.line_segment().distance(self.odometer.prediction.point) > 0.1:
                 self.clear()
         else:
             self.plant_provider.clear()
 
-        # NOTE: it's useful to set the reference point to the reference of the field; that way the cartesian coordinates are simpler to comprehend
-        self.gnss.reference = self.field.reference
         await rosys.sleep(0.1)  # wait for GNSS to update
         self.automation_watcher.start_field_watch(self.field.outline)
         return True
@@ -72,7 +67,6 @@ async def finish(self) -> None:
 
     async def _drive(self, distance: float) -> None:
         assert self.field is not None
-        assert self.field.reference is not None
         if self.state == self.State.APPROACHING_ROW_START:
             # TODO only drive to row if we are not on any rows and near the row start
             await self._drive_to_row(self.current_row)
@@ -82,20 +76,20 @@ async def _drive(self, distance: float) -> None:
         if self.state == self.State.FOLLOWING_ROW:
             if not self.implement.is_active:
                 await self.implement.activate()
-            if self.odometer.prediction.point.distance(self.current_row.points[-1].cartesian(self.field.reference)) >= 0.1:
+            if self.odometer.prediction.point.distance(self.current_row.points[-1].cartesian()) >= 0.1:
                 await super()._drive(distance)
             else:
                 await self.implement.deactivate()
                 self.state = self.State.RETURNING_TO_START
                 self.log.info('Returning to start...')
         if self.state == self.State.RETURNING_TO_START:
             self.driver.parameters.can_drive_backwards = True
-            end = self.current_row.points[0].cartesian(self.field.reference)
+            end = self.current_row.points[0].cartesian()
             await self.driver.drive_to(end, backward=True)  # TODO replace with following crops or replay recorded path
             inverse_yaw = (self.odometer.prediction.yaw + math.pi) % (2 * math.pi)
             next_row = self.field.rows[self.row_index + 1] if self.row_index + \
                 1 < len(self.field.rows) else self.current_row
-            between = end.interpolate(next_row.points[0].cartesian(self.field.reference), 0.5)
+            between = end.interpolate(next_row.points[0].cartesian(), 0.5)
             await self.driver.drive_to(between.polar(1.5, inverse_yaw), backward=True)
             self.driver.parameters.can_drive_backwards = False
             self.state = self.State.ROW_COMPLETED
@@ -112,9 +106,9 @@ def _should_finish(self) -> bool:
 
     async def _drive_to_row(self, row: Row):
         self.log.info(f'Driving to "{row.name}"...')
-        assert self.field and self.field.reference
-        target = row.points[0].cartesian(self.field.reference)
-        direction = target.direction(row.points[-1].cartesian(self.field.reference))
+        assert self.field
+        target = row.points[0].cartesian()
+        direction = target.direction(row.points[-1].cartesian())
         end_pose = Pose(x=target.x, y=target.y, yaw=direction)
         spline = Spline.from_poses(self.odometer.prediction, end_pose)
         await self.driver.drive_spline(spline)
@@ -162,11 +156,10 @@ def create_simulation(self) -> None:
         self.detector.simulated_objects.clear()
         if self.field is None:
             return
-        assert self.field.reference is not None
         for row in self.field.rows:
             if len(row.points) < 2:
                 continue
-            cartesian = row.cartesian(self.field.reference)
+            cartesian = row.cartesian()
             start = cartesian[0]
             end = cartesian[-1]
             length = start.distance(end)

field_friend/automations/path_provider.py

@@ -10,7 +10,6 @@
 class Path:
     name: str
     path_segments: list[rosys.driving.PathSegment] = field(default_factory=list)
-    reference: Optional[GeoPoint] = None
     visualized: bool = False
 
 

field_friend/automations/path_recorder.py

@@ -36,11 +36,6 @@ async def record_path(self, path: Path) -> None:
             self.log.warning(f'not recording path "{path.name}" not found')
             return
         self.log.info(f'recording path {path.name}')
-        if self.gnss.device != 'simulation':
-            if self.gnss.reference is None:
-                self.log.warning('not recording because no reference location set')
-                return
-            path.reference = self.gnss.reference
         rosys.notify(f'Recording...Please drive the path {path.name} now.')
         self.current_path_recording = path.name
         self.state = 'recording'
@@ -84,17 +79,6 @@ async def drive_path(self, path: Path) -> None:
         if path == []:
             self.log.warning(f'path {path.name} is empty')
             return
-        if self.gnss.device != 'simulation':
-            if path.reference is None:
-                self.log.warning('not driving because no reference location set')
-                return
-            # NOTE the target location is the path reference point; we do not approach a path if it is to far away
-            self.gnss.reference = path.reference
-            distance = self.gnss.distance(self.gnss.current)
-            if not distance or distance > 10:
-                self.log.warning('not driving because distance to reference location is too large')
-                rosys.notify('Distance to reference location is too large', 'negative')
-                return
         self.log.info(f'path: {path}')
         self.current_path_driving = path.name
         rosys.notify(f'Driving path {path.name}...', 'info')

field_friend/interface/components/field_creator.py

@@ -151,28 +151,27 @@ def build_geometry(self) -> bool:
         assert self.first_row_start is not None
         assert self.first_row_end is not None
         assert self.last_row_end is not None
-        self.field.reference = self.first_row_start
         distance = self.first_row_end.distance(self.last_row_end)
         number_of_rows = distance / (self.row_spacing) + 1
         # get AB line
-        a = self.first_row_start.cartesian(self.field.reference)
-        b = self.first_row_end.cartesian(self.field.reference)
-        c = self.last_row_end.cartesian(self.field.reference)
+        a = self.first_row_start.cartesian()
+        b = self.first_row_end.cartesian()
+        c = self.last_row_end.cartesian()
         ab = a.direction(b)
         bc = b.direction(c)
         d = a.polar(distance, bc)
         for i in range(int(number_of_rows)):
             start = a.polar(i * self.row_spacing, bc)
             end = b.polar(i * self.row_spacing, bc)
             self.field.rows.append(Row(id=str(uuid4()), name=f'Row #{len(self.field.rows)}',
-                                       points=[self.field.reference.shifted(start),
-                                               self.field.reference.shifted(end)]
+                                       points=[self.first_row_start.shifted(start),
+                                               self.first_row_start.shifted(end)]
                                        ))
         bottom_left = a.polar(-self.padding_bottom, ab).polar(-self.padding, bc)
         top_left = b.polar(self.padding, ab).polar(-self.padding, bc)
         top_right = c.polar(self.padding, ab).polar(self.padding, bc)
         bottom_right = d.polar(-self.padding_bottom, ab).polar(self.padding, bc)
-        self.field.points = [self.field.reference.shifted(p) for p in [bottom_left, top_left, top_right, bottom_right]]
+        self.field.points = [self.first_row_start.shifted(p) for p in [bottom_left, top_left, top_right, bottom_right]]
         return 1 - number_of_rows % 1 < 0.1
 
     def _apply(self) -> None:

field_friend/interface/components/field_object.py

@@ -1,7 +1,8 @@
 import numpy as np
 from nicegui.elements.scene_objects import Box, Curve, Cylinder, Extrusion, Group
 from rosys.geometry import Spline
-from ...automations import FieldProvider, Field
+
+from ...automations import Field, FieldProvider
 
 
 class field_object(Group):
@@ -42,7 +43,7 @@ def update(self, active_field: Field | None) -> None:
         [obj.delete() for obj in list(self.scene.objects.values()) if obj.name and obj.name.startswith('field_')]
         [obj.delete() for obj in list(self.scene.objects.values()) if obj.name and obj.name.startswith('obstacle_')]
         [obj.delete() for obj in list(self.scene.objects.values()) if obj.name and obj.name.startswith('row_')]
-        if active_field and active_field.reference is not None:
+        if active_field:
             field = active_field
             outline = [[point.x, point.y] for point in field.outline]
             if len(outline) > 1:  # Make sure there are at least two points to form a segment
@@ -51,17 +52,15 @@ def update(self, active_field: Field | None) -> None:
                     end = outline[(i + 1) % len(outline)]  # Loop back to the first point
                     self.create_fence(start, end)
 
-            if not field.reference:
-                return
             for obstacle in field.obstacles:
-                outline = [[point.x, point.y] for point in obstacle.cartesian(field.reference)]
+                outline = [[point.x, point.y] for point in obstacle.cartesian()]
                 Extrusion(outline, 0.1).with_name(f'obstacle_{obstacle.id}').material('#B80F0A')
 
             for row in field.rows:
                 if len(row.points) == 1:
                     continue
                 else:
-                    row_points = row.cartesian(field.reference)
+                    row_points = row.cartesian()
                     for i in range(len(row_points) - 1):
                         spline = Spline.from_points(row_points[i], row_points[i + 1])
                         Curve(

field_friend/interface/components/field_planner.py

@@ -1,11 +1,9 @@
 import logging
-from typing import TYPE_CHECKING, Any, Literal, Optional, TypedDict
+from typing import TYPE_CHECKING, Literal, Optional, TypedDict
 
-import rosys
 from nicegui import events, ui
 
-from ...automations import Field, FieldObstacle, FieldProvider, Row
-from ...localization import Gnss
+from ...automations import Field, FieldObstacle, Row
 from .field_creator import FieldCreator
 from .leaflet_map import leaflet_map
 
@@ -40,10 +38,13 @@ def __init__(self, system: 'System', leaflet: leaflet_map) -> None:
                         .classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;")
                     ui.button("Field Wizard", on_click=lambda: FieldCreator(system)).tooltip("Build a field with rows in a few simple steps") \
                         .classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;")
-                    ui.button("Clear fields", on_click=self.field_provider.clear_fields).props("outline color=warning") \
-                        .tooltip("Delete all fields").classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;")
                 with ui.row().style("width: 100%;"):
                     self.show_field_table()
+                with ui.row():
+                    ui.button("Clear fields", on_click=self.field_provider.clear_fields).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") \
+                        .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()
             self.field_provider.FIELDS_CHANGED.register_ui(self.refresh_ui)