Merge commit '81581ef9ebb9cb2b66347a69cc0cd9bab86ab34b' into A-B-fiel…

…dnavigation

config/f15_config_f15/camera.py

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

config/f15_config_f15/hardware.py

@@ -28,6 +28,7 @@
         'min_position': -0.11,
         'max_position': 0.11,
         'axis_offset': 0.115,
+        'reversed_direction': False,
     },
     'z_axis': {
         'version': 'axis_d1',
@@ -38,9 +39,10 @@
         'profile_acceleration': 500000,
         'profile_velocity': 40000,
         'profile_deceleration': 500000,
-        'min_position': 0.230,
-        'max_position': 0,
-        'axis_offset': 0.01,
+        'min_position': -0.230,
+        'max_position': 0.0,
+        'axis_offset': -0.01,
+        'reversed_direction': True,
     },
     'estop': {
         'name': 'estop',
@@ -61,7 +63,11 @@
         'status_pin': 13,
     },
     'flashlight': {
-        'version': 'none',
+        'version': 'flashlight_pwm_v2',
+        'name': 'flashlight',
+        'on_expander': True,
+        'front_pin': 12,
+        'back_pin': 23,
     },
     'bumper': {
         'name': 'bumper',

config/f15_config_f15/params.py

@@ -4,7 +4,8 @@
     'pitch': 0.033,
     'wheel_distance': 0.47,
     'antenna_offset': 0.205,
-    'work_x': 0.0125,
+    'work_x': -0.06933333,
+    'work_y': 0.0094166667,
     'drill_radius': 0.025,
     'tool': 'weed_screw',
 }

docker-compose.jetson.orin.yml

@@ -6,24 +6,21 @@ services:
     privileged: true
 
   detector:
-    image: "zauberzeug/yolov5-detector:0.1.2-nlv0.10.10-35.4.1"
+    image: "zauberzeug/yolov5-detector:0.1.3-nlv0.10.12-35.4.1"
     restart: always
     ports:
       - "8004:80"
     hostname: ${ROBOT_ID}
     environment:
       - HOST=learning-loop.ai
       - NVIDIA_VISIBLE_DEVICES=all
-      # - ORGANIZATION=zauberzeug
-      # - PROJECT=coins
-      - ORGANIZATION=zuckerruebe
-      - PROJECT=uckerbots
+      - ORGANIZATION=feldfreund
+      - PROJECT=plants
     volumes:
-      # - ~/data_coins:/data
       - ~/data_plants:/data
 
   monitoring:
-    image: "zauberzeug/yolov5-detector:0.1.2-nlv0.10.10-35.4.1"
+    image: "zauberzeug/yolov5-detector:0.1.3-nlv0.10.12-35.4.1"
     restart: always
     ports:
       - "8005:80"

docker-compose.jetson.orin.zedxmini.yml

@@ -0,0 +1,20 @@
+version: "3.9"
+services:
+  zedxmini:
+    restart: always
+    privileged: true
+    runtime: nvidia
+    build:
+      context: ../zedxmini
+      dockerfile: ../zedxmini/Dockerfile
+    volumes:
+      - ../zedxmini:/app
+      - /dev:/dev
+      - /tmp:/tmp
+      - /var/nvidia/nvcam/settings/:/var/nvidia/nvcam/settings
+      - /etc/systemd/system/zed_x_daemon.service:/etc/systemd/system/zed_x_daemon.service
+      - /usr/local/zed/resources:/usr/local/zed/resources
+    ports:
+      - "8003:8003"
+    environment:
+      - TZ=Europe/Amsterdam

docker.sh

@@ -68,6 +68,14 @@ case $os in
 esac
 export DOCKER_BUILDKIT=1
 
+if [ -d /usr/local/zed ]; then
+    if [ -d ../zedxmini ]; then
+        compose_args="$compose_args -f docker-compose.jetson.orin.zedxmini.yml"
+    else
+        echo -e "\033[33mWARNING:\033[0m Zed X Mini not found. https://github.com/zauberzeug/zedxmini"
+    fi
+fi
+
 cmd=$1
 cmd_args=${@:2}
 set -x

field_friend.code-workspace

@@ -5,6 +5,9 @@
     },
     {
       "path": "../rosys"
+    },
+    {
+      "path": "../zedxmini"
     }
   ],
   "settings": {},

field_friend/automations/field_provider.py

@@ -1,5 +1,4 @@
 import logging
-import os
 import uuid
 from typing import Any, Optional
 
@@ -96,13 +95,6 @@ def clear_fields(self) -> None:
         self.FIELDS_CHANGED.emit()
         self.invalidate()
 
-    def update_reference(self) -> None:
-        if self.gnss.current is None:
-            rosys.notify('No GNSS position available.')
-            return
-        localization.reference = self.gnss.current.location
-        os.utime('main.py')
-
     def create_obstacle(self, field: Field, points: list[GeoPoint] = []) -> FieldObstacle:
         obstacle = FieldObstacle(id=f'{str(uuid.uuid4())}', name=f'obstacle_{len(field.obstacles)+1}', points=points)
         field.obstacles.append(obstacle)

field_friend/automations/implements/weeding_implement.py

@@ -26,7 +26,7 @@ def __init__(self,  name: str, system: 'System', persistence_key: str = 'weeding
         rosys.persistence.PersistentModule.__init__(self,
                                                     persistence_key=f'field_friend.automations.implements.{persistence_key}')
 
-        self.relevant_weeds = system.small_weed_category_names + system.big_weed_category_names
+        self.relevant_weeds = system.plant_locator.weed_category_names
         self.log = logging.getLogger('field_friend.weeding')
         self.system = system
         self.kpi_provider = system.kpi_provider
@@ -84,6 +84,7 @@ async def deactivate(self):
         self.kpi_provider.increment_weeding_kpi('rows_weeded')
 
     async def start_workflow(self) -> None:
+        # TODO: only sleep when moving
         await rosys.sleep(2)  # wait for robot to stand still
         if not self._has_plants_to_handle():
             return
@@ -154,8 +155,8 @@ def _has_plants_to_handle(self) -> bool:
                     safe_weed_position = Point3d.from_point(Point3d.projection(weed_position).polar(
                         offset, Point3d.projection(crop_position).direction(weed_position)))
                     upcoming_weed_positions[weed] = safe_weed_position
-                    self.log.info(f'Moved weed {weed} from {weed_position} to {safe_weed_position} ' +
-                                  f'by {offset} to safe {crop} at {crop_position}')
+                    # self.log.info(f'Moved weed {weed} from {weed_position} to {safe_weed_position} ' +
+                    #               f'by {offset} to safe {crop} at {crop_position}')
 
         # Sort the upcoming positions so nearest comes first
         sorted_weeds = dict(sorted(upcoming_weed_positions.items(), key=lambda item: item[1].x))
@@ -209,7 +210,7 @@ def _update_time_and_distance(self):
 
     def settings_ui(self):
         super().settings_ui()
-        ui.select(self.system.crop_category_names, label='cultivated crop', on_change=self.request_backup) \
+        ui.select(self.system.plant_locator.crop_category_names, label='cultivated crop', on_change=self.request_backup) \
             .bind_value(self, 'cultivated_crop').props('clearable') \
             .classes('w-40').tooltip('Set the cultivated crop which should be kept safe')
         ui.number('Crop safety distance', step=0.001, min=0.001, max=0.05, format='%.3f', on_change=self.request_backup) \

field_friend/automations/implements/weeding_screw.py

@@ -14,7 +14,7 @@ class WeedingScrew(WeedingImplement):
 
     def __init__(self, system: 'System') -> None:
         super().__init__('Weed Screw', system, 'weeding_screw')
-        self.relevant_weeds = system.small_weed_category_names + system.big_weed_category_names
+        self.relevant_weeds = system.plant_locator.weed_category_names
         self.log.info(f'Using relevant weeds: {self.relevant_weeds}')
         self.weed_screw_depth: float = 0.13
         self.max_crop_distance: float = 0.08

field_friend/automations/navigation/__init__.py

@@ -1,6 +1,7 @@
 
 from .a_b_line_navigation import ABLineNavigation
 from .coverage_navigation import CoverageNavigation
+from .crossglide_demo_navigation import CrossglideDemoNavigation
 from .field_navigation import FieldNavigation
 from .follow_crops_navigation import FollowCropsNavigation
 from .navigation import Navigation, WorkflowException
@@ -16,4 +17,5 @@
     'CoverageNavigation',
     'ABLineNavigation',
     'FieldNavigation',
+    'CrossglideDemoNavigation',
 ]

field_friend/automations/navigation/crossglide_demo_navigation.py

@@ -0,0 +1,79 @@
+from typing import TYPE_CHECKING, Any
+
+import numpy as np
+import rosys
+
+from ...automations.implements.implement import Implement
+from .navigation import Navigation
+
+if TYPE_CHECKING:
+    from system import System
+
+
+class WorkflowException(Exception):
+    pass
+
+
+class CrossglideDemoNavigation(Navigation):
+
+    def __init__(self, system: 'System', tool: Implement) -> None:
+        super().__init__(system, tool)
+        self.MAX_STRETCH_DISTANCE: float = 5.0
+        self.detector = system.detector
+        self.name = 'Crossglide Demo'
+        self.origin: rosys.geometry.Point
+        self.target: rosys.geometry.Point
+
+    async def prepare(self) -> bool:
+        await super().prepare()
+        self.log.info(f'Activating {self.implement.name}...')
+        await self.implement.activate()
+        return True
+
+    async def start(self) -> None:
+        try:
+            await self.implement.stop_workflow()
+            if not await self.implement.prepare():
+                self.log.error('Tool-Preparation failed')
+                return
+            if not await self.prepare():
+                self.log.error('Preparation failed')
+                return
+            if isinstance(self.driver.wheels, rosys.hardware.WheelsSimulation) and not rosys.is_test:
+                self.create_simulation()
+            self.log.info('Navigation started')
+            while not self._should_finish():
+                self.implement.next_punch_y_position = np.random.uniform(-0.11, 0.1)
+                await self.implement.start_workflow()
+        except WorkflowException as e:
+            self.kpi_provider.increment_weeding_kpi('automation_stopped')
+            self.log.error(f'WorkflowException: {e}')
+        finally:
+            self.kpi_provider.increment_weeding_kpi('weeding_completed')
+            await self.implement.finish()
+            await self.finish()
+            await self.driver.wheels.stop()
+
+    async def finish(self) -> None:
+        await super().finish()
+        await self.implement.deactivate()
+
+    async def _drive(self, distance: float) -> None:
+        pass
+
+    def _should_finish(self) -> bool:
+        return False
+
+    def create_simulation(self):
+        pass
+        # TODO: implement create_simulation
+
+    def settings_ui(self) -> None:
+        super().settings_ui()
+
+    def backup(self) -> dict:
+        return super().backup() | {
+        }
+
+    def restore(self, data: dict[str, Any]) -> None:
+        super().restore(data)

field_friend/automations/navigation/navigation.py

@@ -46,6 +46,8 @@ async def start(self) -> None:
                 self.log.error('Preparation failed')
                 return
             await self.gnss.update_robot_pose()
+            if self.gnss.check_distance_to_reference():
+                raise WorkflowException('reference to far away from robot')
             self.start_position = self.odometer.prediction.point
             if isinstance(self.driver.wheels, rosys.hardware.WheelsSimulation) and not rosys.is_test:
                 self.create_simulation()
@@ -65,6 +67,7 @@ async def start(self) -> None:
         except WorkflowException as e:
             self.kpi_provider.increment_weeding_kpi('automation_stopped')
             self.log.error(f'WorkflowException: {e}')
+            rosys.notify(f'An exception occurred during automation: {e}', 'negative')
         finally:
             self.kpi_provider.increment_weeding_kpi('weeding_completed')
             await self.implement.finish()
@@ -79,6 +82,7 @@ async def prepare(self) -> bool:
         if isinstance(self.detector, rosys.vision.DetectorSimulation) and not rosys.is_test:
             self.detector.simulated_objects = []
         self.log.info('clearing plant provider')
