From 98affc6aaca3b347fe6d0315be84bcef2906fc2c Mon Sep 17 00:00:00 2001 From: Lukas Baecker Date: Fri, 20 Sep 2024 17:38:03 +0200 Subject: [PATCH] calculating rows and field outline with correct meter values --- field_friend/automations/field.py | 37 ++++++++++-------- .../interface/components/field_creator.py | 1 + .../localization/point_transformation.py | 38 +++++++++++++++++++ 3 files changed, 60 insertions(+), 16 deletions(-) diff --git a/field_friend/automations/field.py b/field_friend/automations/field.py index b6c234c2..5c75bacd 100644 --- a/field_friend/automations/field.py +++ b/field_friend/automations/field.py @@ -3,11 +3,14 @@ from uuid import uuid4 import rosys +from rosys.geometry import Point from shapely import offset_curve from shapely.geometry import LineString, Polygon from field_friend.localization import GeoPoint, GeoPointCollection +from ..localization.point_transformation import cartesian_to_wgs84, wgs84_to_cartesian + @dataclass(slots=True, kw_only=True) class FieldObstacle(GeoPointCollection): @@ -46,25 +49,25 @@ class Field(GeoPointCollection): def outline(self) -> list[GeoPoint]: assert self.first_row_start is not None assert self.first_row_end is not None - ab_line = LineString([self.first_row_start.tuple, self.first_row_end.tuple]) - last_row_linestring = offset_curve(ab_line, self.row_spacing * self.row_number / 100000) - end_row_points: list[GeoPoint] = [] + first_row_start_cartesian = Point(x=0, y=0) # first_row_start as reference for calculation + first_row_end_cartesian = wgs84_to_cartesian(self.first_row_start, self.first_row_end) + ab_line_cartesian = LineString([first_row_start_cartesian.tuple, first_row_end_cartesian.tuple]) + last_row_linestring = offset_curve(ab_line_cartesian, -self.row_spacing * self.row_number) + end_row_points: list[Point] = [] for point in last_row_linestring.coords: - end_row_points.append(GeoPoint(lat=point[0], long=point[1])) - outline_unbuffered: list[GeoPoint] = [] + end_row_points.append(Point(x=point[0], y=point[1])) + outline_unbuffered: list[Point] = [] for i, point in enumerate(end_row_points): outline_unbuffered.append(point) - outline_unbuffered.append(self.first_row_end) - outline_unbuffered.append(self.first_row_start) - print(f'outline_unbuffered: {outline_unbuffered}') - outline_polygon = Polygon([(p.lat, p.long) for p in outline_unbuffered]) + outline_unbuffered.append(first_row_end_cartesian) + outline_unbuffered.append(first_row_start_cartesian) + outline_polygon = Polygon([p.tuple for p in outline_unbuffered]) bufferd_polygon = outline_polygon.buffer( - self.outline_buffer_width/100000, join_style='mitre', mitre_limit=math.inf) + self.outline_buffer_width, join_style='mitre', mitre_limit=math.inf) bufferd_polygon_coords = bufferd_polygon.exterior.coords outline: list[GeoPoint] = [] for p in bufferd_polygon_coords: - outline.append(GeoPoint(lat=p[0], long=p[1])) - outline.append(outline[0]) + outline.append(cartesian_to_wgs84(self.first_row_start, Point(x=p[0], y=p[1]))) return outline @ property @@ -88,14 +91,16 @@ def worked_area(self, worked_rows: int) -> float: def rows(self) -> list[Row]: assert self.first_row_start is not None assert self.first_row_end is not None - ab_line = LineString([self.first_row_start.tuple, self.first_row_end.tuple]) + first_row_start_cartesian = Point(x=0, y=0) # first_row_start as reference for calculation + first_row_end_cartesian = wgs84_to_cartesian(self.first_row_start, self.first_row_end) + ab_line_cartesian = LineString([first_row_start_cartesian.tuple, first_row_end_cartesian.tuple]) rows = [] - for i in range(self.row_number+1): + for i in range(int(self.row_number)+1): offset = i * self.row_spacing - offset_row_coordinated = offset_curve(ab_line, offset/100_000).coords + offset_row_coordinated = offset_curve(ab_line_cartesian, -offset).coords row_points: list[GeoPoint] = [] for point in offset_row_coordinated: - row_points.append(GeoPoint(lat=point[0], long=point[1])) + row_points.append(cartesian_to_wgs84(self.first_row_start, Point(x=point[0], y=point[1]))) row = Row(id=str(uuid4()), name=f'{i + 1}', points=row_points) rows.append(row) return rows diff --git a/field_friend/interface/components/field_creator.py b/field_friend/interface/components/field_creator.py index 1bb74f37..b3a03f8a 100644 --- a/field_friend/interface/components/field_creator.py +++ b/field_friend/interface/components/field_creator.py @@ -118,6 +118,7 @@ def confirm_geometry(self) -> None: def _apply(self) -> None: self.dialog.close() + self.field_provider.fields = [] # TODO: for now we only want a single field to be saved self.field_provider.fields.append(Field(id=str(uuid4()), name=f'field_{len(self.field_provider.fields) + 1}', first_row_start=self.first_row_start, first_row_end=self.first_row_end, row_spacing=self.row_spacing, row_number=self.row_number)) self.field_provider.request_backup() diff --git a/field_friend/localization/point_transformation.py b/field_friend/localization/point_transformation.py index b6ad0484..a9788624 100644 --- a/field_friend/localization/point_transformation.py +++ b/field_friend/localization/point_transformation.py @@ -1,5 +1,6 @@ import numpy as np from geographiclib.geodesic import Geodesic +from rosys.geometry import Point from .geo_point import GeoPoint @@ -19,3 +20,40 @@ def get_new_position(reference: GeoPoint, distance: float, yaw: float) -> GeoPoi result = Geodesic.WGS84.Direct(reference.lat, reference.long, azimuth_deg, distance) new_position = [result['lat2'], result['lon2']] return GeoPoint.from_list(new_position) + + +def wgs84_to_cartesian(reference: GeoPoint, point: GeoPoint) -> Point: + """ + Converts a GeoPoint from WGS84 coordinates to Cartesian point relative to a reference GeoPoint. + Args: + reference (GeoPoint): The reference point in WGS84 coordinates (latitude and longitude). + point (GeoPoint): The point to be converted in WGS84 coordinates (latitude and longitude). + Returns: + Point: The Cartesian coordinates of the point relative to the reference point. + """ + print(f'{reference.lat} and {reference.long} but point is {point.lat} and {point.long}') + r = Geodesic.WGS84.Inverse(reference.lat, reference.long, point.lat, point.long) + s = r['s12'] + a = np.deg2rad(r['azi1']) # Azimuth in radians (bearing from reference to point) + a_cartesian = np.pi/2 - a + x = s * np.cos(a_cartesian) + y = s * np.sin(a_cartesian) + cartesian_coords = Point(x=x, y=y) + return cartesian_coords + + +def cartesian_to_wgs84(reference: GeoPoint, point: Point) -> GeoPoint: + """ + Converts a Cartesian Point to WGS84 geographic coordinates Geopoint. + + Args: + reference (GeoPoint): The reference point in WGS84 coordinates. + point (Point): The point in Cartesian coordinates to be transformed. + + Returns: + GeoPoint: The transformed point in WGS84 coordinates. + """ + r = Geodesic.WGS84.Direct(reference.lat, reference.long, 90.0, point.x) + r = Geodesic.WGS84.Direct(r['lat2'], r['lon2'], 0.0, point.y) + wgs84_coords = GeoPoint.from_list([r['lat2'], r['lon2']]) + return wgs84_coords