Implement Zed X Mini Stereo Camera and setup F15

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,10 @@ case $os in
 esac
 export DOCKER_BUILDKIT=1
 
+if [ -d /usr/local/zed ] && [ -d ../zedxmini ]; then
+    compose_args="$compose_args -f docker-compose.jetson.orin.zedxmini.yml"
+fi
+
 cmd=$1
 cmd_args=${@:2}
 set -x

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:

field_friend/hardware/axis_D1.py

@@ -19,6 +19,7 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
                  profile_velocity: int = 20,
                  profile_acceleration: int = 200,
                  profile_deceleration: int = 400,
+                 reverse_direction: bool = False,
 
 
                  ** kwargs
@@ -32,7 +33,7 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
         """
         self.name = name
         self.statusword: int = 0
-        self._position: int = 0
+        self.steps: int = 0
         self.velocity: int = 0
 
         # flags of the Statusword for more information refer to the CANopen standard and D1 manual
@@ -56,7 +57,7 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
             {self.name}_motor.homing_acceleration = {homing_acceleration}
             {self.name}_motor.switch_search_speed = {homing_velocity}
             {self.name}_motor.zero_search_speed = {homing_velocity}
-            {self.name}_motor.profile_acceleration = {profile_acceleration} 
+            {self.name}_motor.profile_acceleration = {profile_acceleration}
             {self.name}_motor.profile_deceleration = {profile_deceleration}
             {self.name}_motor.profile_velocity = {profile_velocity}
         ''')
@@ -75,24 +76,18 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
             min_position=min_position,
             axis_offset=axis_offset,
             reference_speed=homing_velocity,
-            steps_per_m=0,
-            reversed_direction=False,
+            steps_per_m=D1_STEPS_P_M,
+            reversed_direction=reverse_direction,
             **kwargs)
 
-    @property
-    def position(self) -> float:
-        return (self._position / D1_STEPS_P_M) - self.axis_offset
-
     async def stop(self):
         pass
 
     async def move_to(self, position: float, speed: int | None = None) -> None:
-        if self.is_referenced:
-            await self.robot_brain.send(f'{self.name}_motor.profile_position({self._compute_steps(position)});')
-        if not self.is_referenced:
-            self.log.error(f'AxisD1 {self.name} is not refernced')
-            return
-        while abs(self.position - position) > 0.02:
+        await super().move_to(position)
+        while (abs(self.position - position)) > 0.01:
+            # sometimes the moving command is not executed, so it is send in each loop (for demo purposes)
+            await self.robot_brain.send(f'{self.name}_motor.profile_position({self.compute_steps(position)});')
             await rosys.sleep(0.1)
 
     async def enable_motor(self):
@@ -104,25 +99,27 @@ async def enable_motor(self):
     async def reset_error(self):
         if self.fault:
             await self.robot_brain.send(f'{self.name}_motor.reset()')
-        else: self.log.error(f'AxisD1 {self.name} is not in fault state')
+        else:
+            self.log.error(f'AxisD1 {self.name} is not in fault state')
 
-    async def try_reference(self):
+    async def try_reference(self) -> bool:
         if not self._valid_status():
             await self.enable_motor()
         if self.is_referenced:
             self.log.error(f'AxisD1 {self.name} is already referenced')
         else:
-            #due to some timing issues, the homing command is sent twice
+            # due to some timing issues, the homing command is sent twice
             await self.robot_brain.send(f'{self.name}_motor.home()')
             await self.robot_brain.send(f'{self.name}_motor.home()')
+        return self.is_referenced
 
     async def speed_Mode(self, speed: int):
-        #due to some timing issues, the speed command is sent twice
+        # due to some timing issues, the speed command is sent twice
         await self.robot_brain.send(f'{self.name}_motor.profile_velocity({speed});')
         await self.robot_brain.send(f'{self.name}_motor.profile_velocity({speed});')
 
     def handle_core_output(self, time: float, words: list[str]) -> None:
-        self._position = int(words.pop(0))
+        self.steps = int(words.pop(0))
         self.velocity = int(words.pop(0))
         self.statusword = int(words.pop(0))
         self.is_referenced = int(words.pop(0)) == 1
@@ -131,9 +128,6 @@ def handle_core_output(self, time: float, words: list[str]) -> None:
     def _valid_status(self) -> bool:
         return self.ready_to_switch_on and self.switched_on and self.operation_enabled and self.quick_stop
 
-    def _compute_steps(self, position: float) -> int:
-        return int(abs(position + self.axis_offset) * D1_STEPS_P_M)
-
     def _split_statusword(self) -> None:
         self.ready_to_switch_on = ((self.statusword >> 0) & 1) == 1
         self.switched_on = ((self.statusword >> 1) & 1) == 1