Implement Zed X Mini Stereo Camera and setup F15 (#168)

This PR introduces the [igus D1 motor
controller](https://www.igus.de/product/D1) and the [StereoLabs Zed X
Mini stereo
camera](https://www.stereolabs.com/en-de/store/products/zed-x-mini-stereo-camera)
for our new prototype field friend F15.

The code that's controlling the camera hardware is in [this
repository](https://github.com/zauberzeug/zedxmini), but will probably
replaced by the [ROS2 container](https://www.stereolabs.com/docs/ros2)
developed by StereoLabs themselves in the near future, to save the
effort on our end.

Other PRs regarding the development of F15 are these:

- #167
- #174
- #159

---------

Co-authored-by: Johannes-Thiel <[email protected]>
Co-authored-by: Lukas Baecker <[email protected]>

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.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/implements/weeding_implement.py

@@ -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))

field_friend/automations/navigation/__init__.py

@@ -5,6 +5,7 @@
 from .navigation import Navigation, WorkflowException
 from .row_on_field_navigation import RowsOnFieldNavigation
 from .straight_line_navigation import StraightLineNavigation
+from .crossglide_demo_navigation import CrossglideDemoNavigation
 
 __all__ = [
     'Navigation',
@@ -14,4 +15,5 @@
     'FollowCropsNavigation',
     'CoverageNavigation',
     'ABLineNavigation',
+    '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/plant_locator.py

@@ -4,8 +4,11 @@
 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', 'weedy_area', ]
@@ -100,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

field_friend/hardware/axis.py

@@ -57,14 +57,14 @@ async def try_reference(self) -> bool:
         return True
 
     def compute_steps(self, position: float) -> int:
-        """Compute the number of steps to move the y axis to the given position.   
+        """Compute the number of steps to move the axis to the given position.
 
         The position is given in meters.
         """
         return int((position + self.axis_offset) * self.steps_per_m) * (-1 if self.reversed_direction else 1)
 
     def compute_position(self, steps: int) -> float:
-        return steps / self.steps_per_m - self.axis_offset * (-1 if self.reversed_direction else 1)
+        return steps / self.steps_per_m * (-1 if self.reversed_direction else 1) - self.axis_offset
 
     @property
     def position(self) -> float: