From 3aa9b9fa2d2e8ae4dcdd58ff7e805b9cd1086a4e Mon Sep 17 00:00:00 2001 From: Lukas Baecker Date: Thu, 19 Sep 2024 17:14:40 +0200 Subject: [PATCH 01/14] field_creation_wizard and field update --- field_friend/automations/field.py | 59 +++- field_friend/automations/field_provider.py | 2 + .../interface/components/field_creator.py | 103 ++---- .../interface/components/field_planner.py | 18 +- .../interface/components/geodata_picker.py | 334 ++++++++++++++++++ .../interface/components/leaflet_map.py | 7 + field_friend/interface/pages/main_page.py | 5 +- 7 files changed, 431 insertions(+), 97 deletions(-) create mode 100644 field_friend/interface/components/geodata_picker.py diff --git a/field_friend/automations/field.py b/field_friend/automations/field.py index da2c4542..b6c234c2 100644 --- a/field_friend/automations/field.py +++ b/field_friend/automations/field.py @@ -1,7 +1,10 @@ +import math from dataclasses import dataclass, field +from uuid import uuid4 import rosys -from shapely.geometry import Polygon +from shapely import offset_curve +from shapely.geometry import LineString, Polygon from field_friend.localization import GeoPoint, GeoPointCollection @@ -22,6 +25,8 @@ def reversed(self): points=list(reversed(self.points)), ) + return self # Add this line to fix the missing return statement error + def line_segment(self) -> rosys.geometry.LineSegment: return rosys.geometry.LineSegment(point1=self.points[0].cartesian(), point2=self.points[-1].cartesian()) @@ -30,15 +35,39 @@ def line_segment(self) -> rosys.geometry.LineSegment: @dataclass(slots=True, kw_only=True) class Field(GeoPointCollection): visualized: bool = False + first_row_start: GeoPoint | None = None + first_row_end: GeoPoint | None = None + row_spacing: float = 0.5 + row_number: int = 10 obstacles: list[FieldObstacle] = field(default_factory=list) - rows: list[Row] = field(default_factory=list) - crop: str | None = None + outline_buffer_width: float = 2 @property - def outline(self) -> list[rosys.geometry.Point]: - return self.cartesian() + 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] = [] + for point in last_row_linestring.coords: + end_row_points.append(GeoPoint(lat=point[0], long=point[1])) + outline_unbuffered: list[GeoPoint] = [] + 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]) + bufferd_polygon = outline_polygon.buffer( + self.outline_buffer_width/100000, 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]) + return outline - @property + @ property def outline_as_tuples(self) -> list[tuple[float, float]]: return [p.tuple for p in self.outline] @@ -46,7 +75,7 @@ def area(self) -> float: outline = self.outline if not outline: return 0.0 - polygon = Polygon([(p.x, p.y) for p in outline]) + polygon = Polygon([(p.lat, p.long) for p in outline]) return polygon.area def worked_area(self, worked_rows: int) -> float: @@ -54,3 +83,19 @@ def worked_area(self, worked_rows: int) -> float: if self.area() > 0: worked_area = worked_rows * self.area() / len(self.rows) return worked_area + + @ property + 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]) + rows = [] + for i in range(self.row_number+1): + offset = i * self.row_spacing + offset_row_coordinated = offset_curve(ab_line, offset/100_000).coords + row_points: list[GeoPoint] = [] + for point in offset_row_coordinated: + row_points.append(GeoPoint(lat=point[0], long=point[1])) + row = Row(id=str(uuid4()), name=f'{i + 1}', points=row_points) + rows.append(row) + return rows diff --git a/field_friend/automations/field_provider.py b/field_friend/automations/field_provider.py index 33fcd2b3..b2e4ba66 100644 --- a/field_friend/automations/field_provider.py +++ b/field_friend/automations/field_provider.py @@ -37,6 +37,7 @@ def backup(self) -> dict: } def restore(self, data: dict[str, Any]) -> None: + print('tes') fields_data = data.get('fields', []) rosys.persistence.replace_list(self.fields, Field, fields_data) @@ -49,6 +50,7 @@ def restore(self, data: dict[str, Any]) -> None: for j, row in enumerate(rows): for point in row.get('points_wgs84', []): f.rows[j].points.append(GeoPoint(lat=point[0], long=point[1])) + print(f'fields 🛑: {self.fields[0].rows[10]}') def invalidate(self) -> None: self.request_backup() diff --git a/field_friend/interface/components/field_creator.py b/field_friend/interface/components/field_creator.py index 1b2ec413..1bb74f37 100644 --- a/field_friend/interface/components/field_creator.py +++ b/field_friend/interface/components/field_creator.py @@ -4,7 +4,7 @@ import rosys from nicegui import ui -from field_friend.automations import Field, Row +from field_friend.automations import Field from field_friend.automations.navigation import StraightLineNavigation from field_friend.interface.components.monitoring import CameraPosition from field_friend.localization import GeoPoint @@ -29,14 +29,10 @@ def __init__(self, system: 'System'): self.plant_locator = system.plant_locator self.gnss = system.gnss self.field_provider = system.field_provider - - self.field = Field(id=str(uuid4()), name=f'Field #{len(self.field_provider.fields) + 1}') self.first_row_start: GeoPoint | None = None self.first_row_end: GeoPoint | None = None - self.last_row_end: GeoPoint | None = None self.row_spacing = 0.5 - self.padding = 1 - self.padding_bottom = 2 + self.row_number = 10 self.next: Callable = self.find_first_row with ui.dialog() as self.dialog, ui.card().style('width: 900px; max-width: none'): @@ -63,26 +59,25 @@ def find_first_row(self) -> None: 'right before the first crop. ' 'The blue line should be in the center of the row.') \ .classes('text-lg text-center') + ui.label('Place the back center of the robot over the start point of the row.') \ + .classes('text-lg text-center') self.next = self.get_infos def get_infos(self) -> None: + self.headline.text = 'Field Parameters' assert self.gnss.current is not None if not ("R" in self.gnss.current.mode or self.gnss.current.mode == "SSSS"): with self.content: ui.label('No RTK fix available.').classes('text-red') self.first_row_start = self.gnss.current.location - self.row_sight.content = '' - crops = self.plant_locator.crop_category_names[:] - crops.remove('coin_with_hole') self.content.clear() with self.content: - ui.select(label='Cultivated Crop', options=crops, clearable=True).classes('w-40') \ - .bind_value(self.field, 'crop') - ui.number('Crop Spacing', suffix='cm', - value=20, step=1, min=1, max=60) \ + ui.number('Number of rows', + value=10, step=1, min=1, max=500) \ .props('dense outlined').classes('w-40') \ - .tooltip('Set the distance between the crops') + .tooltip('Set the number of rows.')\ + .bind_value(self, 'row_number') ui.number('Row Spacing', suffix='cm', value=50, step=5, min=20, max=100) \ .props('dense outlined').classes('w-40') \ @@ -93,90 +88,38 @@ def get_infos(self) -> None: def find_row_ending(self) -> None: self.headline.text = 'Find Row Ending' self.content.clear() - with self.content: - with ui.row().classes('items-center'): - rosys.automation.automation_controls(self.automator) - ui.label('Press "Play" to start driving forward. ' - 'At the end of the row, press the "Stop" button.') \ - .classes('text-lg text-center') - self.next = self.drive_to_last_row - - def drive_to_last_row(self) -> None: - assert self.gnss.current is not None - if not("R" in self.gnss.current.mode or self.gnss.current.mode == "SSSS"): - with self.content: - ui.label('No RTK fix available.').classes('text-red') - self.first_row_end = self.gnss.current.location - - self.headline.text = 'Drive to Last Row' - self.content.clear() with self.content: rosys.driving.joystick(self.steerer, size=50, color='#6E93D6') - ui.label('Drive the robot to the last row on this side, ' - 'right before the first crop.') \ + ui.label('Drive the robot to the end of the current row.') \ + .classes('text-lg text-center') + ui.label('Place the back center of the robot over the end point of the row.') \ .classes('text-lg text-center') self.next = self.confirm_geometry def confirm_geometry(self) -> None: assert self.gnss.current is not None - if not("R" in self.gnss.current.mode or self.gnss.current.mode == "SSSS"): + if not ("R" in self.gnss.current.mode or self.gnss.current.mode == "SSSS"): with self.content: ui.label('No RTK fix available.').classes('text-red') - self.last_row_end = self.gnss.current.location - + self.first_row_end = self.gnss.current.location assert self.first_row_end is not None - assert self.last_row_end is not None - if not self.build_geometry(): - d = self.first_row_end.distance(self.last_row_end) - ui.label(f'The distance between first row and last row is {d:.2f} m. ' - f'Which does not match well with the provided row spacing of {self.row_spacing} cm.') \ - .classes('text-red') - + # TODO we do not need a last row + # we now only work with first row, row_spacing and numbers of rows self.headline.text = 'Confirm Geometry' self.content.clear() with self.content: with ui.row().classes('items-center'): - rosys.automation.automation_controls(self.automator) - ui.label('Press "Play" to start driving forward. ' - 'At the end of the row, press the "Stop" button.') \ - .classes('w-64 text-lg') + ui.label(f'First Row Start: {self.first_row_start}').classes('text-lg') + ui.label(f'First Row End: {self.first_row_end}').classes('text-lg') + ui.label(f'Row Spacing: {self.row_spacing} m').classes('text-lg') + ui.label(f'Number of Rows: {self.row_number}').classes('text-lg') + ui.button('Cancel', on_click=self.dialog.close).props('color=red') self.next = self._apply - def build_geometry(self) -> bool: - """Build the geometry of the field based on the given points. - - Returns True if the row spacing matches the distance between the first and last row, False otherwise. - Will create rows in any case to make testing easier. - """ - assert self.first_row_start is not None - assert self.first_row_end is not None - assert self.last_row_end is not None - 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() - 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.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.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: self.dialog.close() - self.field_provider.fields.append(self.field) + 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() self.field_provider.FIELDS_CHANGED.emit() diff --git a/field_friend/interface/components/field_planner.py b/field_friend/interface/components/field_planner.py index 822974ef..c4c4299b 100644 --- a/field_friend/interface/components/field_planner.py +++ b/field_friend/interface/components/field_planner.py @@ -12,7 +12,7 @@ from field_friend.system import System -TabType = Literal["Plants", "Obstacles", "Outline", "Rows"] +TabType = Literal["Obstacles", "Outline", "Rows"] class ActiveObject(TypedDict): @@ -27,9 +27,9 @@ def __init__(self, system: 'System', leaflet: leaflet_map) -> None: self.field_provider = system.field_provider self.odometer = system.odometer self.gnss = system.gnss - self.cultivatable_crops = system.crop_category_names + # self.cultivatable_crops = system.crop_category_names self.leaflet_map = leaflet - self.tab: TabType = "Plants" + self.tab: TabType = "Outline" self.active_object: ActiveObject | None = None self.active_field: Field | None = None self.restore_actives() @@ -134,16 +134,16 @@ def show_field_settings(self) -> None: .classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;") \ .tooltip("Delete field") with ui.tabs().style("width: 100%;") as self.tabs: - ui.tab("Plants", "Plants") + # ui.tab("Plants", "Plants") ui.tab("Outline", "Outline") ui.tab("Obstacles", "Obstacles") ui.tab("Rows", "Rows") with ui.tab_panels(self.tabs, value=f"{self.tab}", on_change=self.set_tab).style("width: 100%;") as self.panels: - with ui.tab_panel("Plants").style("width: 100%;"): - ui.select(self.cultivatable_crops, label="Cultivated Crop", on_change=self.field_provider.request_backup) \ - .classes("w-40").props('clearable') \ - .bind_value(self.active_field, "crop") \ - .tooltip('Set the cultivated crop which should be kept safe') + # with ui.tab_panel("Plants").style("width: 100%;"): + # ui.select(self.cultivatable_crops, label="Cultivated Crop", on_change=self.field_provider.request_backup) \ + # .classes("w-40").props('clearable') \ + # .bind_value(self.active_field, "crop") \ + # .tooltip('Set the cultivated crop which should be kept safe') with ui.tab_panel("Outline").style("width: 100%;"): for geo_point in self.active_field.points: with ui.row().style("width: 100%;"): diff --git a/field_friend/interface/components/geodata_picker.py b/field_friend/interface/components/geodata_picker.py new file mode 100644 index 00000000..a9dce750 --- /dev/null +++ b/field_friend/interface/components/geodata_picker.py @@ -0,0 +1,334 @@ +import json +import math +import uuid +import xml.etree.ElementTree as ET +from pathlib import Path +from typing import Optional + +import fiona +import geopandas as gpd +from shapely import difference, offset_curve +from shapely.geometry import (GeometryCollection, LinearRing, LineString, MultiLineString, MultiPoint, Polygon, + mapping) +from shapely.ops import transform + +import rosys +from nicegui import events, ui + +from ...automations import Field, FieldObstacle, FieldProvider, Row +from ...navigation.point_transformation import cartesian_to_wgs84 + +# Enable fiona driver +fiona.drvsupport.supported_drivers['kml'] = 'rw' # enable KML support which is disabled by default +fiona.drvsupport.supported_drivers['KML'] = 'rw' +fiona.drvsupport.supported_drivers['LIBKML'] = 'rw' + + +class geodata_picker(ui.dialog): + def __init__(self, field_provider: FieldProvider) -> None: + super().__init__() + self.field_provider = field_provider + self.is_farmdroid = False + self.safety_distance = 2.7 + self.working_width = 2.7 + self.headland_tracks = 1 + self.ab_line = 1 + + with self, ui.card(): + with ui.row(): + ui.label("Upload a file.").classes('text-xl w-80') + with ui.row(): + ui.label( + "Only a single polygon will be processed. Supported file formates: .xml with ISO 11783, .shp, .kml.").classes('w-80') + with ui.row(): + ui.label( + "If you want to upload a shape, create a zip-file containing all files (minimum: .shp, .shx, .dbf) and upload the zip.").classes('w-80') + with ui.row(): + ui.upload(on_upload=self.restore_from_file, multiple=False) + with ui.row(): + farmdroid_checkbox = ui.checkbox('FarmDroid Data', on_change=lambda e: self.set_farmdroid(e.value)) + with ui.row().bind_visibility_from(farmdroid_checkbox, 'value').classes('w-80'): + with ui.row().classes('w-full place-items-center'): + with ui.icon('help').classes('text-xl'): + ui.tooltip('The width of the attached tool.') + ui.number(label='Working Width (in m)', value=self.working_width, format='%.2f', + on_change=lambda e: self.set_working_width(e.value)) + with ui.row().classes('w-full place-items-center'): + with ui.icon('help').classes('text-xl'): + ui.tooltip('The distance between the border of the field and the headland.') + ui.number(label='Safety Distance (in m)', value=self.safety_distance, format='%.2f', + on_change=lambda e: self.set_safety_distance(e.value)) + with ui.row().classes('w-full place-items-center'): + with ui.icon('help').classes('text-xl'): + ui.tooltip('Number of headland tracks.') + ui.number(label='headland Tracks', value=self.headland_tracks, format='%.2f', + on_change=lambda e: self.set_headland_tracks(e.value)) + with ui.row().classes('w-full place-items-center'): + with ui.icon('help').classes('text-xl'): + ui.tooltip('In which direction does the AB-line run?') + switch = ui.switch('AB-Line Direction', on_change=lambda e: self.set_ab_line(e.value)) + # TODO EINGABE FÜR DAS OFFSET DER FARMDROID DATEN + + with ui.row().classes('w-full justify-end'): + ui.button('Cancel', on_click=self.close).props('outline') + + def set_farmdroid(self, value) -> None: + self.is_farmdroid = value + + def set_ab_line(self, value) -> None: + if value: + self.ab_line = 2 + else: + self.ab_line = 1 + + def set_safety_distance(self, value) -> None: + self.safety_distance = value + + def set_working_width(self, value) -> None: + self.working_width = value + + def set_headland_tracks(self, value) -> None: + self.headland_tracks = value + + def extract_coordinates_kml(self, event: events.UploadEventArguments) -> list: + coordinates = [] + gdf = gpd.read_file(event.content, drivr="KML") + x_coordinate, y_coordinate = gdf['geometry'].iloc[0].xy + extracted_points = list(zip(x_coordinate, y_coordinate)) + for point in extracted_points: + coordinates.append([point[1], point[0]]) + return [coordinates] + + def extract_coordinates_xml(self, event: events.UploadEventArguments) -> list: + coordinates = [] + tree = ET.parse(event.content) + root = tree.getroot() + for geo_data in root.findall('.//LSG'): + for point in geo_data.findall('.//PNT'): + lat = float(point.attrib['C']) + lon = float(point.attrib['D']) + coordinates.append([lat, lon]) + return [coordinates] + + def extract_coordinates_shp(self, event: events.UploadEventArguments) -> Optional[list]: + coordinates = [] + try: + gdf = gpd.read_file(event.content) + gdf['geometry'] = gdf['geometry'].apply(lambda geom: transform(self.swap_coordinates, geom)) + feature = json.loads(gdf.to_json()) + coordinates = feature["features"][0]["geometry"]["coordinates"] + return coordinates + except: + rosys.notify("The .zip file does not contain a shape file.", type='warning') + return None + + def swap_coordinates(self, lon, lat): + return lat, lon + + def get_extrapoled_line(self, p1, p2) -> LineString: + extrapol_ratio = 10 + a = [p1[0]+extrapol_ratio*(p1[0]-p2[0]), p1[1]+extrapol_ratio*(p1[1]-p2[1])] + b = [p2[0]+extrapol_ratio*(p2[0]-p1[0]), p2[1]+extrapol_ratio*(p2[1]-p1[1])] + return LineString([a, b]) + + def get_rows(self, field: Field) -> list: + if not self.is_farmdroid: + return [] + if self.safety_distance + (self.working_width / 2) < 2.7: + rosys.notify( + 'The distance between the outer headland track and the field boundary is too small. Please check the specified values.') + return [] + lines = [] + outline = [] + for point in field.outline: + outline.append([point.x, point.y]) + outline_polygon = Polygon(outline) + # TODO: hinzufügen von abstand zwischen reihen und arbeitsbreite + buffer_width = self.safety_distance + (self.headland_tracks * self.working_width) + row_spacing = self.working_width / 6 + working_area_meter = outline_polygon.buffer(-buffer_width, join_style='mitre', mitre_limit=math.inf) + working_area_coordinates_meter = mapping(working_area_meter)['coordinates'][0] + working_area_coordinates = [] + for point in working_area_coordinates_meter: + transformed_point = cartesian_to_wgs84([field.reference_lat, field.reference_lon], [-point[1], point[0]]) + working_area_coordinates.append([transformed_point[0], transformed_point[1]]) + if self.ab_line == 1: + ab_line = LineString([(working_area_coordinates[0][0], working_area_coordinates[0][1]), + (working_area_coordinates[1][0], working_area_coordinates[1][1])]) + ab_line_meter = LineString([(working_area_coordinates_meter[0][0], working_area_coordinates_meter[0][1]), + (working_area_coordinates_meter[1][0], working_area_coordinates_meter[1][1])]) + if self.ab_line == 2: + if working_area_coordinates[0][0] == working_area_coordinates[-1][0] and working_area_coordinates[0][1] == working_area_coordinates[-1][1]: + ab_line = LineString([(working_area_coordinates[0][0], working_area_coordinates[0][1]), + (working_area_coordinates[-2][0], working_area_coordinates[-2][1])]) + ab_line_meter = LineString([(working_area_coordinates_meter[0][0], working_area_coordinates_meter[0][1]), + (working_area_coordinates_meter[-2][0], working_area_coordinates_meter[-2][1])]) + else: + ab_line = LineString([(working_area_coordinates[0][0], working_area_coordinates[0][1]), + (working_area_coordinates[-1][0], working_area_coordinates[-1][1])]) + ab_line_meter = LineString([(working_area_coordinates_meter[0][0], working_area_coordinates_meter[0][1]), + (working_area_coordinates_meter[-1][0], working_area_coordinates_meter[-1][1])]) + + direction_check = offset_curve(ab_line_meter, -row_spacing) + if working_area_meter.contains(direction_check) or working_area_meter.intersects(direction_check): + row_spacing = -row_spacing + lines_meter = [] + # lines_meter.append(ab_line_meter) + # lines.append(ab_line) + line_inside = True + while line_inside: + if len(lines_meter) == 0: + line_meter = offset_curve(ab_line_meter, (row_spacing/2)) + else: + line_meter = offset_curve(lines_meter[-1], row_spacing) + if working_area_meter.contains(line_meter) or working_area_meter.intersects(line_meter): + lines_meter.append(line_meter) + line = [] + for point in mapping(line_meter)["coordinates"]: + point_wgs84 = cartesian_to_wgs84([field.reference_lat, field.reference_lon], [-point[1], point[0]]) + line.append(point_wgs84) + linestring = LineString([(line[0][0], line[0][1]), (line[1][0], line[1][1])]) + lines.append(linestring) + else: + line_inside = False + working_area_ring = LinearRing(working_area_coordinates) + rows = [] + row_number = 1 + for line in lines: + point_list = list(mapping(line)['coordinates']) + for index, point in enumerate(point_list): + point_list[index] = list(point) + extrapolated_line = self.get_extrapoled_line(point_list[0], point_list[1]) + intersection_geometry = working_area_ring.intersection(extrapolated_line) + if isinstance(intersection_geometry, MultiPoint): + intersection_list = [] + # TODO hier noch Fälle beachten, bei denen die Linie den Umring in mehr als zwei getrennten Punkten/Segmenten schneidet + for point in list(mapping(intersection_geometry)['coordinates']): + intersection_list.append([point[0], point[1]]) + divided_lines = [] + for obstacle in field.obstacles: + # TODO hier Obstacles buffern und dann diese als Beschnitt nutzen + obstacle_meter = obstacle.points([field.reference_lat, field.reference_lon]) + obstacle_points_meter = [] + for point in obstacle_meter: + obstacle_points_meter.append([point.x, point.y]) + obstacle_polygon = Polygon(obstacle_points_meter) + obstacle_buffer_width = self.working_width + self.safety_distance + obstacle_buffer = obstacle_polygon.buffer( + obstacle_buffer_width, join_style='mitre', mitre_limit=math.inf) + obstacle_coordinates_meter = mapping(obstacle_buffer)['coordinates'][0] + obstacle_coordinates = [] + for point in obstacle_coordinates_meter: + transformed_point = cartesian_to_wgs84( + [field.reference_lat, field.reference_lon], [-point[1], point[0]]) + obstacle_coordinates.append([transformed_point[0], transformed_point[1]]) + divided_lines.append(difference(LineString(intersection_list), Polygon(obstacle_coordinates))) + if any(isinstance(x, MultiLineString) for x in divided_lines): + for segment in divided_lines: + if isinstance(segment, MultiLineString): + row_segment_number = 1 + for line in mapping(segment)['coordinates']: + row_coordinates = [] + for point in line: + row_coordinates.append([point[0], point[1]]) + row_id = str(uuid.uuid4()) + new_row = Row(id=f'{row_id}', name=f'{row_number}_{row_segment_number}', + points_wgs84=row_coordinates) + rows.append(new_row) + row_segment_number += 1 + else: + row_id = str(uuid.uuid4()) + new_row = Row(id=f'{row_id}', name=f'{row_number}', points_wgs84=intersection_list) + rows.append(new_row) + elif isinstance(intersection_geometry, GeometryCollection): + for geometry in mapping(intersection_geometry)['geometries']: + if geometry == LineString or geometry == MultiPoint: + intersection_list = [] + # TODO hier noch Fälle beachten, bei denen die Linie den Umring in mehr als zwei getrennten Punkten/Segmenten schneidet + for point in list(mapping(geometry)['coordinates']): + intersection_list.append([point[0], point[1]]) + divided_lines = [] + for obstacle in field.obstacles: + # TODO hier Obstacles buffern und dann diese als Beschnitt nutzen + obstacle_meter = obstacle.points([field.reference_lat, field.reference_lon]) + obstacle_points_meter = [] + for point in obstacle_meter: + obstacle_points_meter.append([point.x, point.y]) + obstacle_polygon = Polygon(obstacle_points_meter) + obstacle_buffer_width = self.working_width + self.safety_distance + obstacle_buffer = obstacle_polygon.buffer( + obstacle_buffer_width, join_style='mitre', mitre_limit=math.inf) + obstacle_coordinates_meter = mapping(obstacle_buffer)['coordinates'][0] + obstacle_coordinates = [] + for point in obstacle_coordinates_meter: + transformed_point = cartesian_to_wgs84( + [field.reference_lat, field.reference_lon], [-point[1], point[0]]) + obstacle_coordinates.append([transformed_point[0], transformed_point[1]]) + divided_lines.append(difference(LineString(intersection_list), + Polygon(obstacle_coordinates))) + if any(isinstance(x, MultiLineString) for x in divided_lines): + for segment in divided_lines: + if isinstance(segment, MultiLineString): + row_segment_number = 1 + for line in mapping(segment)['coordinates']: + row_coordinates = [] + for point in line: + row_coordinates.append([point[0], point[1]]) + row_id = str(uuid.uuid4()) + new_row = Row( + id=f'{row_id}', name=f'{row_id}_{row_segment_number}', points_wgs84=row_coordinates) + rows.append(new_row) + row_segment_number += 1 + else: + row_id = str(uuid.uuid4()) + new_row = Row(id=f'{row_id}', name=f'{row_number}', points_wgs84=intersection_list) + rows.append(new_row) + # elif type(geometry) == Point: + # intersection_list.append([geometry[0], geometry[1]]) + row_number += 1 + return rows + + def get_obstacles(self, polygon_list): + obstacle_list = [] + for polygon in polygon_list[1:]: + row_id = str(uuid.uuid4()) + new_obstacle = FieldObstacle(id=f'{row_id}', name=f'{row_id}', points_wgs84=polygon) + obstacle_list.append(new_obstacle) + return obstacle_list + + async def restore_from_file(self, e: events.UploadEventArguments) -> None: + self.close() + coordinates: list = [] + if e is None or e.content is None: + rosys.notify("You can only upload the following file formates: .kml ,.xml. with ISO and shape files.", type='warning') + return + elif e.name[-3:].casefold() == "zip": + coordinates = self.extract_coordinates_shp(e) + elif e.name[-3:].casefold() == "kml": + coordinates = self.extract_coordinates_kml(e) + elif e.name[-3:].casefold() == "xml": + coordinates = self.extract_coordinates_xml(e) + else: + rosys.notify("You can only upload the following file formates: .kml ,.xml. with ISO and shape files.", type='warning') + return + if coordinates is None: + rosys.notify("An error occurred while importing the file.", type='negative') + return + reference_point = coordinates[0][0] + field_id = str(uuid.uuid4()) + field = Field(id=f'{field_id}', name=f'{field_id}', outline_wgs84=coordinates[0], + reference_lat=reference_point[0], reference_lon=reference_point[1]) + obstacles = [] + if len(coordinates) > 1: + obstacles = self.get_obstacles(coordinates) + field.obstacles = obstacles + rows = self.get_rows(field) + field.rows = rows + outline = [] + for point in field.outline: + outline.append([point.x, point.y]) + field.area = Polygon(outline).area + if len(coordinates[0]) > 2 and coordinates[0][0] == coordinates[0][-1]: + coordinates[0].pop() # the last point is the same as the first point + self.field_provider.add_field(field) + return diff --git a/field_friend/interface/components/leaflet_map.py b/field_friend/interface/components/leaflet_map.py index 0fcb8a38..6e069c16 100644 --- a/field_friend/interface/components/leaflet_map.py +++ b/field_friend/interface/components/leaflet_map.py @@ -141,6 +141,13 @@ def update_layers(self) -> None: for layer in self.row_layers: self.m.remove_layer(layer) self.row_layers = [] + if len(self.field_provider.fields) > 0: + print(self.field_provider.fields[0].outline_as_tuples) + for row in self.field_provider.fields[0].rows: + self.m.generic_layer(name="polyline", args=[ + row.points_as_tuples, {'color': '#6E93D6'}]) + self.m.generic_layer(name="polygon", args=[ + self.field_provider.fields[0].outline_as_tuples, {'color': '#6E93D6'}]) if current_field is None: return for obstacle in current_field.obstacles: diff --git a/field_friend/interface/pages/main_page.py b/field_friend/interface/pages/main_page.py index b1a6d102..616f1b99 100644 --- a/field_friend/interface/pages/main_page.py +++ b/field_friend/interface/pages/main_page.py @@ -6,6 +6,7 @@ from field_friend.system import System from ..components import camera_card, leaflet_map, operation, robot_scene +from ..components.field_creator import FieldCreator class main_page(): @@ -35,7 +36,6 @@ def content(self, devmode) -> None: operation(self.system) with ui.column().classes('h-full').style('width: calc(45% - 2rem); flex-wrap: nowrap;'): camera_card(self.system) - robot_scene(self.system, self.system.field_navigation.field) with ui.row().style("margin: 1rem; width: calc(100% - 2rem);"): with ui.column(): ui.button('emergency stop', on_click=lambda: self.system.field_friend.estop.set_soft_estop(True)).props('color=red') \ @@ -46,3 +46,6 @@ def content(self, devmode) -> None: ui.space() with ui.row(): rosys.automation.automation_controls(self.system.automator) + with ui.row(): + ui.button("Field Creation Wizard", on_click=lambda: FieldCreator(self.system)).tooltip("Build a field with AB-line in a few simple steps") \ + .classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;") From 98affc6aaca3b347fe6d0315be84bcef2906fc2c Mon Sep 17 00:00:00 2001 From: Lukas Baecker Date: Fri, 20 Sep 2024 17:38:03 +0200 Subject: [PATCH 02/14] 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 From e8affa844354e974d80f3cf501cdf6433c24bbb8 Mon Sep 17 00:00:00 2001 From: Lukas Baecker Date: Fri, 20 Sep 2024 17:40:43 +0200 Subject: [PATCH 03/14] hide field planner in header bar --- field_friend/interface/components/header_bar.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/field_friend/interface/components/header_bar.py b/field_friend/interface/components/header_bar.py index e6ce2bfe..ab131ecf 100644 --- a/field_friend/interface/components/header_bar.py +++ b/field_friend/interface/components/header_bar.py @@ -38,7 +38,7 @@ def __init__(self, system: 'System', right_drawer: ui.right_drawer): ui.label('Software ESTOP is active!').classes('text-white text-3xl').props('elevated') with ui.row(): - ui.link('Field planner', '/field').classes('text-white text-lg !no-underline') + # ui.link('Field planner', '/field').classes('text-white text-lg !no-underline') # ui.link('Path recorder', '/path').classes('text-white text-lg !no-underline') ui.link('Circle Sight', '/monitor').classes('text-white text-lg !no-underline') # ui.link('Development', '/dev').classes('text-white text-lg !no-underline') From 80c927226983a67df61041fdb2f8cf99194297d5 Mon Sep 17 00:00:00 2001 From: Lukas Baecker Date: Mon, 23 Sep 2024 17:49:20 +0200 Subject: [PATCH 04/14] field optimizations, and field in leaflet map cleanup --- field_friend/automations/field.py | 22 +++---- field_friend/automations/field_provider.py | 2 - .../interface/components/field_creator.py | 6 +- .../interface/components/leaflet_map.py | 66 ++++++++----------- .../localization/point_transformation.py | 37 ----------- 5 files changed, 36 insertions(+), 97 deletions(-) diff --git a/field_friend/automations/field.py b/field_friend/automations/field.py index 5c75bacd..357cc72e 100644 --- a/field_friend/automations/field.py +++ b/field_friend/automations/field.py @@ -7,9 +7,7 @@ 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 +from field_friend.localization import GeoPoint, GeoPointCollection, reference @dataclass(slots=True, kw_only=True) @@ -28,8 +26,6 @@ def reversed(self): points=list(reversed(self.points)), ) - return self # Add this line to fix the missing return statement error - def line_segment(self) -> rosys.geometry.LineSegment: return rosys.geometry.LineSegment(point1=self.points[0].cartesian(), point2=self.points[-1].cartesian()) @@ -49,25 +45,23 @@ class Field(GeoPointCollection): def outline(self) -> list[GeoPoint]: assert self.first_row_start is not None assert self.first_row_end is not None - 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) + ab_line_cartesian = LineString([self.first_row_start.cartesian().tuple, self.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(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(first_row_end_cartesian) - outline_unbuffered.append(first_row_start_cartesian) + outline_unbuffered.append(self.first_row_end.cartesian()) + outline_unbuffered.append(self.first_row_start.cartesian()) outline_polygon = Polygon([p.tuple for p in outline_unbuffered]) bufferd_polygon = outline_polygon.buffer( 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(cartesian_to_wgs84(self.first_row_start, Point(x=p[0], y=p[1]))) + outline.append(self.first_row_start.shifted(Point(x=p[0], y=p[1]))) return outline @ property @@ -92,7 +86,7 @@ def rows(self) -> list[Row]: assert self.first_row_start is not None assert self.first_row_end is not None 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) + first_row_end_cartesian = self.first_row_end.cartesian() ab_line_cartesian = LineString([first_row_start_cartesian.tuple, first_row_end_cartesian.tuple]) rows = [] for i in range(int(self.row_number)+1): @@ -100,7 +94,7 @@ def rows(self) -> list[Row]: offset_row_coordinated = offset_curve(ab_line_cartesian, -offset).coords row_points: list[GeoPoint] = [] for point in offset_row_coordinated: - row_points.append(cartesian_to_wgs84(self.first_row_start, Point(x=point[0], y=point[1]))) + row_points.append(self.first_row_start.shifted(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/automations/field_provider.py b/field_friend/automations/field_provider.py index b2e4ba66..33fcd2b3 100644 --- a/field_friend/automations/field_provider.py +++ b/field_friend/automations/field_provider.py @@ -37,7 +37,6 @@ def backup(self) -> dict: } def restore(self, data: dict[str, Any]) -> None: - print('tes') fields_data = data.get('fields', []) rosys.persistence.replace_list(self.fields, Field, fields_data) @@ -50,7 +49,6 @@ def restore(self, data: dict[str, Any]) -> None: for j, row in enumerate(rows): for point in row.get('points_wgs84', []): f.rows[j].points.append(GeoPoint(lat=point[0], long=point[1])) - print(f'fields 🛑: {self.fields[0].rows[10]}') def invalidate(self) -> None: self.request_backup() diff --git a/field_friend/interface/components/field_creator.py b/field_friend/interface/components/field_creator.py index b3a03f8a..aafff60e 100644 --- a/field_friend/interface/components/field_creator.py +++ b/field_friend/interface/components/field_creator.py @@ -3,7 +3,6 @@ import rosys from nicegui import ui - from field_friend.automations import Field from field_friend.automations.navigation import StraightLineNavigation from field_friend.interface.components.monitoring import CameraPosition @@ -103,8 +102,6 @@ def confirm_geometry(self) -> None: ui.label('No RTK fix available.').classes('text-red') self.first_row_end = self.gnss.current.location assert self.first_row_end is not None - # TODO we do not need a last row - # we now only work with first row, row_spacing and numbers of rows self.headline.text = 'Confirm Geometry' self.content.clear() with self.content: @@ -113,12 +110,13 @@ def confirm_geometry(self) -> None: ui.label(f'First Row End: {self.first_row_end}').classes('text-lg') ui.label(f'Row Spacing: {self.row_spacing} m').classes('text-lg') ui.label(f'Number of Rows: {self.row_number}').classes('text-lg') + with ui.row().classes('items-center'): ui.button('Cancel', on_click=self.dialog.close).props('color=red') self.next = self._apply 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 = [] # currently only a single field is 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/interface/components/leaflet_map.py b/field_friend/interface/components/leaflet_map.py index 6e069c16..dc6e5beb 100644 --- a/field_friend/interface/components/leaflet_map.py +++ b/field_friend/interface/components/leaflet_map.py @@ -56,11 +56,12 @@ def __init__(self, system: 'System', draw_tools: bool) -> None: self.drawn_marker = None self.obstacle_layers: list = [] self.row_layers: list = [] - self._active_field: str | None = None - self.active_object: Active_object | None = None + self.active_field: str | None = None + self.set_active_field() self.update_layers() - self.field_provider.FIELDS_CHANGED.register(self.update_layers) self.zoom_to_robot() + self.field_provider.FIELDS_CHANGED.register(self.set_active_field) + self.field_provider.FIELDS_CHANGED.register(self.update_layers) def handle_draw(e: events.GenericEventArguments): if e.args['layerType'] == 'marker': @@ -107,7 +108,6 @@ def buttons(self) -> None: ui.button(icon='my_location', on_click=self.zoom_to_robot).props('dense flat') \ .tooltip('Center map on robot position').classes('ml-0') ui.button(on_click=self.zoom_to_field) \ - .bind_enabled_from(self, 'active_field') \ .props('icon=polyline dense flat') \ .tooltip('center map on field boundaries').classes('ml-0') @@ -115,15 +115,15 @@ def abort_point_drawing(self, dialog) -> None: self.on_dialog_close() dialog.close() - def add_point_active_object(self, latlon, dialog) -> None: - dialog.close() - self.on_dialog_close() - if self.active_object and self.active_object.get("object") is not None: - self.active_object.get("object").points.append(GeoPoint.from_list(latlon)) - self.field_provider.invalidate() - self.update_layers() - else: - ui.notify("No object selected. Point could not be added to the void.") + # def add_point_active_object(self, latlon, dialog) -> None: + # dialog.close() + # self.on_dialog_close() + # if self.active_object and self.active_object.get("object") is not None: + # self.active_object.get("object").points.append(GeoPoint.from_list(latlon)) + # self.field_provider.invalidate() + # self.update_layers() + # else: + # ui.notify("No object selected. Point could not be added to the void.") def update_layers(self) -> None: for layer in self.field_layers: @@ -133,7 +133,7 @@ def update_layers(self) -> None: for field in self.field_provider.fields: color = '#6E93D6' if field.id == self.active_field else '#999' self.field_layers.append(self.m.generic_layer(name="polygon", - args=[field.points_as_tuples, {'color': color}])) + args=[field.outline_as_tuples, {'color': color}])) current_field: Field | None = self.field_provider.get_field(self.active_field) for layer in self.obstacle_layers: self.m.remove_layer(layer) @@ -141,30 +141,13 @@ def update_layers(self) -> None: for layer in self.row_layers: self.m.remove_layer(layer) self.row_layers = [] - if len(self.field_provider.fields) > 0: - print(self.field_provider.fields[0].outline_as_tuples) - for row in self.field_provider.fields[0].rows: - self.m.generic_layer(name="polyline", args=[ - row.points_as_tuples, {'color': '#6E93D6'}]) - self.m.generic_layer(name="polygon", args=[ - self.field_provider.fields[0].outline_as_tuples, {'color': '#6E93D6'}]) - if current_field is None: - return - for obstacle in current_field.obstacles: - self.obstacle_layers.append(self.m.generic_layer(name="polygon", - args=[obstacle.points_as_tuples, {'color': '#C10015'}])) - for row in current_field.rows: - self.row_layers.append(self.m.generic_layer(name="polyline", - args=[row.points_as_tuples, {'color': '#F2C037'}])) - - @property - def active_field(self) -> str | None: - return self._active_field - - @active_field.setter - def active_field(self, field_id: str | None) -> None: - self._active_field = field_id - self.update_layers() + if current_field is not None: + for obstacle in current_field.obstacles: + self.obstacle_layers.append(self.m.generic_layer(name="polygon", + args=[obstacle.points_as_tuples, {'color': '#C10015'}])) + for row in current_field.rows: + self.row_layers.append(self.m.generic_layer(name="polyline", + args=[row.points_as_tuples, {'color': '#F2C037'}])) def update_robot_position(self, position: GeoPoint, dialog=None) -> None: if dialog: @@ -184,10 +167,10 @@ def zoom_to_robot(self) -> None: self.m.set_zoom(self.current_basemap.options['maxZoom'] - 1) def zoom_to_field(self) -> None: - field = self.field_provider.get_field(self.active_field) + field = self.field_provider.fields[0] if len(self.field_provider.fields) > 0 else None if field is None: return - coords = field.points_as_tuples + coords = field.outline_as_tuples center = sum(lat for lat, _ in coords) / len(coords), sum(lon for _, lon in coords) / len(coords) self.m.set_center(center) self.m.set_zoom(self.current_basemap.options['maxZoom'] - 1) # TODO use field boundaries to calculate zoom @@ -229,3 +212,6 @@ def on_dialog_close(self) -> None: if self.drawn_marker is not None: self.m.remove_layer(self.drawn_marker) self.drawn_marker = None + + def set_active_field(self) -> None: + self.active_field = self.field_provider.fields[0].id if len(self.field_provider.fields) > 0 else None diff --git a/field_friend/localization/point_transformation.py b/field_friend/localization/point_transformation.py index a9788624..a9841a16 100644 --- a/field_friend/localization/point_transformation.py +++ b/field_friend/localization/point_transformation.py @@ -20,40 +20,3 @@ 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 From e5e562e1aba107396d8db06b832cc6049a27737f Mon Sep 17 00:00:00 2001 From: Johannes-Thiel Date: Wed, 25 Sep 2024 16:02:22 +0200 Subject: [PATCH 05/14] make Point GeoPoint --- field_friend/automations/automation_watcher.py | 6 ++++-- field_friend/interface/components/field_object.py | 7 +++++-- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/field_friend/automations/automation_watcher.py b/field_friend/automations/automation_watcher.py index c3e1a148..c2965706 100644 --- a/field_friend/automations/automation_watcher.py +++ b/field_friend/automations/automation_watcher.py @@ -7,6 +7,8 @@ from shapely.geometry import Point as ShapelyPoint from shapely.geometry import Polygon as ShapelyPolygon +from field_friend.localization import GeoPoint + if TYPE_CHECKING: from system import System @@ -105,8 +107,8 @@ def try_resume(self) -> None: self.log.info('resetting resume_delay') self.resume_delay = DEFAULT_RESUME_DELAY - def start_field_watch(self, field_boundaries: list[rosys.geometry.Point]) -> None: - self.field_polygon = ShapelyPolygon([(point.x, point.y) for point in field_boundaries]) + def start_field_watch(self, field_boundaries: list[GeoPoint]) -> None: + self.field_polygon = ShapelyPolygon([point.cartesian().tuple for point in field_boundaries]) self.field_watch_active = True def stop_field_watch(self) -> None: diff --git a/field_friend/interface/components/field_object.py b/field_friend/interface/components/field_object.py index 3921419b..aab0d453 100644 --- a/field_friend/interface/components/field_object.py +++ b/field_friend/interface/components/field_object.py @@ -2,6 +2,8 @@ from nicegui.elements.scene_objects import Box, Curve, Cylinder, Extrusion, Group from rosys.geometry import Spline +from field_friend.localization import GeoPoint + from ...automations import Field, FieldProvider @@ -31,7 +33,8 @@ def create_fence(self, start, end): Box(length, height, depth).move(x=center_x, y=center_y, z=height / 2 + 0.2).with_name('field_').material('#8b4513').rotate(np.pi/2, 0, angle) Box(length, height, depth).move(x=center_x, y=center_y, - z=height / 2 + 0.5).with_name('field_').material('#8b4513').rotate(np.pi/2, 0, angle) # Convert angle from radians to degrees + # Convert angle from radians to degrees + z=height / 2 + 0.5).with_name('field_').material('#8b4513').rotate(np.pi/2, 0, angle) Box(length, height, depth).move(x=center_x, y=center_y, z=height / 2 + 0.8).with_name('field_').material('#8b4513').rotate(np.pi/2, 0, angle) Cylinder(0.1, 0.1, 1.0).move(x=start[0], y=start[1], z=0.5).with_name( @@ -45,7 +48,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('row_')] if active_field: field = active_field - outline = [[point.x, point.y] for point in field.outline] + outline = [point.cartesian().tuple for point in field.outline] if len(outline) > 1: # Make sure there are at least two points to form a segment for i in range(len(outline)): start = outline[i] From c8dc3fe55c3492198cf16cbbf899a50dcda02110 Mon Sep 17 00:00:00 2001 From: Johannes-Thiel Date: Wed, 25 Sep 2024 16:02:48 +0200 Subject: [PATCH 06/14] include robot scene --- field_friend/interface/pages/main_page.py | 1 + 1 file changed, 1 insertion(+) diff --git a/field_friend/interface/pages/main_page.py b/field_friend/interface/pages/main_page.py index 616f1b99..2a4c3c17 100644 --- a/field_friend/interface/pages/main_page.py +++ b/field_friend/interface/pages/main_page.py @@ -36,6 +36,7 @@ def content(self, devmode) -> None: operation(self.system) with ui.column().classes('h-full').style('width: calc(45% - 2rem); flex-wrap: nowrap;'): camera_card(self.system) + robot_scene(self.system, self.system.field_navigation.field) with ui.row().style("margin: 1rem; width: calc(100% - 2rem);"): with ui.column(): ui.button('emergency stop', on_click=lambda: self.system.field_friend.estop.set_soft_estop(True)).props('color=red') \ From 7e3ff79dab73df8b6d4115cc7f5b8b7ff4058438 Mon Sep 17 00:00:00 2001 From: Johannes-Thiel Date: Wed, 25 Sep 2024 16:03:16 +0200 Subject: [PATCH 07/14] improve Field navigation --- .../navigation/a_b_field_navigation.py | 49 ++++++++++++++----- .../navigation/follow_crops_navigation.py | 3 -- field_friend/system.py | 4 +- 3 files changed, 38 insertions(+), 18 deletions(-) diff --git a/field_friend/automations/navigation/a_b_field_navigation.py b/field_friend/automations/navigation/a_b_field_navigation.py index 9b593f76..5a68e6af 100644 --- a/field_friend/automations/navigation/a_b_field_navigation.py +++ b/field_friend/automations/navigation/a_b_field_navigation.py @@ -32,6 +32,7 @@ def __init__(self, system: 'System', tool: Implement) -> None: self.state = self.State.APPROACHING_ROW_START self.field: Field | None = None + self.rows: list[Row] | None = None self.row_index = 0 self.start_point: Point | None = None self.end_point: Point | None = None @@ -39,16 +40,18 @@ def __init__(self, system: 'System', tool: Implement) -> None: @property def current_row(self) -> Row: assert self.field - return self.field.rows[self.row_index] + return self.rows[self.row_index] async def prepare(self) -> bool: await super().prepare() + self.field = self.field_provider.fields[0] if self.field is None: rosys.notify('No field selected', 'negative') return False if not self.field.rows: rosys.notify('No rows available', 'negative') return False + self.rows = self.field.rows if self.gnss.device is None: rosys.notify('GNSS is not available', 'negative') return False @@ -56,13 +59,10 @@ async def prepare(self) -> bool: if not len(row.points) >= 2: rosys.notify(f'Row {idx} on field {self.field.name} has not enough points', 'negative') return False - self.current_row = self.get_nearest_row() - self.row_index = self.field.rows.index(row) + row = self.get_nearest_row() + self.row_index = self.rows.index(row) if self.state == self.State.FIELD_COMPLETED: self.clear() - if self.state == self.State.FOLLOWING_ROW: - if self.current_row.line_segment().distance(self.odometer.prediction.point) > 0.1: - self.clear() else: self.plant_provider.clear() @@ -90,27 +90,29 @@ async def finish(self) -> None: def get_nearest_row(self) -> Row: assert self.field is not None assert self.gnss.device is not None - row = min(self.field.rows, key=lambda r: r.line_segment().line.foot_point( + row = min(self.rows, key=lambda r: r.line_segment().line.foot_point( self.odometer.prediction.point).distance(self.odometer.prediction.point)) self.log.info(f'Nearest row is {row.name}') - self.row_index = self.field.rows.index(row) + self.row_index = self.rows.index(row) return row def set_start_and_end_points(self): assert self.field is not None self.start_point = None self.end_point = None - relative_point_0 = self.odometer.prediction.relative_point(self.current_row.points[0].cartesian()) - relative_point_1 = self.odometer.prediction.relative_point(self.current_row.points[-1].cartesian()) - if relative_point_0.x < 0 or relative_point_0.x < relative_point_1.x: + relative_point_0 = self.odometer.prediction.distance(self.current_row.points[0].cartesian()) + relative_point_1 = self.odometer.prediction.distance(self.current_row.points[-1].cartesian()) + self.log.info(f'Relative point 0: {relative_point_0} Relative point 1: {relative_point_1}') + if relative_point_0 < relative_point_1: self.start_point = self.current_row.points[0].cartesian() self.end_point = self.current_row.points[-1].cartesian() - elif relative_point_1.x < 0 or relative_point_1.x < relative_point_0.x: + elif relative_point_1 < relative_point_0: self.start_point = self.current_row.points[-1].cartesian() self.end_point = self.current_row.points[0].cartesian() + self.log.info(f'Start point: {self.start_point} End point: {self.end_point}') def update_target(self) -> None: - pass + self.origin = self.odometer.prediction.point async def _drive(self, distance: float) -> None: assert self.field is not None @@ -121,14 +123,18 @@ async def _drive(self, distance: float) -> None: self.state = self.State.FOLLOWING_ROW self.log.info(f'Following "{self.current_row.name}"...') self.plant_provider.clear() + self.log.info(f'State is {self.state}...') if self.state == self.State.FOLLOWING_ROW: if not self.implement.is_active: await self.implement.activate() if self.odometer.prediction.point.distance(self.end_point) >= 0.1: + super().update_target() await super()._drive(distance) else: # TODO: drive forward to clear the row and not drive over the plants await self.implement.deactivate() self.state = self.State.ROW_COMPLETED + await self._drive_forward(0.5) + self.log.info(f'State is {self.state}...') if self.state == self.State.ROW_COMPLETED: if self.current_row == self.field.rows[-1]: self.state = self.State.FIELD_COMPLETED @@ -137,13 +143,29 @@ async def _drive(self, distance: float) -> None: self.row_index += 1 self.state = self.State.APPROACHING_ROW_START + async def _drive_forward(self, distance: float) -> None: + target = Pose(x=self.odometer.prediction.point.x+distance,y=self.odometer.prediction.point.y, yaw=self.odometer.prediction.direction) + hook_offset = rosys.geometry.Point(x=self.driver.parameters.hook_offset, y=0) + carrot_offset = rosys.geometry.Point(x=self.driver.parameters.carrot_offset, y=0) + target_point = target.transform(carrot_offset) + hook = self.odometer.prediction.transform(hook_offset) + turn_angle = rosys.helpers.angle(self.odometer.prediction.yaw, hook.direction(target_point)) + curvature = np.tan(turn_angle) / hook_offset.x + if curvature != 0 and abs(1 / curvature) < self.driver.parameters.minimum_turning_radius: + curvature = (-1 if curvature < 0 else 1) / self.driver.parameters.minimum_turning_radius + with self.driver.parameters.set(linear_speed_limit=self.linear_speed_limit, angular_speed_limit=self.angular_speed_limit): + await self.driver.wheels.drive(*self.driver._throttle(1.0, curvature)) # pylint: disable=protected-access + await self.driver.wheels.stop() + async def _drive_to_row(self): self.log.info(f'Driving to "{self.current_row.name}"...') assert self.field target = self.start_point direction = target.direction(self.end_point) end_pose = Pose(x=target.x, y=target.y, yaw=direction) + self.log.info(f'Driving to {end_pose} from {self.odometer.prediction}...') spline = Spline.from_poses(self.odometer.prediction, end_pose) + self.log.info(f'Driving spline {spline}...') await self.driver.drive_spline(spline) self.log.info(f'Arrived at row {self.current_row.name} starting at {target}') @@ -162,6 +184,7 @@ def restore(self, data: dict[str, Any]) -> None: self.state = self.State[data.get('state', self.State.APPROACHING_ROW_START.name)] def clear(self) -> None: + return self.state = self.State.APPROACHING_ROW_START self.row_index = 0 diff --git a/field_friend/automations/navigation/follow_crops_navigation.py b/field_friend/automations/navigation/follow_crops_navigation.py index 34e8741f..e085dbe7 100644 --- a/field_friend/automations/navigation/follow_crops_navigation.py +++ b/field_friend/automations/navigation/follow_crops_navigation.py @@ -57,9 +57,6 @@ async def _drive(self, distance: float) -> None: # flip if it is pointing backwards if np.abs(yaw - self.odometer.prediction.yaw) > math.pi / 2: yaw = yaw + math.pi - if np.abs(yaw - self.odometer.prediction.yaw) > 0.2: - self.log.error(f'Yaw difference is too high: {np.abs(yaw - self.odometer.prediction.yaw)}') - return fitted_line = rosys.geometry.Line(a=m, b=-1, c=c) closest_point = fitted_line.foot_point(self.odometer.prediction.point) target = rosys.geometry.Pose(x=closest_point.x, y=closest_point.y, yaw=yaw) diff --git a/field_friend/system.py b/field_friend/system.py index 90badfdb..553e907c 100644 --- a/field_friend/system.py +++ b/field_friend/system.py @@ -151,8 +151,8 @@ def watch_robot() -> None: self.automation_watcher = AutomationWatcher(self) self.monitoring = Recorder(self) self.timelapse_recorder = rosys.analysis.TimelapseRecorder() - self.timelapse_recorder.frame_info_builder = lambda _: f'{self.version}, { - self.current_navigation.name}, tags: {", ".join(self.plant_locator.tags)}' + self.timelapse_recorder.frame_info_builder = lambda _: f'''{self.version},\ { + self.current_navigation.name}, tags: {", ".join(self.plant_locator.tags)}''' rosys.NEW_NOTIFICATION.register(self.timelapse_recorder.notify) rosys.on_startup(self.timelapse_recorder.compress_video) # NOTE: cleanup JPEGs from before last shutdown self.field_navigation = RowsOnFieldNavigation(self, self.monitoring) From 78ab854eb015a15dd212790a4cd7f76c6f6121e6 Mon Sep 17 00:00:00 2001 From: Pascal Schade Date: Fri, 27 Sep 2024 00:12:32 +0200 Subject: [PATCH 08/14] wip: remove fieldfriend from leaflet --- field_friend/interface/components/leaflet_map.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/field_friend/interface/components/leaflet_map.py b/field_friend/interface/components/leaflet_map.py index dc6e5beb..40f9ad38 100644 --- a/field_friend/interface/components/leaflet_map.py +++ b/field_friend/interface/components/leaflet_map.py @@ -155,8 +155,8 @@ def update_robot_position(self, position: GeoPoint, dialog=None) -> None: dialog.close() self.gnss.relocate(position) self.robot_marker = self.robot_marker or self.m.marker(latlng=position.tuple) - icon = 'L.icon({iconUrl: "assets/robot_position_side.png", iconSize: [50,50], iconAnchor:[20,20]})' - self.robot_marker.run_method(':setIcon', icon) + # icon = 'L.icon({iconUrl: "assets/robot_position_side.png", iconSize: [50,50], iconAnchor:[20,20]})' + # self.robot_marker.run_method(':setIcon', icon) self.robot_marker.move(*position.tuple) def zoom_to_robot(self) -> None: From 18c9d6ee2bf69b81bedb1eec29efa31a7bdee0d6 Mon Sep 17 00:00:00 2001 From: Pascal Schade Date: Fri, 27 Sep 2024 00:15:52 +0200 Subject: [PATCH 09/14] rework fieldnavigation --- .../automations/navigation/__init__.py | 4 +- .../navigation/a_b_field_navigation.py | 229 -------------- .../navigation/field_navigation.py | 285 ++++++++++++++++++ field_friend/interface/pages/dev_page.py | 2 + field_friend/system.py | 8 +- 5 files changed, 293 insertions(+), 235 deletions(-) delete mode 100644 field_friend/automations/navigation/a_b_field_navigation.py create mode 100644 field_friend/automations/navigation/field_navigation.py diff --git a/field_friend/automations/navigation/__init__.py b/field_friend/automations/navigation/__init__.py index 032c7c51..6cabac1f 100644 --- a/field_friend/automations/navigation/__init__.py +++ b/field_friend/automations/navigation/__init__.py @@ -1,7 +1,7 @@ -from .a_b_field_navigation import ABFieldNavigatoin from .a_b_line_navigation import ABLineNavigation from .coverage_navigation import CoverageNavigation +from .field_navigation import FieldNavigation from .follow_crops_navigation import FollowCropsNavigation from .navigation import Navigation, WorkflowException from .row_on_field_navigation import RowsOnFieldNavigation @@ -15,5 +15,5 @@ 'FollowCropsNavigation', 'CoverageNavigation', 'ABLineNavigation', - 'ABFieldNavigatoin', + 'FieldNavigation', ] diff --git a/field_friend/automations/navigation/a_b_field_navigation.py b/field_friend/automations/navigation/a_b_field_navigation.py deleted file mode 100644 index 5a68e6af..00000000 --- a/field_friend/automations/navigation/a_b_field_navigation.py +++ /dev/null @@ -1,229 +0,0 @@ -import math -from enum import Enum, auto -from typing import TYPE_CHECKING, Any - -import rosys -from nicegui import ui -from rosys.geometry import Point, Pose, Spline - -from ..field import Field, Row -from ..implements import Implement, WeedingImplement -from .follow_crops_navigation import FollowCropsNavigation - -if TYPE_CHECKING: - from system import System - - -class ABFieldNavigatoin(FollowCropsNavigation): - class State(Enum): - APPROACHING_ROW_START = auto() - FOLLOWING_ROW = auto() - ROW_COMPLETED = auto() - FIELD_COMPLETED = auto() - - def __init__(self, system: 'System', tool: Implement) -> None: - super().__init__(system, tool) - - self.name = 'Field Navigation' - self.gnss = system.gnss - self.bms = system.field_friend.bms - self.automation_watcher = system.automation_watcher - self.field_provider = system.field_provider - - self.state = self.State.APPROACHING_ROW_START - self.field: Field | None = None - self.rows: list[Row] | None = None - self.row_index = 0 - self.start_point: Point | None = None - self.end_point: Point | None = None - - @property - def current_row(self) -> Row: - assert self.field - return self.rows[self.row_index] - - async def prepare(self) -> bool: - await super().prepare() - self.field = self.field_provider.fields[0] - if self.field is None: - rosys.notify('No field selected', 'negative') - return False - if not self.field.rows: - rosys.notify('No rows available', 'negative') - return False - self.rows = self.field.rows - if self.gnss.device is None: - rosys.notify('GNSS is not available', 'negative') - return False - for idx, row in enumerate(self.field.rows): - if not len(row.points) >= 2: - rosys.notify(f'Row {idx} on field {self.field.name} has not enough points', 'negative') - return False - row = self.get_nearest_row() - self.row_index = self.rows.index(row) - if self.state == self.State.FIELD_COMPLETED: - self.clear() - else: - self.plant_provider.clear() - - await rosys.sleep(1) # wait for GNSS to update - self.automation_watcher.start_field_watch(self.field.outline) - - # determine start and end point - self.set_start_and_end_points() - assert self.start_point is not None - assert self.end_point is not None - self.log.info(f'Start point: {self.start_point} End point: {self.end_point}') - - self.log.info(f'Activating {self.implement.name}...') - await self.implement.activate() - return True - - def _should_finish(self) -> bool: - return self.state == self.State.FIELD_COMPLETED - - async def finish(self) -> None: - await super().finish() - self.automation_watcher.stop_field_watch() - await self.implement.deactivate() - - def get_nearest_row(self) -> Row: - assert self.field is not None - assert self.gnss.device is not None - row = min(self.rows, key=lambda r: r.line_segment().line.foot_point( - self.odometer.prediction.point).distance(self.odometer.prediction.point)) - self.log.info(f'Nearest row is {row.name}') - self.row_index = self.rows.index(row) - return row - - def set_start_and_end_points(self): - assert self.field is not None - self.start_point = None - self.end_point = None - relative_point_0 = self.odometer.prediction.distance(self.current_row.points[0].cartesian()) - relative_point_1 = self.odometer.prediction.distance(self.current_row.points[-1].cartesian()) - self.log.info(f'Relative point 0: {relative_point_0} Relative point 1: {relative_point_1}') - if relative_point_0 < relative_point_1: - self.start_point = self.current_row.points[0].cartesian() - self.end_point = self.current_row.points[-1].cartesian() - elif relative_point_1 < relative_point_0: - self.start_point = self.current_row.points[-1].cartesian() - self.end_point = self.current_row.points[0].cartesian() - self.log.info(f'Start point: {self.start_point} End point: {self.end_point}') - - def update_target(self) -> None: - self.origin = self.odometer.prediction.point - - async def _drive(self, distance: float) -> None: - assert self.field 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 - self.set_start_and_end_points() - await self._drive_to_row() - self.state = self.State.FOLLOWING_ROW - self.log.info(f'Following "{self.current_row.name}"...') - self.plant_provider.clear() - self.log.info(f'State is {self.state}...') - if self.state == self.State.FOLLOWING_ROW: - if not self.implement.is_active: - await self.implement.activate() - if self.odometer.prediction.point.distance(self.end_point) >= 0.1: - super().update_target() - await super()._drive(distance) - else: # TODO: drive forward to clear the row and not drive over the plants - await self.implement.deactivate() - self.state = self.State.ROW_COMPLETED - await self._drive_forward(0.5) - self.log.info(f'State is {self.state}...') - if self.state == self.State.ROW_COMPLETED: - if self.current_row == self.field.rows[-1]: - self.state = self.State.FIELD_COMPLETED - await rosys.sleep(0.1) # wait for base class to finish navigation - else: - self.row_index += 1 - self.state = self.State.APPROACHING_ROW_START - - async def _drive_forward(self, distance: float) -> None: - target = Pose(x=self.odometer.prediction.point.x+distance,y=self.odometer.prediction.point.y, yaw=self.odometer.prediction.direction) - hook_offset = rosys.geometry.Point(x=self.driver.parameters.hook_offset, y=0) - carrot_offset = rosys.geometry.Point(x=self.driver.parameters.carrot_offset, y=0) - target_point = target.transform(carrot_offset) - hook = self.odometer.prediction.transform(hook_offset) - turn_angle = rosys.helpers.angle(self.odometer.prediction.yaw, hook.direction(target_point)) - curvature = np.tan(turn_angle) / hook_offset.x - if curvature != 0 and abs(1 / curvature) < self.driver.parameters.minimum_turning_radius: - curvature = (-1 if curvature < 0 else 1) / self.driver.parameters.minimum_turning_radius - with self.driver.parameters.set(linear_speed_limit=self.linear_speed_limit, angular_speed_limit=self.angular_speed_limit): - await self.driver.wheels.drive(*self.driver._throttle(1.0, curvature)) # pylint: disable=protected-access - await self.driver.wheels.stop() - - async def _drive_to_row(self): - self.log.info(f'Driving to "{self.current_row.name}"...') - assert self.field - target = self.start_point - direction = target.direction(self.end_point) - end_pose = Pose(x=target.x, y=target.y, yaw=direction) - self.log.info(f'Driving to {end_pose} from {self.odometer.prediction}...') - spline = Spline.from_poses(self.odometer.prediction, end_pose) - self.log.info(f'Driving spline {spline}...') - await self.driver.drive_spline(spline) - self.log.info(f'Arrived at row {self.current_row.name} starting at {target}') - - def backup(self) -> dict: - return super().backup() | { - 'field_id': self.field.id if self.field else None, - 'row_index': self.row_index, - 'state': self.state.name, - } - - def restore(self, data: dict[str, Any]) -> None: - super().restore(data) - field_id = data.get('field_id', self.field_provider.fields[0].id if self.field_provider.fields else None) - self.field = self.field_provider.get_field(field_id) - self.row_index = data.get('row_index', 0) - self.state = self.State[data.get('state', self.State.APPROACHING_ROW_START.name)] - - def clear(self) -> None: - return - self.state = self.State.APPROACHING_ROW_START - self.row_index = 0 - - def settings_ui(self) -> None: - super().settings_ui() - field_selection = ui.select( - {f.id: f.name for f in self.field_provider.fields if len(f.rows) >= 1 and len(f.points) >= 3}, - on_change=lambda args: self._set_field(args.value), - label='Field')\ - .classes('w-32') \ - .tooltip('Select the field to work on') - field_selection.bind_value_from(self, 'field', lambda f: f.id if f else None) - - def _set_field(self, field_id: str) -> None: - field = self.field_provider.get_field(field_id) - if field is not None: - self.field = field - if isinstance(self.implement, WeedingImplement): - self.implement.cultivated_crop = field.crop - self.clear() - else: - if isinstance(self.implement, WeedingImplement): - self.implement.cultivated_crop = None - - def create_simulation(self) -> None: - self.detector.simulated_objects.clear() - if self.field is None: - return - for row in self.field.rows: - if len(row.points) < 2: - continue - cartesian = row.cartesian() - start = cartesian[0] - end = cartesian[-1] - length = start.distance(end) - self.log.info(f'Adding plants from {start} to {end} (length {length:.1f} m)') - crop_count = length / 0.15 - for i in range(int(crop_count)): - p = start.interpolate(end, (0.15 * i) / length) - p3d = rosys.geometry.Point3d(x=p.x, y=p.y, z=0) - plant = rosys.vision.SimulatedObject(category_name='maize', position=p3d) - self.detector.simulated_objects.append(plant) diff --git a/field_friend/automations/navigation/field_navigation.py b/field_friend/automations/navigation/field_navigation.py new file mode 100644 index 00000000..cf037513 --- /dev/null +++ b/field_friend/automations/navigation/field_navigation.py @@ -0,0 +1,285 @@ +from enum import Enum, auto +from random import randint +from typing import TYPE_CHECKING, Any + +import numpy as np +import rosys +from nicegui import ui +from rosys.geometry import Point, Pose, Spline + +from ..field import Field, Row +from ..implements import Implement, WeedingImplement +from .follow_crops_navigation import FollowCropsNavigation +from .straight_line_navigation import StraightLineNavigation + +if TYPE_CHECKING: + from system import System + + +class State(Enum): + APPROACHING_ROW_START = auto() + FOLLOWING_ROW = auto() + ROW_COMPLETED = auto() + CLEAR_ROW = auto() + FIELD_COMPLETED = auto() + + +class FieldNavigation(FollowCropsNavigation): + CLEAR_ROW_DISTANCE = 0.5 + + def __init__(self, system: 'System', implement: Implement) -> None: + super().__init__(system, implement) + + self.name = 'Field Navigation' + self.gnss = system.gnss + self.bms = system.field_friend.bms + self.automation_watcher = system.automation_watcher + self.field_provider = system.field_provider + + self._state = State.APPROACHING_ROW_START + self.row_index = 0 + self.start_point: Point | None = None + self.end_point: Point | None = None + + self.field: Field | None = None + self.clear_row_distance: float = self.CLEAR_ROW_DISTANCE + + @property + def current_row(self) -> Row: + assert self.field + return self.field.rows[self.row_index] + + async def prepare(self) -> bool: + await super().prepare() + # self.field = self.field_provider.fields[0] + if self.field is None: + rosys.notify('No field selected', 'negative') + return False + if not self.field.rows: + rosys.notify('No rows available', 'negative') + return False + if self.gnss.device is None: + rosys.notify('GNSS is not available', 'negative') + return False + for idx, row in enumerate(self.field.rows): + if not len(row.points) >= 2: + rosys.notify(f'Row {idx} on field {self.field.name} has not enough points', 'negative') + return False + # row = self.get_nearest_row() + # self.row_index = self.field.rows.index(row) + self.row_index = 0 + # if self._state == State.FIELD_COMPLETED: + # self.clear() + # else: + self._state = State.APPROACHING_ROW_START + self.plant_provider.clear() + + await rosys.sleep(1) # wait for GNSS to update + # self.automation_watcher.start_field_watch(self.field.outline) + + self.log.info(f'Activating {self.implement.name}...') + await self.implement.activate() + return True + + def _should_finish(self) -> bool: + return self._state == State.FIELD_COMPLETED + + async def finish(self) -> None: + await super().finish() + self.automation_watcher.stop_field_watch() + await self.implement.deactivate() + + def get_nearest_row(self) -> Row: + # currently not used, starts on row 0 + # assert self.field is not None + # assert self.gnss.device is not None + # row = min(self.field.rows, key=lambda r: r.line_segment().line.foot_point( + # self.odometer.prediction.point).distance(self.odometer.prediction.point)) + # self.log.info(f'Nearest row is {row.name}') + # self.row_index = self.field.rows.index(row) + # return row + return Row(id='dummy', name='dummy', points=[]) + + def set_start_and_end_points(self): + assert self.field is not None + self.start_point = None + self.end_point = None + relative_point_0 = self.odometer.prediction.distance(self.current_row.points[0].cartesian()) + relative_point_1 = self.odometer.prediction.distance(self.current_row.points[-1].cartesian()) + # self.log.info(f'Relative point 0: {relative_point_0} Relative point 1: {relative_point_1}') + if relative_point_0 < relative_point_1: + self.start_point = self.current_row.points[0].cartesian() + self.end_point = self.current_row.points[-1].cartesian() + elif relative_point_1 < relative_point_0: + self.start_point = self.current_row.points[-1].cartesian() + self.end_point = self.current_row.points[0].cartesian() + # self.log.info(f'Start point: {self.start_point} End point: {self.end_point}') + + def update_target(self) -> None: + self.origin = self.odometer.prediction.point + if self.end_point is None: + return + self.target = self.end_point + + async def _drive(self, distance: float) -> None: + assert self.field is not None + + # self.update_target() + if self._state == State.APPROACHING_ROW_START: + self._state = await self._run_approaching_row_start() + elif self._state == State.FOLLOWING_ROW: + self._state = await self._run_following_row(distance) + elif self._state == State.CLEAR_ROW: + self._state = await self._run_clear_row() + elif self._state == State.ROW_COMPLETED: + self._state = await self._run_row_completed() + + async def _run_approaching_row_start(self) -> State: + # TODO only drive to row if we are not on any rows and near the row start + self.set_start_and_end_points() + await self._wait_for_gnss() + await self._drive_to_row() + await self._wait_for_gnss() + # self.log.info(f'Following "{self.current_row.name}"...') + self.plant_provider.clear() + self.set_start_and_end_points() + self.update_target() + return State.FOLLOWING_ROW + + async def _run_following_row(self, distance: float) -> State: + if not self.implement.is_active: + await self.implement.activate() + # self.update_target() + # if self.odometer.prediction.point.distance(self.target) < 0.05: + end_pose = rosys.geometry.Pose(x=self.target.x, y=self.target.y, yaw=self.origin.direction(self.target), time=0) + relative_point = end_pose.relative_point(self.odometer.prediction.point).x + bool_check = relative_point > 0 + self.log.info(f'End pose: {end_pose} - relative_point: {relative_point} - Bool check: {bool_check}') + # if StraightLineNavigation._should_finish(self): # type: disable=W0212 + if bool_check: + await self.implement.deactivate() + return State.CLEAR_ROW + await super()._drive(distance) + return State.FOLLOWING_ROW + + async def _run_clear_row(self) -> State: + target = self.odometer.prediction.transform_pose(rosys.geometry.Pose(x=self.clear_row_distance, y=0)) + await self._drive_towards_target(0.02, target) + await self._wait_for_gnss() + return State.ROW_COMPLETED + + async def _run_row_completed(self) -> State: + assert self.field + next_state: State = State.ROW_COMPLETED + if self.current_row == self.field.rows[-1]: + next_state = State.FIELD_COMPLETED + await rosys.sleep(0.1) # wait for base class to finish navigation + else: + self.row_index += 1 + next_state = State.APPROACHING_ROW_START + + # TODO: remove later, when any direction is possible + if self.row_index >= len(self.field.rows): + next_state = State.FIELD_COMPLETED + return next_state + + async def _wait_for_gnss(self, waiting_time: float = 2.0) -> None: + self.gnss.is_paused = False + # TODO: dont wait so long + await rosys.sleep(waiting_time) + await self.gnss.update_robot_pose() + self.gnss.is_paused = True + + async def _drive_to_row(self) -> None: + # self.log.info(f'Driving to "{self.current_row.name}"...') + assert self.field + assert self.start_point + assert self.end_point + direction = self.start_point.direction(self.end_point) + row_start_pose = Pose(x=self.start_point.x, y=self.start_point.y, yaw=direction) + target_pose = row_start_pose.transform_pose(Pose(x=-self.clear_row_distance)) + # self.log.info(f'Driving to {end_pose} from {self.odometer.prediction}...') + spline = Spline.from_poses(self.odometer.prediction, target_pose) + # self.log.info(f'Driving spline {spline}...') + await self.driver.drive_spline(spline) + # self.log.info(f'Arrived at row {self.current_row.name} starting at {target}') + + def backup(self) -> dict: + return super().backup() | { + 'field_id': self.field.id if self.field else None, + 'row_index': self.row_index, + 'state': self._state.name, + 'clear_row_distance': self.clear_row_distance, + } + + def restore(self, data: dict[str, Any]) -> None: + super().restore(data) + field_id = data.get('field_id', self.field_provider.fields[0].id if self.field_provider.fields else None) + self.field = self.field_provider.get_field(field_id) + self.row_index = data.get('row_index', 0) + self._state = State[data.get('state', State.APPROACHING_ROW_START.name)] + self.clear_row_distance = data.get('clear_row_distance', self.CLEAR_ROW_DISTANCE) + + def clear(self) -> None: + return + self._state = State.APPROACHING_ROW_START + self.row_index = 0 + + def settings_ui(self) -> None: + super().settings_ui() + field_selection = ui.select( + {f.id: f.name for f in self.field_provider.fields if len(f.rows) >= 1}, + on_change=lambda args: self._set_field(args.value), + label='Field')\ + .classes('w-32') \ + .tooltip('Select the field to work on') + field_selection.bind_value_from(self, 'field', lambda f: f.id if f else None) + ui.number('Clear Row Distance', step=0.05, min=0.01, max=5.0, format='%.2f', on_change=self.request_backup) \ + .props('dense outlined') \ + .classes('w-24') \ + .bind_value(self, 'clear_row_distance') \ + .tooltip(f'Safety distance to row in m (default: {self.CLEAR_ROW_DISTANCE:.2f})') + + def developer_ui(self) -> None: + # super().developer_ui() + ui.label('').bind_text_from(self, '_state', lambda state: f'State: {state.name}') + ui.label('').bind_text_from(self, 'row_index', lambda row_index: f'Row Index: {row_index}') + ui.label('').bind_text_from(self, 'start_point', lambda start_point: f'Start Point: {start_point}') + ui.label('').bind_text_from(self, 'end_point', lambda end_point: f'End Point: {end_point}') + + def _set_field(self, field_id: str) -> None: + field = self.field_provider.get_field(field_id) + if field is not None: + self.field = field + # if isinstance(self.implement, WeedingImplement): + # self.implement.cultivated_crop = field.crop + self.clear() + else: + if isinstance(self.implement, WeedingImplement): + self.implement.cultivated_crop = None + + def create_simulation(self, crop_distance: float = 0.5) -> None: + self.detector.simulated_objects.clear() + if self.field is None: + return + for row in self.field.rows: + if len(row.points) < 2: + continue + cartesian = row.cartesian() + start = cartesian[0] + end = cartesian[-1] + length = start.distance(end) + self.log.info(f'Adding plants from {start} to {end} (length {length:.1f} m)') + crop_count = length / crop_distance + for i in range(int(crop_count)): + p = start.interpolate(end, (crop_distance * i) / length) + p3d = rosys.geometry.Point3d(x=p.x, y=p.y, z=0) + plant = rosys.vision.SimulatedObject(category_name='maize', position=p3d) + self.detector.simulated_objects.append(plant) + + for _ in range(1, 7): + p = start.polar(crop_distance * i + randint(-5, 5) * 0.01, + start.direction(end)) \ + .polar(randint(-15, 15)*0.01, self.odometer.prediction.yaw + np.pi/2) + self.detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='weed', + position=rosys.geometry.Point3d(x=p.x, y=p.y, z=0))) diff --git a/field_friend/interface/pages/dev_page.py b/field_friend/interface/pages/dev_page.py index 89bc98eb..65de5d7c 100644 --- a/field_friend/interface/pages/dev_page.py +++ b/field_friend/interface/pages/dev_page.py @@ -19,4 +19,6 @@ def page() -> None: self.content() def content(self) -> None: + with ui.card(): + self.system.field_navigation.developer_ui() development(self.system) diff --git a/field_friend/system.py b/field_friend/system.py index 553e907c..c14804e1 100644 --- a/field_friend/system.py +++ b/field_friend/system.py @@ -27,9 +27,9 @@ WeedingScrew, ) from .automations.navigation import ( - ABFieldNavigatoin, ABLineNavigation, CoverageNavigation, + FieldNavigation, FollowCropsNavigation, Navigation, RowsOnFieldNavigation, @@ -155,14 +155,14 @@ def watch_robot() -> None: self.current_navigation.name}, tags: {", ".join(self.plant_locator.tags)}''' rosys.NEW_NOTIFICATION.register(self.timelapse_recorder.notify) rosys.on_startup(self.timelapse_recorder.compress_video) # NOTE: cleanup JPEGs from before last shutdown - self.field_navigation = RowsOnFieldNavigation(self, self.monitoring) + self.rows_on_field_navigation = RowsOnFieldNavigation(self, self.monitoring) self.straight_line_navigation = StraightLineNavigation(self, self.monitoring) self.follow_crops_navigation = FollowCropsNavigation(self, self.monitoring) self.coverage_navigation = CoverageNavigation(self, self.monitoring) self.a_b_line_navigation = ABLineNavigation(self, self.monitoring) - self.field_navigation = ABFieldNavigatoin(self, self.monitoring) + self.field_navigation = FieldNavigation(self, self.monitoring) - self.navigation_strategies = {n.name: n for n in [self.field_navigation, + self.navigation_strategies = {n.name: n for n in [self.rows_on_field_navigation, self.straight_line_navigation, self.follow_crops_navigation, self.coverage_navigation, From f839fe90da4f46891a481f4249bf3249221465dd Mon Sep 17 00:00:00 2001 From: Pascal Schade Date: Fri, 27 Sep 2024 00:27:14 +0200 Subject: [PATCH 10/14] revert debugging code --- field_friend/automations/navigation/field_navigation.py | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/field_friend/automations/navigation/field_navigation.py b/field_friend/automations/navigation/field_navigation.py index cf037513..24614990 100644 --- a/field_friend/automations/navigation/field_navigation.py +++ b/field_friend/automations/navigation/field_navigation.py @@ -149,14 +149,7 @@ async def _run_approaching_row_start(self) -> State: async def _run_following_row(self, distance: float) -> State: if not self.implement.is_active: await self.implement.activate() - # self.update_target() - # if self.odometer.prediction.point.distance(self.target) < 0.05: - end_pose = rosys.geometry.Pose(x=self.target.x, y=self.target.y, yaw=self.origin.direction(self.target), time=0) - relative_point = end_pose.relative_point(self.odometer.prediction.point).x - bool_check = relative_point > 0 - self.log.info(f'End pose: {end_pose} - relative_point: {relative_point} - Bool check: {bool_check}') - # if StraightLineNavigation._should_finish(self): # type: disable=W0212 - if bool_check: + if StraightLineNavigation._should_finish(self): # type: disable=W0212 await self.implement.deactivate() return State.CLEAR_ROW await super()._drive(distance) From f3518815a2b5931cb97edb3dc675c3c480f2526e Mon Sep 17 00:00:00 2001 From: Pascal Schade Date: Fri, 27 Sep 2024 00:27:23 +0200 Subject: [PATCH 11/14] remove else --- field_friend/automations/navigation/field_navigation.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/field_friend/automations/navigation/field_navigation.py b/field_friend/automations/navigation/field_navigation.py index 24614990..2ec277cb 100644 --- a/field_friend/automations/navigation/field_navigation.py +++ b/field_friend/automations/navigation/field_navigation.py @@ -165,11 +165,9 @@ async def _run_row_completed(self) -> State: assert self.field next_state: State = State.ROW_COMPLETED if self.current_row == self.field.rows[-1]: - next_state = State.FIELD_COMPLETED - await rosys.sleep(0.1) # wait for base class to finish navigation - else: - self.row_index += 1 - next_state = State.APPROACHING_ROW_START + return State.FIELD_COMPLETED + self.row_index += 1 + next_state = State.APPROACHING_ROW_START # TODO: remove later, when any direction is possible if self.row_index >= len(self.field.rows): From 0d598216b4e0ee8edb5e1a4c984ac7bda31a2aab Mon Sep 17 00:00:00 2001 From: Pascal Schade Date: Fri, 27 Sep 2024 00:36:19 +0200 Subject: [PATCH 12/14] Revert "Merge remote-tracking branch 'origin/field_creation_wizard' into test-navigation" This reverts commit 049c05c47f66df6b0f7ecc1e6b7d4483b799615b, reversing changes made to 427c78679c7f6335ae89055fc2e82429b4e6384f. --- field_friend/automations/field.py | 60 +--- .../interface/components/field_creator.py | 104 ++++-- .../interface/components/field_planner.py | 18 +- .../interface/components/geodata_picker.py | 334 ------------------ .../interface/components/header_bar.py | 2 +- .../interface/components/leaflet_map.py | 59 ++-- field_friend/interface/pages/main_page.py | 4 - .../localization/point_transformation.py | 1 - 8 files changed, 132 insertions(+), 450 deletions(-) delete mode 100644 field_friend/interface/components/geodata_picker.py diff --git a/field_friend/automations/field.py b/field_friend/automations/field.py index 357cc72e..da2c4542 100644 --- a/field_friend/automations/field.py +++ b/field_friend/automations/field.py @@ -1,13 +1,9 @@ -import math from dataclasses import dataclass, field -from uuid import uuid4 import rosys -from rosys.geometry import Point -from shapely import offset_curve -from shapely.geometry import LineString, Polygon +from shapely.geometry import Polygon -from field_friend.localization import GeoPoint, GeoPointCollection, reference +from field_friend.localization import GeoPoint, GeoPointCollection @dataclass(slots=True, kw_only=True) @@ -34,37 +30,15 @@ def line_segment(self) -> rosys.geometry.LineSegment: @dataclass(slots=True, kw_only=True) class Field(GeoPointCollection): visualized: bool = False - first_row_start: GeoPoint | None = None - first_row_end: GeoPoint | None = None - row_spacing: float = 0.5 - row_number: int = 10 obstacles: list[FieldObstacle] = field(default_factory=list) - outline_buffer_width: float = 2 + rows: list[Row] = field(default_factory=list) + crop: str | None = None @property - def outline(self) -> list[GeoPoint]: - assert self.first_row_start is not None - assert self.first_row_end is not None - ab_line_cartesian = LineString([self.first_row_start.cartesian().tuple, self.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(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.cartesian()) - outline_unbuffered.append(self.first_row_start.cartesian()) - outline_polygon = Polygon([p.tuple for p in outline_unbuffered]) - bufferd_polygon = outline_polygon.buffer( - 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(self.first_row_start.shifted(Point(x=p[0], y=p[1]))) - return outline + def outline(self) -> list[rosys.geometry.Point]: + return self.cartesian() - @ property + @property def outline_as_tuples(self) -> list[tuple[float, float]]: return [p.tuple for p in self.outline] @@ -72,7 +46,7 @@ def area(self) -> float: outline = self.outline if not outline: return 0.0 - polygon = Polygon([(p.lat, p.long) for p in outline]) + polygon = Polygon([(p.x, p.y) for p in outline]) return polygon.area def worked_area(self, worked_rows: int) -> float: @@ -80,21 +54,3 @@ def worked_area(self, worked_rows: int) -> float: if self.area() > 0: worked_area = worked_rows * self.area() / len(self.rows) return worked_area - - @ property - def rows(self) -> list[Row]: - assert self.first_row_start is not None - assert self.first_row_end is not None - first_row_start_cartesian = Point(x=0, y=0) # first_row_start as reference for calculation - first_row_end_cartesian = self.first_row_end.cartesian() - ab_line_cartesian = LineString([first_row_start_cartesian.tuple, first_row_end_cartesian.tuple]) - rows = [] - for i in range(int(self.row_number)+1): - offset = i * self.row_spacing - offset_row_coordinated = offset_curve(ab_line_cartesian, -offset).coords - row_points: list[GeoPoint] = [] - for point in offset_row_coordinated: - row_points.append(self.first_row_start.shifted(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 aafff60e..1b2ec413 100644 --- a/field_friend/interface/components/field_creator.py +++ b/field_friend/interface/components/field_creator.py @@ -3,7 +3,8 @@ import rosys from nicegui import ui -from field_friend.automations import Field + +from field_friend.automations import Field, Row from field_friend.automations.navigation import StraightLineNavigation from field_friend.interface.components.monitoring import CameraPosition from field_friend.localization import GeoPoint @@ -28,10 +29,14 @@ def __init__(self, system: 'System'): self.plant_locator = system.plant_locator self.gnss = system.gnss self.field_provider = system.field_provider + + self.field = Field(id=str(uuid4()), name=f'Field #{len(self.field_provider.fields) + 1}') self.first_row_start: GeoPoint | None = None self.first_row_end: GeoPoint | None = None + self.last_row_end: GeoPoint | None = None self.row_spacing = 0.5 - self.row_number = 10 + self.padding = 1 + self.padding_bottom = 2 self.next: Callable = self.find_first_row with ui.dialog() as self.dialog, ui.card().style('width: 900px; max-width: none'): @@ -58,25 +63,26 @@ def find_first_row(self) -> None: 'right before the first crop. ' 'The blue line should be in the center of the row.') \ .classes('text-lg text-center') - ui.label('Place the back center of the robot over the start point of the row.') \ - .classes('text-lg text-center') self.next = self.get_infos def get_infos(self) -> None: - self.headline.text = 'Field Parameters' assert self.gnss.current is not None if not ("R" in self.gnss.current.mode or self.gnss.current.mode == "SSSS"): with self.content: ui.label('No RTK fix available.').classes('text-red') self.first_row_start = self.gnss.current.location + self.row_sight.content = '' + crops = self.plant_locator.crop_category_names[:] + crops.remove('coin_with_hole') self.content.clear() with self.content: - ui.number('Number of rows', - value=10, step=1, min=1, max=500) \ + ui.select(label='Cultivated Crop', options=crops, clearable=True).classes('w-40') \ + .bind_value(self.field, 'crop') + ui.number('Crop Spacing', suffix='cm', + value=20, step=1, min=1, max=60) \ .props('dense outlined').classes('w-40') \ - .tooltip('Set the number of rows.')\ - .bind_value(self, 'row_number') + .tooltip('Set the distance between the crops') ui.number('Row Spacing', suffix='cm', value=50, step=5, min=20, max=100) \ .props('dense outlined').classes('w-40') \ @@ -88,37 +94,89 @@ def find_row_ending(self) -> None: self.headline.text = 'Find Row Ending' self.content.clear() with self.content: - rosys.driving.joystick(self.steerer, size=50, color='#6E93D6') - ui.label('Drive the robot to the end of the current row.') \ + with ui.row().classes('items-center'): + rosys.automation.automation_controls(self.automator) + ui.label('Press "Play" to start driving forward. ' + 'At the end of the row, press the "Stop" button.') \ .classes('text-lg text-center') - ui.label('Place the back center of the robot over the end point of the row.') \ + self.next = self.drive_to_last_row + + def drive_to_last_row(self) -> None: + assert self.gnss.current is not None + if not("R" in self.gnss.current.mode or self.gnss.current.mode == "SSSS"): + with self.content: + ui.label('No RTK fix available.').classes('text-red') + self.first_row_end = self.gnss.current.location + + self.headline.text = 'Drive to Last Row' + self.content.clear() + with self.content: + rosys.driving.joystick(self.steerer, size=50, color='#6E93D6') + ui.label('Drive the robot to the last row on this side, ' + 'right before the first crop.') \ .classes('text-lg text-center') self.next = self.confirm_geometry def confirm_geometry(self) -> None: assert self.gnss.current is not None - if not ("R" in self.gnss.current.mode or self.gnss.current.mode == "SSSS"): + if not("R" in self.gnss.current.mode or self.gnss.current.mode == "SSSS"): with self.content: ui.label('No RTK fix available.').classes('text-red') - self.first_row_end = self.gnss.current.location + self.last_row_end = self.gnss.current.location + assert self.first_row_end is not None + assert self.last_row_end is not None + if not self.build_geometry(): + d = self.first_row_end.distance(self.last_row_end) + ui.label(f'The distance between first row and last row is {d:.2f} m. ' + f'Which does not match well with the provided row spacing of {self.row_spacing} cm.') \ + .classes('text-red') + self.headline.text = 'Confirm Geometry' self.content.clear() with self.content: with ui.row().classes('items-center'): - ui.label(f'First Row Start: {self.first_row_start}').classes('text-lg') - ui.label(f'First Row End: {self.first_row_end}').classes('text-lg') - ui.label(f'Row Spacing: {self.row_spacing} m').classes('text-lg') - ui.label(f'Number of Rows: {self.row_number}').classes('text-lg') - with ui.row().classes('items-center'): - ui.button('Cancel', on_click=self.dialog.close).props('color=red') + rosys.automation.automation_controls(self.automator) + ui.label('Press "Play" to start driving forward. ' + 'At the end of the row, press the "Stop" button.') \ + .classes('w-64 text-lg') self.next = self._apply + def build_geometry(self) -> bool: + """Build the geometry of the field based on the given points. + + Returns True if the row spacing matches the distance between the first and last row, False otherwise. + Will create rows in any case to make testing easier. + """ + assert self.first_row_start is not None + assert self.first_row_end is not None + assert self.last_row_end is not None + 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() + 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.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.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: self.dialog.close() - self.field_provider.fields = [] # currently only a single field is 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.fields.append(self.field) self.field_provider.request_backup() self.field_provider.FIELDS_CHANGED.emit() diff --git a/field_friend/interface/components/field_planner.py b/field_friend/interface/components/field_planner.py index c4c4299b..822974ef 100644 --- a/field_friend/interface/components/field_planner.py +++ b/field_friend/interface/components/field_planner.py @@ -12,7 +12,7 @@ from field_friend.system import System -TabType = Literal["Obstacles", "Outline", "Rows"] +TabType = Literal["Plants", "Obstacles", "Outline", "Rows"] class ActiveObject(TypedDict): @@ -27,9 +27,9 @@ def __init__(self, system: 'System', leaflet: leaflet_map) -> None: self.field_provider = system.field_provider self.odometer = system.odometer self.gnss = system.gnss - # self.cultivatable_crops = system.crop_category_names + self.cultivatable_crops = system.crop_category_names self.leaflet_map = leaflet - self.tab: TabType = "Outline" + self.tab: TabType = "Plants" self.active_object: ActiveObject | None = None self.active_field: Field | None = None self.restore_actives() @@ -134,16 +134,16 @@ def show_field_settings(self) -> None: .classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;") \ .tooltip("Delete field") with ui.tabs().style("width: 100%;") as self.tabs: - # ui.tab("Plants", "Plants") + ui.tab("Plants", "Plants") ui.tab("Outline", "Outline") ui.tab("Obstacles", "Obstacles") ui.tab("Rows", "Rows") with ui.tab_panels(self.tabs, value=f"{self.tab}", on_change=self.set_tab).style("width: 100%;") as self.panels: - # with ui.tab_panel("Plants").style("width: 100%;"): - # ui.select(self.cultivatable_crops, label="Cultivated Crop", on_change=self.field_provider.request_backup) \ - # .classes("w-40").props('clearable') \ - # .bind_value(self.active_field, "crop") \ - # .tooltip('Set the cultivated crop which should be kept safe') + with ui.tab_panel("Plants").style("width: 100%;"): + ui.select(self.cultivatable_crops, label="Cultivated Crop", on_change=self.field_provider.request_backup) \ + .classes("w-40").props('clearable') \ + .bind_value(self.active_field, "crop") \ + .tooltip('Set the cultivated crop which should be kept safe') with ui.tab_panel("Outline").style("width: 100%;"): for geo_point in self.active_field.points: with ui.row().style("width: 100%;"): diff --git a/field_friend/interface/components/geodata_picker.py b/field_friend/interface/components/geodata_picker.py deleted file mode 100644 index a9dce750..00000000 --- a/field_friend/interface/components/geodata_picker.py +++ /dev/null @@ -1,334 +0,0 @@ -import json -import math -import uuid -import xml.etree.ElementTree as ET -from pathlib import Path -from typing import Optional - -import fiona -import geopandas as gpd -from shapely import difference, offset_curve -from shapely.geometry import (GeometryCollection, LinearRing, LineString, MultiLineString, MultiPoint, Polygon, - mapping) -from shapely.ops import transform - -import rosys -from nicegui import events, ui - -from ...automations import Field, FieldObstacle, FieldProvider, Row -from ...navigation.point_transformation import cartesian_to_wgs84 - -# Enable fiona driver -fiona.drvsupport.supported_drivers['kml'] = 'rw' # enable KML support which is disabled by default -fiona.drvsupport.supported_drivers['KML'] = 'rw' -fiona.drvsupport.supported_drivers['LIBKML'] = 'rw' - - -class geodata_picker(ui.dialog): - def __init__(self, field_provider: FieldProvider) -> None: - super().__init__() - self.field_provider = field_provider - self.is_farmdroid = False - self.safety_distance = 2.7 - self.working_width = 2.7 - self.headland_tracks = 1 - self.ab_line = 1 - - with self, ui.card(): - with ui.row(): - ui.label("Upload a file.").classes('text-xl w-80') - with ui.row(): - ui.label( - "Only a single polygon will be processed. Supported file formates: .xml with ISO 11783, .shp, .kml.").classes('w-80') - with ui.row(): - ui.label( - "If you want to upload a shape, create a zip-file containing all files (minimum: .shp, .shx, .dbf) and upload the zip.").classes('w-80') - with ui.row(): - ui.upload(on_upload=self.restore_from_file, multiple=False) - with ui.row(): - farmdroid_checkbox = ui.checkbox('FarmDroid Data', on_change=lambda e: self.set_farmdroid(e.value)) - with ui.row().bind_visibility_from(farmdroid_checkbox, 'value').classes('w-80'): - with ui.row().classes('w-full place-items-center'): - with ui.icon('help').classes('text-xl'): - ui.tooltip('The width of the attached tool.') - ui.number(label='Working Width (in m)', value=self.working_width, format='%.2f', - on_change=lambda e: self.set_working_width(e.value)) - with ui.row().classes('w-full place-items-center'): - with ui.icon('help').classes('text-xl'): - ui.tooltip('The distance between the border of the field and the headland.') - ui.number(label='Safety Distance (in m)', value=self.safety_distance, format='%.2f', - on_change=lambda e: self.set_safety_distance(e.value)) - with ui.row().classes('w-full place-items-center'): - with ui.icon('help').classes('text-xl'): - ui.tooltip('Number of headland tracks.') - ui.number(label='headland Tracks', value=self.headland_tracks, format='%.2f', - on_change=lambda e: self.set_headland_tracks(e.value)) - with ui.row().classes('w-full place-items-center'): - with ui.icon('help').classes('text-xl'): - ui.tooltip('In which direction does the AB-line run?') - switch = ui.switch('AB-Line Direction', on_change=lambda e: self.set_ab_line(e.value)) - # TODO EINGABE FÜR DAS OFFSET DER FARMDROID DATEN - - with ui.row().classes('w-full justify-end'): - ui.button('Cancel', on_click=self.close).props('outline') - - def set_farmdroid(self, value) -> None: - self.is_farmdroid = value - - def set_ab_line(self, value) -> None: - if value: - self.ab_line = 2 - else: - self.ab_line = 1 - - def set_safety_distance(self, value) -> None: - self.safety_distance = value - - def set_working_width(self, value) -> None: - self.working_width = value - - def set_headland_tracks(self, value) -> None: - self.headland_tracks = value - - def extract_coordinates_kml(self, event: events.UploadEventArguments) -> list: - coordinates = [] - gdf = gpd.read_file(event.content, drivr="KML") - x_coordinate, y_coordinate = gdf['geometry'].iloc[0].xy - extracted_points = list(zip(x_coordinate, y_coordinate)) - for point in extracted_points: - coordinates.append([point[1], point[0]]) - return [coordinates] - - def extract_coordinates_xml(self, event: events.UploadEventArguments) -> list: - coordinates = [] - tree = ET.parse(event.content) - root = tree.getroot() - for geo_data in root.findall('.//LSG'): - for point in geo_data.findall('.//PNT'): - lat = float(point.attrib['C']) - lon = float(point.attrib['D']) - coordinates.append([lat, lon]) - return [coordinates] - - def extract_coordinates_shp(self, event: events.UploadEventArguments) -> Optional[list]: - coordinates = [] - try: - gdf = gpd.read_file(event.content) - gdf['geometry'] = gdf['geometry'].apply(lambda geom: transform(self.swap_coordinates, geom)) - feature = json.loads(gdf.to_json()) - coordinates = feature["features"][0]["geometry"]["coordinates"] - return coordinates - except: - rosys.notify("The .zip file does not contain a shape file.", type='warning') - return None - - def swap_coordinates(self, lon, lat): - return lat, lon - - def get_extrapoled_line(self, p1, p2) -> LineString: - extrapol_ratio = 10 - a = [p1[0]+extrapol_ratio*(p1[0]-p2[0]), p1[1]+extrapol_ratio*(p1[1]-p2[1])] - b = [p2[0]+extrapol_ratio*(p2[0]-p1[0]), p2[1]+extrapol_ratio*(p2[1]-p1[1])] - return LineString([a, b]) - - def get_rows(self, field: Field) -> list: - if not self.is_farmdroid: - return [] - if self.safety_distance + (self.working_width / 2) < 2.7: - rosys.notify( - 'The distance between the outer headland track and the field boundary is too small. Please check the specified values.') - return [] - lines = [] - outline = [] - for point in field.outline: - outline.append([point.x, point.y]) - outline_polygon = Polygon(outline) - # TODO: hinzufügen von abstand zwischen reihen und arbeitsbreite - buffer_width = self.safety_distance + (self.headland_tracks * self.working_width) - row_spacing = self.working_width / 6 - working_area_meter = outline_polygon.buffer(-buffer_width, join_style='mitre', mitre_limit=math.inf) - working_area_coordinates_meter = mapping(working_area_meter)['coordinates'][0] - working_area_coordinates = [] - for point in working_area_coordinates_meter: - transformed_point = cartesian_to_wgs84([field.reference_lat, field.reference_lon], [-point[1], point[0]]) - working_area_coordinates.append([transformed_point[0], transformed_point[1]]) - if self.ab_line == 1: - ab_line = LineString([(working_area_coordinates[0][0], working_area_coordinates[0][1]), - (working_area_coordinates[1][0], working_area_coordinates[1][1])]) - ab_line_meter = LineString([(working_area_coordinates_meter[0][0], working_area_coordinates_meter[0][1]), - (working_area_coordinates_meter[1][0], working_area_coordinates_meter[1][1])]) - if self.ab_line == 2: - if working_area_coordinates[0][0] == working_area_coordinates[-1][0] and working_area_coordinates[0][1] == working_area_coordinates[-1][1]: - ab_line = LineString([(working_area_coordinates[0][0], working_area_coordinates[0][1]), - (working_area_coordinates[-2][0], working_area_coordinates[-2][1])]) - ab_line_meter = LineString([(working_area_coordinates_meter[0][0], working_area_coordinates_meter[0][1]), - (working_area_coordinates_meter[-2][0], working_area_coordinates_meter[-2][1])]) - else: - ab_line = LineString([(working_area_coordinates[0][0], working_area_coordinates[0][1]), - (working_area_coordinates[-1][0], working_area_coordinates[-1][1])]) - ab_line_meter = LineString([(working_area_coordinates_meter[0][0], working_area_coordinates_meter[0][1]), - (working_area_coordinates_meter[-1][0], working_area_coordinates_meter[-1][1])]) - - direction_check = offset_curve(ab_line_meter, -row_spacing) - if working_area_meter.contains(direction_check) or working_area_meter.intersects(direction_check): - row_spacing = -row_spacing - lines_meter = [] - # lines_meter.append(ab_line_meter) - # lines.append(ab_line) - line_inside = True - while line_inside: - if len(lines_meter) == 0: - line_meter = offset_curve(ab_line_meter, (row_spacing/2)) - else: - line_meter = offset_curve(lines_meter[-1], row_spacing) - if working_area_meter.contains(line_meter) or working_area_meter.intersects(line_meter): - lines_meter.append(line_meter) - line = [] - for point in mapping(line_meter)["coordinates"]: - point_wgs84 = cartesian_to_wgs84([field.reference_lat, field.reference_lon], [-point[1], point[0]]) - line.append(point_wgs84) - linestring = LineString([(line[0][0], line[0][1]), (line[1][0], line[1][1])]) - lines.append(linestring) - else: - line_inside = False - working_area_ring = LinearRing(working_area_coordinates) - rows = [] - row_number = 1 - for line in lines: - point_list = list(mapping(line)['coordinates']) - for index, point in enumerate(point_list): - point_list[index] = list(point) - extrapolated_line = self.get_extrapoled_line(point_list[0], point_list[1]) - intersection_geometry = working_area_ring.intersection(extrapolated_line) - if isinstance(intersection_geometry, MultiPoint): - intersection_list = [] - # TODO hier noch Fälle beachten, bei denen die Linie den Umring in mehr als zwei getrennten Punkten/Segmenten schneidet - for point in list(mapping(intersection_geometry)['coordinates']): - intersection_list.append([point[0], point[1]]) - divided_lines = [] - for obstacle in field.obstacles: - # TODO hier Obstacles buffern und dann diese als Beschnitt nutzen - obstacle_meter = obstacle.points([field.reference_lat, field.reference_lon]) - obstacle_points_meter = [] - for point in obstacle_meter: - obstacle_points_meter.append([point.x, point.y]) - obstacle_polygon = Polygon(obstacle_points_meter) - obstacle_buffer_width = self.working_width + self.safety_distance - obstacle_buffer = obstacle_polygon.buffer( - obstacle_buffer_width, join_style='mitre', mitre_limit=math.inf) - obstacle_coordinates_meter = mapping(obstacle_buffer)['coordinates'][0] - obstacle_coordinates = [] - for point in obstacle_coordinates_meter: - transformed_point = cartesian_to_wgs84( - [field.reference_lat, field.reference_lon], [-point[1], point[0]]) - obstacle_coordinates.append([transformed_point[0], transformed_point[1]]) - divided_lines.append(difference(LineString(intersection_list), Polygon(obstacle_coordinates))) - if any(isinstance(x, MultiLineString) for x in divided_lines): - for segment in divided_lines: - if isinstance(segment, MultiLineString): - row_segment_number = 1 - for line in mapping(segment)['coordinates']: - row_coordinates = [] - for point in line: - row_coordinates.append([point[0], point[1]]) - row_id = str(uuid.uuid4()) - new_row = Row(id=f'{row_id}', name=f'{row_number}_{row_segment_number}', - points_wgs84=row_coordinates) - rows.append(new_row) - row_segment_number += 1 - else: - row_id = str(uuid.uuid4()) - new_row = Row(id=f'{row_id}', name=f'{row_number}', points_wgs84=intersection_list) - rows.append(new_row) - elif isinstance(intersection_geometry, GeometryCollection): - for geometry in mapping(intersection_geometry)['geometries']: - if geometry == LineString or geometry == MultiPoint: - intersection_list = [] - # TODO hier noch Fälle beachten, bei denen die Linie den Umring in mehr als zwei getrennten Punkten/Segmenten schneidet - for point in list(mapping(geometry)['coordinates']): - intersection_list.append([point[0], point[1]]) - divided_lines = [] - for obstacle in field.obstacles: - # TODO hier Obstacles buffern und dann diese als Beschnitt nutzen - obstacle_meter = obstacle.points([field.reference_lat, field.reference_lon]) - obstacle_points_meter = [] - for point in obstacle_meter: - obstacle_points_meter.append([point.x, point.y]) - obstacle_polygon = Polygon(obstacle_points_meter) - obstacle_buffer_width = self.working_width + self.safety_distance - obstacle_buffer = obstacle_polygon.buffer( - obstacle_buffer_width, join_style='mitre', mitre_limit=math.inf) - obstacle_coordinates_meter = mapping(obstacle_buffer)['coordinates'][0] - obstacle_coordinates = [] - for point in obstacle_coordinates_meter: - transformed_point = cartesian_to_wgs84( - [field.reference_lat, field.reference_lon], [-point[1], point[0]]) - obstacle_coordinates.append([transformed_point[0], transformed_point[1]]) - divided_lines.append(difference(LineString(intersection_list), - Polygon(obstacle_coordinates))) - if any(isinstance(x, MultiLineString) for x in divided_lines): - for segment in divided_lines: - if isinstance(segment, MultiLineString): - row_segment_number = 1 - for line in mapping(segment)['coordinates']: - row_coordinates = [] - for point in line: - row_coordinates.append([point[0], point[1]]) - row_id = str(uuid.uuid4()) - new_row = Row( - id=f'{row_id}', name=f'{row_id}_{row_segment_number}', points_wgs84=row_coordinates) - rows.append(new_row) - row_segment_number += 1 - else: - row_id = str(uuid.uuid4()) - new_row = Row(id=f'{row_id}', name=f'{row_number}', points_wgs84=intersection_list) - rows.append(new_row) - # elif type(geometry) == Point: - # intersection_list.append([geometry[0], geometry[1]]) - row_number += 1 - return rows - - def get_obstacles(self, polygon_list): - obstacle_list = [] - for polygon in polygon_list[1:]: - row_id = str(uuid.uuid4()) - new_obstacle = FieldObstacle(id=f'{row_id}', name=f'{row_id}', points_wgs84=polygon) - obstacle_list.append(new_obstacle) - return obstacle_list - - async def restore_from_file(self, e: events.UploadEventArguments) -> None: - self.close() - coordinates: list = [] - if e is None or e.content is None: - rosys.notify("You can only upload the following file formates: .kml ,.xml. with ISO and shape files.", type='warning') - return - elif e.name[-3:].casefold() == "zip": - coordinates = self.extract_coordinates_shp(e) - elif e.name[-3:].casefold() == "kml": - coordinates = self.extract_coordinates_kml(e) - elif e.name[-3:].casefold() == "xml": - coordinates = self.extract_coordinates_xml(e) - else: - rosys.notify("You can only upload the following file formates: .kml ,.xml. with ISO and shape files.", type='warning') - return - if coordinates is None: - rosys.notify("An error occurred while importing the file.", type='negative') - return - reference_point = coordinates[0][0] - field_id = str(uuid.uuid4()) - field = Field(id=f'{field_id}', name=f'{field_id}', outline_wgs84=coordinates[0], - reference_lat=reference_point[0], reference_lon=reference_point[1]) - obstacles = [] - if len(coordinates) > 1: - obstacles = self.get_obstacles(coordinates) - field.obstacles = obstacles - rows = self.get_rows(field) - field.rows = rows - outline = [] - for point in field.outline: - outline.append([point.x, point.y]) - field.area = Polygon(outline).area - if len(coordinates[0]) > 2 and coordinates[0][0] == coordinates[0][-1]: - coordinates[0].pop() # the last point is the same as the first point - self.field_provider.add_field(field) - return diff --git a/field_friend/interface/components/header_bar.py b/field_friend/interface/components/header_bar.py index ab131ecf..e6ce2bfe 100644 --- a/field_friend/interface/components/header_bar.py +++ b/field_friend/interface/components/header_bar.py @@ -38,7 +38,7 @@ def __init__(self, system: 'System', right_drawer: ui.right_drawer): ui.label('Software ESTOP is active!').classes('text-white text-3xl').props('elevated') with ui.row(): - # ui.link('Field planner', '/field').classes('text-white text-lg !no-underline') + ui.link('Field planner', '/field').classes('text-white text-lg !no-underline') # ui.link('Path recorder', '/path').classes('text-white text-lg !no-underline') ui.link('Circle Sight', '/monitor').classes('text-white text-lg !no-underline') # ui.link('Development', '/dev').classes('text-white text-lg !no-underline') diff --git a/field_friend/interface/components/leaflet_map.py b/field_friend/interface/components/leaflet_map.py index 40f9ad38..4a9065bc 100644 --- a/field_friend/interface/components/leaflet_map.py +++ b/field_friend/interface/components/leaflet_map.py @@ -56,12 +56,11 @@ def __init__(self, system: 'System', draw_tools: bool) -> None: self.drawn_marker = None self.obstacle_layers: list = [] self.row_layers: list = [] - self.active_field: str | None = None - self.set_active_field() + self._active_field: str | None = None + self.active_object: Active_object | None = None self.update_layers() - self.zoom_to_robot() - self.field_provider.FIELDS_CHANGED.register(self.set_active_field) self.field_provider.FIELDS_CHANGED.register(self.update_layers) + self.zoom_to_robot() def handle_draw(e: events.GenericEventArguments): if e.args['layerType'] == 'marker': @@ -108,6 +107,7 @@ def buttons(self) -> None: ui.button(icon='my_location', on_click=self.zoom_to_robot).props('dense flat') \ .tooltip('Center map on robot position').classes('ml-0') ui.button(on_click=self.zoom_to_field) \ + .bind_enabled_from(self, 'active_field') \ .props('icon=polyline dense flat') \ .tooltip('center map on field boundaries').classes('ml-0') @@ -115,15 +115,15 @@ def abort_point_drawing(self, dialog) -> None: self.on_dialog_close() dialog.close() - # def add_point_active_object(self, latlon, dialog) -> None: - # dialog.close() - # self.on_dialog_close() - # if self.active_object and self.active_object.get("object") is not None: - # self.active_object.get("object").points.append(GeoPoint.from_list(latlon)) - # self.field_provider.invalidate() - # self.update_layers() - # else: - # ui.notify("No object selected. Point could not be added to the void.") + def add_point_active_object(self, latlon, dialog) -> None: + dialog.close() + self.on_dialog_close() + if self.active_object and self.active_object.get("object") is not None: + self.active_object.get("object").points.append(GeoPoint.from_list(latlon)) + self.field_provider.invalidate() + self.update_layers() + else: + ui.notify("No object selected. Point could not be added to the void.") def update_layers(self) -> None: for layer in self.field_layers: @@ -133,7 +133,7 @@ def update_layers(self) -> None: for field in self.field_provider.fields: color = '#6E93D6' if field.id == self.active_field else '#999' self.field_layers.append(self.m.generic_layer(name="polygon", - args=[field.outline_as_tuples, {'color': color}])) + args=[field.points_as_tuples, {'color': color}])) current_field: Field | None = self.field_provider.get_field(self.active_field) for layer in self.obstacle_layers: self.m.remove_layer(layer) @@ -141,13 +141,23 @@ def update_layers(self) -> None: for layer in self.row_layers: self.m.remove_layer(layer) self.row_layers = [] - if current_field is not None: - for obstacle in current_field.obstacles: - self.obstacle_layers.append(self.m.generic_layer(name="polygon", - args=[obstacle.points_as_tuples, {'color': '#C10015'}])) - for row in current_field.rows: - self.row_layers.append(self.m.generic_layer(name="polyline", - args=[row.points_as_tuples, {'color': '#F2C037'}])) + if current_field is None: + return + for obstacle in current_field.obstacles: + self.obstacle_layers.append(self.m.generic_layer(name="polygon", + args=[obstacle.points_as_tuples, {'color': '#C10015'}])) + for row in current_field.rows: + self.row_layers.append(self.m.generic_layer(name="polyline", + args=[row.points_as_tuples, {'color': '#F2C037'}])) + + @property + def active_field(self) -> str | None: + return self._active_field + + @active_field.setter + def active_field(self, field_id: str | None) -> None: + self._active_field = field_id + self.update_layers() def update_robot_position(self, position: GeoPoint, dialog=None) -> None: if dialog: @@ -167,10 +177,10 @@ def zoom_to_robot(self) -> None: self.m.set_zoom(self.current_basemap.options['maxZoom'] - 1) def zoom_to_field(self) -> None: - field = self.field_provider.fields[0] if len(self.field_provider.fields) > 0 else None + field = self.field_provider.get_field(self.active_field) if field is None: return - coords = field.outline_as_tuples + coords = field.points_as_tuples center = sum(lat for lat, _ in coords) / len(coords), sum(lon for _, lon in coords) / len(coords) self.m.set_center(center) self.m.set_zoom(self.current_basemap.options['maxZoom'] - 1) # TODO use field boundaries to calculate zoom @@ -212,6 +222,3 @@ def on_dialog_close(self) -> None: if self.drawn_marker is not None: self.m.remove_layer(self.drawn_marker) self.drawn_marker = None - - def set_active_field(self) -> None: - self.active_field = self.field_provider.fields[0].id if len(self.field_provider.fields) > 0 else None diff --git a/field_friend/interface/pages/main_page.py b/field_friend/interface/pages/main_page.py index 2a4c3c17..b1a6d102 100644 --- a/field_friend/interface/pages/main_page.py +++ b/field_friend/interface/pages/main_page.py @@ -6,7 +6,6 @@ from field_friend.system import System from ..components import camera_card, leaflet_map, operation, robot_scene -from ..components.field_creator import FieldCreator class main_page(): @@ -47,6 +46,3 @@ def content(self, devmode) -> None: ui.space() with ui.row(): rosys.automation.automation_controls(self.system.automator) - with ui.row(): - ui.button("Field Creation Wizard", on_click=lambda: FieldCreator(self.system)).tooltip("Build a field with AB-line in a few simple steps") \ - .classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;") diff --git a/field_friend/localization/point_transformation.py b/field_friend/localization/point_transformation.py index a9841a16..b6ad0484 100644 --- a/field_friend/localization/point_transformation.py +++ b/field_friend/localization/point_transformation.py @@ -1,6 +1,5 @@ import numpy as np from geographiclib.geodesic import Geodesic -from rosys.geometry import Point from .geo_point import GeoPoint From 01d267f3841e99b89c7ffa23550b81a67fcbf096 Mon Sep 17 00:00:00 2001 From: Pascal Schade Date: Fri, 27 Sep 2024 00:49:16 +0200 Subject: [PATCH 13/14] remove logging --- field_friend/automations/navigation/field_navigation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/field_friend/automations/navigation/field_navigation.py b/field_friend/automations/navigation/field_navigation.py index 2ec277cb..f50da249 100644 --- a/field_friend/automations/navigation/field_navigation.py +++ b/field_friend/automations/navigation/field_navigation.py @@ -260,7 +260,7 @@ def create_simulation(self, crop_distance: float = 0.5) -> None: start = cartesian[0] end = cartesian[-1] length = start.distance(end) - self.log.info(f'Adding plants from {start} to {end} (length {length:.1f} m)') + # self.log.info(f'Adding plants from {start} to {end} (length {length:.1f} m)') crop_count = length / crop_distance for i in range(int(crop_count)): p = start.interpolate(end, (crop_distance * i) / length) From 183701800f7c9e571a3bb0d63fc4e5a78eb23491 Mon Sep 17 00:00:00 2001 From: Pascal Schade Date: Fri, 27 Sep 2024 00:49:39 +0200 Subject: [PATCH 14/14] add test field --- field_friend/automations/field_provider.py | 25 ++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/field_friend/automations/field_provider.py b/field_friend/automations/field_provider.py index 33fcd2b3..7f7a0767 100644 --- a/field_friend/automations/field_provider.py +++ b/field_friend/automations/field_provider.py @@ -25,6 +25,31 @@ def __init__(self, gnss: Gnss) -> None: self.needs_backup: bool = False + field = Field(id='dummy', name='dummy', points=[]) + for i, row_list in enumerate([[GeoPoint(lat=51.98316780897856, long=7.434212), GeoPoint(lat=51.98321232423919, long=7.434212)], + [GeoPoint(lat=51.983167808956075, long=7.4342847762085835), + GeoPoint(lat=51.98321232421671, long=7.434284776280727)], + [GeoPoint(lat=51.98316780888862, long=7.4343575524171674), + GeoPoint(lat=51.98321232414926, long=7.434357552561454)], + [GeoPoint(lat=51.9831678087762, long=7.4344303286257505), + GeoPoint(lat=51.98321232403683, long=7.43443032884218)], + [GeoPoint(lat=51.98316780861881, long=7.4345031048343335), + GeoPoint(lat=51.98321232387944, long=7.434503105122906)], + [GeoPoint(lat=51.98316780841646, long=7.434575881042917), + GeoPoint(lat=51.98321232367709, long=7.434575881403632)], + [GeoPoint(lat=51.983167808169135, long=7.434648657251498), + GeoPoint(lat=51.98321232342976, long=7.434648657684356)], + [GeoPoint(lat=51.983167807876846, long=7.434721433460079), + GeoPoint(lat=51.98321232313747, long=7.4347214339650805)], + [GeoPoint(lat=51.98316780753959, long=7.434794209668659), + GeoPoint(lat=51.983212322800206, long=7.434794210245803)], + [GeoPoint(lat=51.98316780715736, long=7.434866985877237), + GeoPoint(lat=51.983212322417984, long=7.434866986526525)], + [GeoPoint(lat=51.98316780673016, long=7.434939762085815), GeoPoint(lat=51.98321232199079, long=7.434939762807245)],]): + row = Row(id=f'row_{i}', name=f'row_{i}', points=list(row_list)) + field.rows.append(row) + self.fields.append(field) + def get_field(self, id_: str | None) -> Field | None: for field in self.fields: if field.id == id_: