External mower

config/f14_config_f14/camera.py

@@ -0,0 +1,13 @@
+configuration = {'parameters': {
+    'width': 1280,
+    'height': 720,
+    'auto_exposure': True,
+    'fps': 10,
+},
+    'crop': {
+    'left': 60,
+    'right': 200,
+    'up': 20,
+    'down': 0,
+}
+}

config/f14_config_f14/hardware.py

@@ -0,0 +1,88 @@
+configuration = {
+    'wheels': {
+        'version': 'double_wheels',
+        'name': 'wheels',
+        'left_back_can_address': 0x100,
+        'left_front_can_address': 0x000,
+        'right_back_can_address': 0x300,
+        'right_front_can_address': 0x200,
+        'is_left_reversed': False,
+        'is_right_reversed': True,
+        'odrive_version': 6,
+    },
+    # # FIXME: PIN checken
+    # 'eyes': {
+    #     'name': 'eyes',
+    #     'on_expander': True,
+    #     'eyes_pin': 12,
+    # },
+    'y_axis': {
+        'version': 'none',
+    },
+    'z_axis': {
+        'version': 'none',
+    },
+    'external_mower': {
+        'name': 'mower',
+        'm0_can_address': 0x400,
+        'm1_can_address': 0x500,
+        'm2_can_address': 0x600,
+        'm_per_tick': 0.01,
+        'speed': 10.0,
+        'is_m0_reversed': False,
+        'is_m1_reversed': False,
+        'is_m2_reversed': False,
+        'odrive_version': 6,
+    },
+    'estop': {
+        'name': 'estop',
+        'pins': {'1': 34, '2': 35},
+    },
+    'bms': {
+        'name': 'bms',
+        'on_expander': True,
+        'rx_pin': 26,
+        'tx_pin': 27,
+        'baud': 9600,
+        'num': 2,
+    },
+    'battery_control': {
+        'name': 'battery_control',
+        'on_expander': True,
+        'reset_pin': 15,
+        'status_pin': 13,
+    },
+    'flashlight': {
+        'version': 'none',
+    },
+    'bumper': {
+        'name': 'bumper',
+        'on_expander': True,
+        'pins': {'front_top': 18, 'front_bottom': 35, 'back': 21},
+    },
+    'status_control': {
+        'name': 'status_control',
+    },
+    'bluetooth': {
+        'name': 'fieldfriend-f14',
+    },
+    'serial': {
+        'name': 'serial',
+        'rx_pin': 26,
+        'tx_pin': 27,
+        'baud': 115200,
+        'num': 1,
+    },
+    'expander': {
+        'name': 'p0',
+        'boot': 25,
+        'enable': 14,
+    },
+    'can': {
+        'name': 'can',
+        'on_expander': False,
+        'rx_pin': 32,
+        'tx_pin': 33,
+        'baud': 1_000_000,
+    },
+}

config/f14_config_f14/params.py

@@ -0,0 +1,8 @@
+configuration = {
+    'motor_gear_ratio': 12.52,
+    'thooth_count': 15,
+    'pitch': 0.033,
+    'wheel_distance': 0.47,
+    'antenna_offset': 0.205,
+    'tool': 'mower',
+}

config/f14_config_f14/robotbrain.py

@@ -0,0 +1,3 @@
+configuration = {'robot_brain': {
+    'flash_params': ['orin', 'v05', 'nand']
+}}

field_friend/automations/field_provider.py