+        await self.gnss.update_robot_pose()
         return True
 
     async def finish(self) -> None:

field_friend/automations/plant_locator.py

@@ -4,12 +4,16 @@
 import aiohttp
 import rosys
 from nicegui import ui
+from rosys.geometry import Point3d
 from rosys.vision import Autoupload
 
+from ..vision import CalibratableUsbCamera
+from ..vision.zedxmini_camera import StereoCamera
 from .plant import Plant
 
-WEED_CATEGORY_NAME = ['coin', 'weed', 'big_weed', 'thistle', 'orache', 'weedy_area', ]
-CROP_CATEGORY_NAME = ['coin_with_hole', 'crop', 'sugar_beet', 'onion', 'garlic', ]
+WEED_CATEGORY_NAME = ['coin', 'weed', 'weedy_area', ]
+CROP_CATEGORY_NAME = ['coin_with_hole', 'borrietsch', 'estragon', 'feldsalat', 'garlic', 'jasione', 'kohlrabi', 'liebstoeckel', 'maize', 'minze', 'onion',
+                      'oregano_majoran', 'pastinake', 'petersilie', 'pimpinelle', 'red_beet', 'salatkopf', 'schnittlauch', 'sugar_beet', 'thymian_bohnenkraut', 'zitronenmelisse', ]
 MINIMUM_CROP_CONFIDENCE = 0.3
 MINIMUM_WEED_CONFIDENCE = 0.3
 
@@ -99,7 +103,20 @@ async def _detect_plants(self) -> None:
                 if d.cx < dead_zone or d.cx > new_image.size.width - dead_zone or d.cy < dead_zone:
                     continue
             image_point = rosys.geometry.Point(x=d.cx, y=d.cy)
-            world_point_3d = camera.calibration.project_from_image(image_point)
+            world_point_3d: rosys.geometry.Point3d | None = None
+            if isinstance(camera, StereoCamera):
+                world_point_3d = camera.calibration.project_from_image(image_point)
+                # TODO: 3d detection
+                # camera_point_3d: Point3d | None = await camera.get_point(
+                #     int(d.cx), int(d.cy))
+                # if camera_point_3d is None:
+                #     self.log.error('could not get a depth value for detection')
+                #     continue
+                # camera.calibration.extrinsics = camera.calibration.extrinsics.as_frame(
+                #     'zedxmini').in_frame(self.odometer.prediction_frame)
+                # world_point_3d = camera_point_3d.in_frame(camera.calibration.extrinsics).resolve()
+            else:
+                world_point_3d = camera.calibration.project_from_image(image_point)
             if world_point_3d is None:
                 self.log.error('could not generate world point of detection, calibration error')
                 continue

field_friend/automations/puncher.py

@@ -63,6 +63,7 @@ async def punch(self,
                     turns: float = 2.0,
                     with_open_tornado: bool = False,
                     ) -> None:
+        y += self.field_friend.WORK_Y
         self.log.info(f'Punching at {y} with depth {depth}...')
         rest_position = 'reference'
         if self.field_friend.y_axis is None or self.field_friend.z_axis is None:
@@ -78,7 +79,7 @@ async def punch(self,
                     rosys.notify('homing failed!', type='negative')
                     self.log.error('homing failed!')
                     raise PuncherException('homing failed')
-                await rosys.sleep(0.5)
+                # await rosys.sleep(0.5)
             if isinstance(self.field_friend.y_axis, ChainAxis):
                 if not self.field_friend.y_axis.min_position <= y <= self.field_friend.y_axis.max_position:
                     rosys.notify('y position out of range', type='negative')
@@ -120,7 +121,7 @@ async def clear_view(self) -> None:
             await self.field_friend.y_axis.return_to_reference()
             return
         elif isinstance(self.field_friend.y_axis, Axis):
-            if isinstance(self.field_friend.z_axis,Axis):
+            if isinstance(self.field_friend.z_axis, Axis):
                 if self.field_friend.z_axis.position != 0:
                     await self.field_friend.z_axis.return_to_reference()
             y = self.field_friend.y_axis.min_position if self.field_friend.y_axis.position <= 0 else self.field_friend.y_axis.max_position