@@ -205,8 +205,6 @@ def add_row_point(self, field: Field, row: Row, point: Optional[GeoPoint] = None
                 rosys.notify("GNSS position is not accurate enough.")
                 return
             new_point = positioning
-        if self.gnss.device != 'simulation':
-            self.ensure_field_reference(field)
         if point is not None:
             index = row.points.index(point)
             row.points[index] = new_point

field_friend/automations/implements/__init__.py

@@ -6,6 +6,7 @@
 from .recorder import Recorder
 from .tornado import Tornado
 from .weeding_screw import WeedingScrew
+from .external_mower import ExternalMower
 
 __all__ = [
     'Implement',
@@ -14,5 +15,6 @@
     'Recorder',
     'WeedingScrew',
     'Tornado',
+    'ExternalMower',
     'ImplementException',
 ]

field_friend/automations/implements/external_mower.py

@@ -0,0 +1,75 @@
+
+
+import logging
+from typing import TYPE_CHECKING, Any
+
+import rosys
+from nicegui import ui
+
+from ...hardware import MowerHardware
+from ..navigation import WorkflowException
+from .weeding_implement import Implement
+
+if TYPE_CHECKING:
+    from system import System
+
+
+class ExternalMower(Implement, rosys.persistence.PersistentModule):
+    STRETCH_DISTANCE: float = 2.0
+
+    def __init__(self, system: 'System') -> None:
+        super().__init__('Mower')
+        self.log = logging.getLogger('field_friend.mower')
+        self.mower_hardware: MowerHardware = system.field_friend.mower
+        self.driver = system.driver
+        assert self.mower_hardware is not None
+        assert self.driver is not None
+        self.is_demo: bool = False
+        self.stretch_distance: float = self.STRETCH_DISTANCE
+
+    async def activate(self):
+        if not self.is_demo:
+            await self.mower_hardware.turn_on()
+            await rosys.sleep(2)
+        await super().activate()
+
+    async def deactivate(self):
+        await self.mower_hardware.turn_off()
+        await super().deactivate()
+
+    async def get_stretch(self, max_distance: float) -> float:
+        if not any([self.mower_hardware.m0_error, self.mower_hardware.m1_error, self.mower_hardware.m2_error]):
+            return min(self.stretch_distance, max_distance)
+        if all([self.mower_hardware.m0_error, self.mower_hardware.m1_error, self.mower_hardware.m2_error]):
+            rosys.notify('All motors are stuck', 'negative')
+            raise WorkflowException('All motors are stuck')
+        # TODO: implement a better error handling
+        await rosys.sleep(0.1)
+        self.log.warning('Stuck motor detected')
+        await self.driver.wheels.stop()
+        await self.mower_hardware.reset_motors()
+        await rosys.sleep(5)
+        await self.mower_hardware.turn_on()
+        await rosys.sleep(2)
+        return 0.0
+
+    def backup(self) -> dict:
+        return {
+            'is_demo': self.is_demo,
+            'stretch_distance': self.stretch_distance,
+        }
+
+    def restore(self, data: dict[str, Any]) -> None:
+        super().restore(data)
+        self.is_demo = data.get('is_demo', self.is_demo)
+        self.stretch_distance = data.get('stretch_distance', self.stretch_distance)
+
+    def settings_ui(self):
+        ui.checkbox('Demo Mode', on_change=self.request_backup) \
+            .bind_value(self, 'is_demo') \
+            .tooltip('Do not start the mowing motors')
+        ui.number('Stretch Distance', step=0.05, min=0.05, max=100.0, format='%.2f', on_change=self.request_backup) \
+            .props('dense outlined') \
+            .classes('w-24') \
+            .bind_value(self, 'stretch_distance') \
+            .tooltip(f'Forward speed limit in m/s (default: {self.STRETCH_DISTANCE:.2f})')

field_friend/automations/navigation/__init__.py

@@ -1,14 +1,17 @@
 
+from .a_b_line_navigation import ABLineNavigation
 from .coverage_navigation import CoverageNavigation
 from .follow_crops_navigation import FollowCropsNavigation
-from .navigation import Navigation
+from .navigation import Navigation, WorkflowException
 from .row_on_field_navigation import RowsOnFieldNavigation
 from .straight_line_navigation import StraightLineNavigation
 
 __all__ = [
     'Navigation',
+    'WorkflowException',
     'RowsOnFieldNavigation',
     'StraightLineNavigation',
     'FollowCropsNavigation',
     'CoverageNavigation',
+    'ABLineNavigation',
 ]

field_friend/automations/navigation/a_b_line_navigation.py

@@ -0,0 +1,142 @@
+from typing import TYPE_CHECKING, Any
+
+import rosys
+from nicegui import ui
+from rosys.geometry import Point
+
+from ..field import Field, Row
+from ..implements.implement import Implement
+from .navigation import Navigation
+
+if TYPE_CHECKING:
+    from system import System
+
+
+class ABLineNavigation(Navigation):
+
+    def __init__(self, system: 'System', tool: Implement) -> None:
+        super().__init__(system, tool)
+        self.MAX_STRETCH_DISTANCE: float = 2.0
+        self.STOP_DISTANCE: float = 0.1
+        self.start_position = self.odometer.prediction.point
+        self.name = 'A-B line'
+        self.gnss = system.gnss
+        self.bms = system.field_friend.bms
+        self.automation_watcher = system.automation_watcher
+        self.field_provider = system.field_provider
+        assert self.gnss is not None
+        assert self.bms is not None
+        assert self.automation_watcher is not None
+        assert self.field_provider is not None
+        self.field: Field | None = None
+        self.row: Row | None = None
+        self.start_point: Point | None = None
+        self.end_point: Point | None = None
+
+    async def prepare(self) -> bool:
+        await super().prepare()
+        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
+        self.row = self.get_nearest_row()
+        if self.row is None:
+            rosys.notify('No row found', 'negative')
+            return False
+        if not len(self.row.points) >= 2:
+            rosys.notify(f'Row {self.row.name} on field {self.field.name} has not enough points', 'negative')
+            return False
+        self.gnss.is_paused = False
+        await rosys.sleep(3)  # wait for GNSS to update
+        self.automation_watcher.start_field_watch(self.field.outline)
+
+        # determine start and end point
+        relative_point_0 = self.odometer.prediction.relative_point(self.row.points[0].cartesian())
+        relative_point_1 = self.odometer.prediction.relative_point(self.row.points[-1].cartesian())
+        self.log.info(f'{relative_point_0=} - {relative_point_1=}')
+        self.start_point = None
+        self.end_point = None
+        if relative_point_0.x < 0 or relative_point_0.x < relative_point_1.x:
+            self.start_point = self.row.points[0].cartesian()
+            self.end_point = self.row.points[-1].cartesian()
+        elif relative_point_1.x < 0 or relative_point_1.x < relative_point_0.x:
+            self.start_point = self.row.points[-1].cartesian()
+            self.end_point = self.row.points[0].cartesian()
+        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
+
+    async def finish(self) -> None:
+        await super().finish()
+        self.automation_watcher.stop_field_watch()
+        await self.implement.deactivate()
+
+    async def _drive(self, distance: float):
+        assert self.field is not None
+        assert self.row is not None
+        assert self.start_point is not None
+        assert self.end_point is not None
+        current_position = self.odometer.prediction.point
+        direction = self.start_point.direction(self.end_point)
+        self.log.info(f'line direction: {direction} robot yaw: {self.odometer.prediction.yaw}')
+        distance_to_end = current_position.distance(self.end_point)
+        distance = min(distance, distance_to_end)
+        near_end = distance > distance_to_end
+        line = self.row.line_segment().line
+        foot_point = line.foot_point(self.odometer.prediction.point)
+        target = foot_point.polar(distance, direction)
+        with self.driver.parameters.set(linear_speed_limit=self.linear_speed_limit, angular_speed_limit=self.angular_speed_limit):
+            await self.driver.drive_to(target, throttle_at_end=near_end, stop_at_end=near_end)
+
+    def _should_finish(self) -> bool:
+        assert self.row is not None
+        distance = self.odometer.prediction.point.distance(self.end_point)
+        if distance < self.STOP_DISTANCE:
+            self.log.info(f'Row {self.row.name} completed')
+            return True
+        if self.bms.is_below_percent(20):
+            self.log.error('Battery is low')
+            return True
+        return False
+
+    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(
+            self.odometer.prediction.point).distance(self.odometer.prediction.point))
+        self.log.info(f'Nearest row is {row.name}')
+        return row
+
+    def _set_field(self, field_id: str) -> None:
+        field = self.field_provider.get_field(field_id)
+        if field is not None:
+            self.field = field
+
+    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 backup(self) -> dict:
+        return super().backup() | {
+            'field_id': self.field.id if self.field else None,
+        }
+
+    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)

field_friend/automations/navigation/coverage_navigation.py

@@ -46,7 +46,7 @@ def __init__(self, system: 'System', implement: Implement) -> None:
         self.continue_mowing: bool = False
 
     def backup(self) -> dict:
-        return {
+        return super().backup() | {
             'padding': self.padding,
             'lane_distance': self.lane_distance,
             'paths': [[rosys.persistence.to_dict(segment) for segment in path] for path in self.paths],
@@ -55,6 +55,7 @@ def backup(self) -> dict:
         }
 
     def restore(self, data: dict[str, Any]) -> None:
+        super().restore(data)
         self.padding = data.get('padding', self.padding)
         self.lane_distance = data.get('lane_distance', self.lane_distance)
         paths_data = data.get('paths', [])