From d1ed761d1400ba79b1e62ec010b1498155eabaca Mon Sep 17 00:00:00 2001 From: Pascal Schade <165774906+pascalzauberzeug@users.noreply.github.com> Date: Wed, 25 Sep 2024 17:12:29 +0200 Subject: [PATCH] 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: - https://github.com/zauberzeug/field_friend/pull/167 - https://github.com/zauberzeug/field_friend/pull/174 - https://github.com/zauberzeug/field_friend/pull/159 --------- Co-authored-by: Johannes-Thiel Co-authored-by: Lukas Baecker --- config/f15_config_f15/camera.py | 24 +-- config/f15_config_f15/hardware.py | 14 +- config/f15_config_f15/params.py | 3 +- docker-compose.jetson.orin.zedxmini.yml | 20 +++ docker.sh | 8 + field_friend.code-workspace | 3 + .../implements/weeding_implement.py | 5 +- .../automations/navigation/__init__.py | 2 + .../navigation/crossglide_demo_navigation.py | 79 +++++++++ field_friend/automations/plant_locator.py | 18 +- field_friend/automations/puncher.py | 5 +- field_friend/hardware/axis.py | 4 +- field_friend/hardware/axis_D1.py | 40 ++--- field_friend/hardware/field_friend.py | 1 + .../hardware/field_friend_hardware.py | 11 +- .../hardware/field_friend_simulation.py | 2 + .../components/calibration_dialog.py | 12 +- .../interface/components/camera_card.py | 41 +++-- .../interface/components/hardware_control.py | 6 +- field_friend/system.py | 26 ++- .../vision/zedxmini_camera/__init__.py | 8 + .../vision/zedxmini_camera/zedxmini_camera.py | 167 ++++++++++++++++++ .../zedxmini_camera_provider.py | 48 +++++ sync.py | 5 + 24 files changed, 479 insertions(+), 73 deletions(-) create mode 100644 docker-compose.jetson.orin.zedxmini.yml create mode 100644 field_friend/automations/navigation/crossglide_demo_navigation.py create mode 100644 field_friend/vision/zedxmini_camera/__init__.py create mode 100644 field_friend/vision/zedxmini_camera/zedxmini_camera.py create mode 100644 field_friend/vision/zedxmini_camera/zedxmini_camera_provider.py diff --git a/config/f15_config_f15/camera.py b/config/f15_config_f15/camera.py index ab04f53c..1f333f70 100644 --- a/config/f15_config_f15/camera.py +++ b/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, + } } diff --git a/config/f15_config_f15/hardware.py b/config/f15_config_f15/hardware.py index c8041ca1..61529c16 100644 --- a/config/f15_config_f15/hardware.py +++ b/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', diff --git a/config/f15_config_f15/params.py b/config/f15_config_f15/params.py index c6937e7a..e2af7b20 100644 --- a/config/f15_config_f15/params.py +++ b/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', } diff --git a/docker-compose.jetson.orin.zedxmini.yml b/docker-compose.jetson.orin.zedxmini.yml new file mode 100644 index 00000000..2fc64ad6 --- /dev/null +++ b/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 diff --git a/docker.sh b/docker.sh index 7f354cbf..d7678279 100755 --- a/docker.sh +++ b/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 diff --git a/field_friend.code-workspace b/field_friend.code-workspace index 6316280b..a5c182e6 100644 --- a/field_friend.code-workspace +++ b/field_friend.code-workspace @@ -5,6 +5,9 @@ }, { "path": "../rosys" + }, + { + "path": "../zedxmini" } ], "settings": {}, diff --git a/field_friend/automations/implements/weeding_implement.py b/field_friend/automations/implements/weeding_implement.py index 505c9bfe..d9859301 100644 --- a/field_friend/automations/implements/weeding_implement.py +++ b/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)) diff --git a/field_friend/automations/navigation/__init__.py b/field_friend/automations/navigation/__init__.py index 794c902f..23f0f50e 100644 --- a/field_friend/automations/navigation/__init__.py +++ b/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', ] diff --git a/field_friend/automations/navigation/crossglide_demo_navigation.py b/field_friend/automations/navigation/crossglide_demo_navigation.py new file mode 100644 index 00000000..60d88225 --- /dev/null +++ b/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) diff --git a/field_friend/automations/plant_locator.py b/field_friend/automations/plant_locator.py index b7b5e255..25088a78 100644 --- a/field_friend/automations/plant_locator.py +++ b/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 diff --git a/field_friend/automations/puncher.py b/field_friend/automations/puncher.py index 0291f01d..f6b6ed93 100644 --- a/field_friend/automations/puncher.py +++ b/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 diff --git a/field_friend/hardware/axis.py b/field_friend/hardware/axis.py index b0aedfdd..75e7e29c 100644 --- a/field_friend/hardware/axis.py +++ b/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: diff --git a/field_friend/hardware/axis_D1.py b/field_friend/hardware/axis_D1.py index aa61f62c..b2988d15 100644 --- a/field_friend/hardware/axis_D1.py +++ b/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,29 @@ 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()') + while not self.is_referenced: + await rosys.sleep(0.1) + 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 +130,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 diff --git a/field_friend/hardware/field_friend.py b/field_friend/hardware/field_friend.py index 2ec9f3b6..8546b579 100644 --- a/field_friend/hardware/field_friend.py +++ b/field_friend/hardware/field_friend.py @@ -19,6 +19,7 @@ class FieldFriend(rosys.hardware.Robot): WHEEL_DIAMETER = 0.041 * 17 / np.pi M_PER_TICK = WHEEL_DIAMETER * np.pi / MOTOR_GEAR_RATIO WORK_X = 0.118 + WORK_Y = 0.0 DRILL_RADIUS = 0.025 CHOP_RADIUS = 0.07 WORK_X_CHOP = 0.04 diff --git a/field_friend/hardware/field_friend_hardware.py b/field_friend/hardware/field_friend_hardware.py index 9849dde7..d3f99287 100644 --- a/field_friend/hardware/field_friend_hardware.py +++ b/field_friend/hardware/field_friend_hardware.py @@ -45,6 +45,8 @@ def __init__(self) -> None: implement: str = config_params['tool'] if implement in ['tornado', 'weed_screw', 'none']: self.WORK_X: float = config_params['work_x'] + if 'work_y' in config_params: + self.WORK_Y: float = config_params['work_y'] self.DRILL_RADIUS: float = config_params['drill_radius'] elif implement in ['dual_mechanism']: self.WORK_X_CHOP: float = config_params['work_x_chop'] @@ -59,7 +61,8 @@ def __init__(self) -> None: communication = rosys.hardware.SerialCommunication() if 'enable_esp_on_startup' in config_robotbrain['robot_brain']: - robot_brain = rosys.hardware.RobotBrain(communication,enable_esp_on_startup=config_robotbrain['robot_brain']['enable_esp_on_startup']) + robot_brain = rosys.hardware.RobotBrain( + communication, enable_esp_on_startup=config_robotbrain['robot_brain']['enable_esp_on_startup']) else: robot_brain = rosys.hardware.RobotBrain(communication) robot_brain.lizard_firmware.flash_params += config_robotbrain['robot_brain']['flash_params'] @@ -121,7 +124,8 @@ def __init__(self) -> None: profile_deceleration=config_hardware['y_axis']['profile_deceleration'], max_position=config_hardware['y_axis']['max_position'], min_position=config_hardware['y_axis']['min_position'], - axis_offset=config_hardware['y_axis']['axis_offset'],) + axis_offset=config_hardware['y_axis']['axis_offset'], + reverse_direction=config_hardware['y_axis']['reversed_direction'],) elif config_hardware['y_axis']['version'] == 'chain_axis': y_axis = ChainAxisHardware(robot_brain, expander=expander, @@ -220,7 +224,8 @@ def __init__(self) -> None: profile_deceleration=config_hardware['z_axis']['profile_deceleration'], max_position=config_hardware['z_axis']['max_position'], min_position=config_hardware['z_axis']['min_position'], - axis_offset=config_hardware['z_axis']['axis_offset'],) + axis_offset=config_hardware['z_axis']['axis_offset'], + reverse_direction=config_hardware['z_axis']['reversed_direction'],) elif config_hardware['z_axis']['version'] == 'tornado': z_axis = TornadoHardware(robot_brain, expander=expander, diff --git a/field_friend/hardware/field_friend_simulation.py b/field_friend/hardware/field_friend_simulation.py index f94860de..f13cd1f0 100644 --- a/field_friend/hardware/field_friend_simulation.py +++ b/field_friend/hardware/field_friend_simulation.py @@ -29,6 +29,8 @@ def __init__(self, robot_id) -> None: tool = config_params['tool'] if tool in ['tornado', 'weed_screw', 'none']: self.WORK_X = config_params['work_x'] + if 'work_y' in config_params: + self.WORK_Y = config_params['work_y'] self.DRILL_RADIUS = config_params['drill_radius'] elif tool in ['dual_mechanism']: self.WORK_X_CHOP = config_params['work_x_chop'] diff --git a/field_friend/interface/components/calibration_dialog.py b/field_friend/interface/components/calibration_dialog.py index 85d240c6..0018df5c 100644 --- a/field_friend/interface/components/calibration_dialog.py +++ b/field_friend/interface/components/calibration_dialog.py @@ -10,7 +10,8 @@ from rosys.geometry import Point3d from rosys.vision import Calibration, Camera, CameraProvider, SimulatedCalibratableCamera -from ...vision import DOT_DISTANCE, Dot, Network +from ...vision import DOT_DISTANCE, CalibratableUsbCamera, Dot, Network +from ...vision.zedxmini_camera import ZedxminiCamera class calibration_dialog(ui.dialog): @@ -150,7 +151,7 @@ def handle_mouse(self, e: events.MouseEventArguments) -> None: if self.moving_dot: self.moving_dot.x = e.image_x self.moving_dot.y = e.image_y - self.render() + # self.render() if e.type == 'mouseup': if self.moving_dot is not None: self.network.try_refine(self.moving_dot) @@ -193,7 +194,12 @@ def apply_calibration(self) -> None: try: if not self.calibration: raise ValueError('No calibration created') - self.camera.calibration = self.calibration + if isinstance(self.camera, CalibratableUsbCamera): + self.camera.calibration = self.calibration + elif isinstance(self.camera, ZedxminiCamera): + self.camera.setup_calibration(self.camera.camera_information) + self.camera.calibration.extrinsics = self.calibration.extrinsics + self.camera.calibration.extrinsics.as_frame(self.camera.id) except Exception as e: self.camera.calibration = None ui.notify(str(e)) diff --git a/field_friend/interface/components/camera_card.py b/field_friend/interface/components/camera_card.py index 2ecb2067..f8046c7a 100644 --- a/field_friend/interface/components/camera_card.py +++ b/field_friend/interface/components/camera_card.py @@ -10,8 +10,9 @@ from field_friend.automations.implements.weeding_implement import WeedingImplement -from ...automations import PlantLocator, Puncher from ...hardware import Axis, FlashlightPWM, FlashlightPWMV2, Tornado +from ...vision import CalibratableUsbCamera +from ...vision.zedxmini_camera import StereoCamera from .calibration_dialog import calibration_dialog if TYPE_CHECKING: @@ -32,7 +33,7 @@ def __init__(self, system: 'System') -> None: self.puncher = system.puncher self.system = system self.punching_enabled: bool = False - self.shrink_factor: float = 2.0 + self.shrink_factor: float = 4.0 self.show_weeds_to_handle: bool = False self.camera: Optional[rosys.vision.CalibratableCamera] = None self.image_view: Optional[ui.interactive_image] = None @@ -118,9 +119,11 @@ def update_content(self) -> None: url = f'{active_camera.get_latest_image_url()}?shrink={self.shrink_factor}' else: url = active_camera.get_latest_image_url() - if self.image_view is None or self.camera.calibration is None: + if self.image_view is None: return self.image_view.set_source(url) + if self.camera.calibration is None: + return image = active_camera.latest_detected_image svg = '' if image and image.detections: @@ -132,19 +135,31 @@ def update_content(self) -> None: def on_mouse_move(self, e: MouseEventArguments): if self.camera is None: return - if e.type == 'mousemove': - point2d = Point(x=e.image_x, y=e.image_y) - if self.camera.calibration is None: - self.debug_position.set_text(f'{point2d} no calibration') - return + + point2d = Point(x=e.image_x, y=e.image_y) + if self.camera.calibration is None: + self.debug_position.set_text(f'{point2d} no calibration') + return + point3d: rosys.geometry.Point3d | None = None + if isinstance(self.camera, CalibratableUsbCamera): point3d = self.camera.calibration.project_from_image(point2d) + elif isinstance(self.camera, StereoCamera): + # TODO: too many calls + # camera_point_3d: Point3d | None = self.camera.get_point( + # int(point2d.x), int(point2d.y)) + # if camera_point_3d is None: + # return + # camera_point_3d = camera_point_3d.in_frame(self.odometer.prediction_frame) + # world_point_3d = camera_point_3d.resolve() + # self.log.info(f'camera_point_3d: {camera_point_3d} -> world_point_3d: {world_point_3d.tuple}') + pass + + if e.type == 'mousemove' and point3d is not None: self.debug_position.set_text(f'screen {point2d} -> local {point3d}') if e.type == 'mouseup': - point2d = Point(x=e.image_x, y=e.image_y) if self.camera.calibration is None: self.debug_position.set_text(f'last punch: {point2d}') return - point3d = self.camera.calibration.project_from_image(point2d) if point3d is not None: self.debug_position.set_text(f'last punch: {point2d} -> {point3d}') if self.puncher is not None and self.punching_enabled: @@ -220,11 +235,11 @@ def build_svg_for_plant_provider(self) -> str: if self.camera is None or self.camera.calibration is None: return '' position = rosys.geometry.Point3d(x=self.camera.calibration.extrinsics.translation[0], - y=self.camera.calibration.extrinsics.translation[1]) + y=self.camera.calibration.extrinsics.translation[1], + z=0) svg = '' for plant in self.plant_provider.get_relevant_weeds(position): - position_3d = rosys.geometry.Point3d(x=plant.position.x, y=plant.position.y, z=0) - screen = self.camera.calibration.project_to_image(position_3d) + screen = self.camera.calibration.project_to_image(plant.position) if screen is not None: svg += f'' svg += f'{plant.id[:4]}' diff --git a/field_friend/interface/components/hardware_control.py b/field_friend/interface/components/hardware_control.py index 33f796bd..b6eed5c8 100644 --- a/field_friend/interface/components/hardware_control.py +++ b/field_friend/interface/components/hardware_control.py @@ -3,8 +3,8 @@ from nicegui.events import ValueChangeEventArguments from ...automations import Puncher -from ...hardware import (ChainAxis, FieldFriend, FieldFriendHardware, Flashlight, FlashlightPWM, FlashlightPWMV2, - FlashlightV2, Mower, MowerHardware, MowerSimulation, Tornado, Axis, YAxisCanOpenHardware, +from ...hardware import (Axis, ChainAxis, FieldFriend, FieldFriendHardware, Flashlight, FlashlightPWM, FlashlightPWMV2, + FlashlightV2, Mower, MowerHardware, MowerSimulation, Tornado, YAxisCanOpenHardware, ZAxisCanOpenHardware) from .confirm_dialog import ConfirmDialog as confirm_dialog from .status_bulb import StatusBulb as status_bulb @@ -142,7 +142,7 @@ async def toggle_flashlight(e: ValueChangeEventArguments) -> None: puncher.punch(field_friend.y_axis.max_position, depth=depth.value))) ui.button(on_click=lambda: automator.start(puncher.punch(0, depth=depth.value))) ui.button(on_click=lambda: automator.start( - puncher.punch(field_friend.y_axis.min_position, depth=depth.value))) + puncher.punch(field_friend.y_axis.min_position - field_friend.WORK_Y, depth=depth.value))) if isinstance(field_friend.mower, Mower): with ui.column(): diff --git a/field_friend/system.py b/field_friend/system.py index 70898fdb..839d7ba0 100644 --- a/field_friend/system.py +++ b/field_friend/system.py @@ -7,12 +7,14 @@ import psutil import rosys +import config.config_selection as config_selector + from . import localization from .automations import (AutomationWatcher, BatteryWatcher, FieldProvider, KpiProvider, PathProvider, PlantLocator, PlantProvider, Puncher) from .automations.implements import ChopAndScrew, ExternalMower, Implement, Recorder, Tornado, WeedingScrew -from .automations.navigation import (ABLineNavigation, CoverageNavigation, FollowCropsNavigation, Navigation, - RowsOnFieldNavigation, StraightLineNavigation) +from .automations.navigation import (ABLineNavigation, CoverageNavigation, CrossglideDemoNavigation, + FollowCropsNavigation, Navigation, RowsOnFieldNavigation, StraightLineNavigation) from .hardware import FieldFriend, FieldFriendHardware, FieldFriendSimulation, TeltonikaRouter from .interface.components.info import Info from .kpi_generator import generate_kpis @@ -20,6 +22,7 @@ from .localization.gnss_hardware import GnssHardware from .localization.gnss_simulation import GnssSimulation from .vision import CalibratableUsbCameraProvider, CameraConfigurator +from .vision.zedxmini_camera import ZedxminiCameraProvider icecream.install() @@ -37,7 +40,7 @@ def __init__(self) -> None: self.is_real = rosys.hardware.SerialCommunication.is_possible() self.AUTOMATION_CHANGED = rosys.event.Event() - self.camera_provider: CalibratableUsbCameraProvider | rosys.vision.SimulatedCameraProvider + self.camera_provider = self.setup_camera_provider() self.detector: rosys.vision.DetectorHardware | rosys.vision.DetectorSimulation self.field_friend: FieldFriend if self.is_real: @@ -46,7 +49,6 @@ def __init__(self) -> None: self.teltonika_router = TeltonikaRouter() except Exception: self.log.exception(f'failed to initialize FieldFriendHardware {self.version}') - self.camera_provider = CalibratableUsbCameraProvider() self.mjpeg_camera_provider = rosys.vision.MjpegCameraProvider(username='root', password='zauberzg!') self.detector = rosys.vision.DetectorHardware(port=8004) self.monitoring_detector = rosys.vision.DetectorHardware(port=8005) @@ -54,7 +56,6 @@ def __init__(self) -> None: self.camera_configurator = CameraConfigurator(self.camera_provider, odometer=self.odometer) else: self.field_friend = FieldFriendSimulation(robot_id=self.version) - self.camera_provider = rosys.vision.SimulatedCameraProvider() # NOTE we run this in rosys.startup to enforce setup AFTER the persistence is loaded rosys.on_startup(self.setup_simulated_usb_camera) self.detector = rosys.vision.DetectorSimulation(self.camera_provider) @@ -122,11 +123,13 @@ def watch_robot() -> None: 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.crossglide_demo_navigation = CrossglideDemoNavigation(self, self.monitoring) self.navigation_strategies = {n.name: n for n in [self.field_navigation, self.straight_line_navigation, self.follow_crops_navigation, self.coverage_navigation, - self.a_b_line_navigation + self.a_b_line_navigation, + self.crossglide_demo_navigation ]} implements: list[Implement] = [self.monitoring] match self.field_friend.implement_name: @@ -208,6 +211,17 @@ def current_navigation(self, navigation: Navigation) -> None: self.AUTOMATION_CHANGED.emit(navigation.name) self.request_backup() + def setup_camera_provider(self) -> CalibratableUsbCameraProvider | rosys.vision.SimulatedCameraProvider | ZedxminiCameraProvider: + if not self.is_real: + return rosys.vision.SimulatedCameraProvider() + camera_config = config_selector.import_config(module='camera') + camera_type = camera_config.get('type', 'CalibratableUsbCamera') + if camera_type == 'CalibratableUsbCamera': + return CalibratableUsbCameraProvider() + if camera_type == 'ZedxminiCamera': + return ZedxminiCameraProvider() + raise NotImplementedError(f'Unknown camera type: {camera_type}') + async def setup_simulated_usb_camera(self): self.camera_provider.remove_all_cameras() camera = rosys.vision.SimulatedCalibratableCamera.create_calibrated(id='bottom_cam', diff --git a/field_friend/vision/zedxmini_camera/__init__.py b/field_friend/vision/zedxmini_camera/__init__.py new file mode 100644 index 00000000..457fa50c --- /dev/null +++ b/field_friend/vision/zedxmini_camera/__init__.py @@ -0,0 +1,8 @@ +from .zedxmini_camera import StereoCamera, ZedxminiCamera +from .zedxmini_camera_provider import ZedxminiCameraProvider + +__all__ = [ + 'StereoCamera', + 'ZedxminiCamera', + 'ZedxminiCameraProvider', +] diff --git a/field_friend/vision/zedxmini_camera/zedxmini_camera.py b/field_friend/vision/zedxmini_camera/zedxmini_camera.py new file mode 100644 index 00000000..b1f7f23c --- /dev/null +++ b/field_friend/vision/zedxmini_camera/zedxmini_camera.py @@ -0,0 +1,167 @@ +import abc +import asyncio +import logging +from typing import Any, Self + +import aiohttp +import rosys +from rosys import persistence +from rosys.geometry import Point3d +from rosys.vision import CalibratableCamera, Calibration, Image, ImageSize, Intrinsics + + +class StereoCamera(CalibratableCamera, abc.ABC): + @abc.abstractmethod + async def get_point(self, x, y) -> Point3d | None: + pass + + +class ZedxminiCamera(StereoCamera): + ip: str = 'localhost' + port: int = 8003 + + def __init__(self, ip: str | None = None, port: int | None = None, focal_length: float = 747.0735473632812, **kwargs) -> None: + self.MAX_IMAGES = 10 + super().__init__(**kwargs) + if ip is not None: + self.ip = ip + if port is not None: + self.port = port + self.focal_length = focal_length + self.connected: bool = False + self.log = logging.getLogger(self.name) + self.log.setLevel(logging.DEBUG) + self.camera_information: dict[str, Any] = {} + + async def connect(self) -> None: + await super().connect() + self.connected = await self.setup_camera_information() + + @staticmethod + async def get_camera_information(ip: str | None = None, port: int | None = None) -> dict[str, Any] | None: + ip = ZedxminiCamera.ip if ip is None else ip + port = ZedxminiCamera.port if port is None else port + url = f'http://{ip}:{port}/information' + data: dict[str, Any] | None = None + async with aiohttp.ClientSession() as session: + try: + async with session.get(url, timeout=2.0) as response: + if response.status != 200: + logging.warning(f"response.status: {response.status}") + return None + data = await response.json() + except aiohttp.ClientError as e: + logging.error(f"Error capturing image: {str(e)}") + return None + except asyncio.TimeoutError: + logging.error("Request timed out") + return None + if data is None: + return + return data + + async def setup_camera_information(self) -> bool: + camera_information = await self.get_camera_information(self.ip, self.port) + if camera_information is None: + return False + assert 'calibration' in camera_information + assert 'left_cam' in camera_information['calibration'] + assert 'fx' in camera_information['calibration']['left_cam'] + self.camera_information = camera_information + self.focal_length = camera_information['calibration']['left_cam']['fy'] + return True + + async def capture_image(self) -> None: + if not self.connected: + return + url = f'http://{self.ip}:{self.port}/image' + data: dict[str, Any] | None = None + async with aiohttp.ClientSession() as session: + try: + async with session.get(url, timeout=2.0) as response: + if response.status != 200: + self.log.warning(f"response.status: {response.status}") + return + data = await response.json() + except aiohttp.ClientError as e: + self.log.error(f"Error capturing image: {str(e)}") + except asyncio.TimeoutError: + self.log.error("Request timed out") + if data is None: + return + assert 'image' in data + image_bytes = await rosys.run.cpu_bound(bytes.fromhex, data['image']) + image = Image( + camera_id=data['camera_id'], + size=ImageSize(width=data['width'], height=data['height']), + time=data['time'], + data=image_bytes, + is_broken=data['is_broken'], + tags=set(data['tags']), + ) + self._add_image(image) + + async def get_point(self, x, y) -> Point3d | None: + url = f'http://{self.ip}:{self.port}/point?x={x}&y={y}' + data: dict[str, Any] | None = None + async with aiohttp.ClientSession() as session: + try: + async with session.get(url, timeout=2.0) as response: + if response.status != 200: + self.log.warning(f"response.status: {response.status}") + return None + data = await response.json() + except aiohttp.ClientError as e: + self.log.error(f"Error capturing image: {str(e)}") + except asyncio.TimeoutError: + self.log.error("Request timed out") + if data is None: + return None + assert 'x' in data + assert 'y' in data + assert 'z' in data + return Point3d(**data) + + @property + def is_connected(self) -> bool: + # TODO: check it in capture_image + return self.connected + + def setup_calibration(self, camera_dict: dict) -> None: + assert 'resolution' in camera_dict + assert 'calibration' in camera_dict + assert 'left_cam' in camera_dict['calibration'] + width = camera_dict['resolution'][0] + height = camera_dict['resolution'][1] + fx = camera_dict['calibration']['left_cam']['fx'] + fy = camera_dict['calibration']['left_cam']['fy'] + self.focal_length = (fx + fy) / 2.0 + fy = camera_dict['calibration']['left_cam']['fy'] + cx = camera_dict['calibration']['left_cam']['cx'] + cy = camera_dict['calibration']['left_cam']['cy'] + k1 = camera_dict['calibration']['left_cam']['k1'] + k2 = camera_dict['calibration']['left_cam']['k2'] + p1 = camera_dict['calibration']['left_cam']['p1'] + p2 = camera_dict['calibration']['left_cam']['p2'] + k3 = camera_dict['calibration']['left_cam']['k3'] + + size = ImageSize(width=width, height=height) + K: list[list[float]] = [[fx, 0.0, cx], + [0.0, fy, cy], + [0.0, 0.0, 1.0]] + D: list[float] = [k1, k2, p1, p2, k3] + intrinsics = Intrinsics(matrix=K, distortion=D, size=size) + self.calibration = Calibration(intrinsics=intrinsics) + + @classmethod + def from_dict(cls, data: dict[str, Any]) -> Self: + logging.info(f'zedxmini - from_dict - data: {data}') + return cls(**(data | { + 'calibration': persistence.from_dict(rosys.vision.Calibration, data['calibration']) if data.get('calibration') else None, + })) + + def to_dict(self) -> dict: + logging.info('zedxmini - to_dict') + return super().to_dict() | { + 'focal_length': self.focal_length, + } diff --git a/field_friend/vision/zedxmini_camera/zedxmini_camera_provider.py b/field_friend/vision/zedxmini_camera/zedxmini_camera_provider.py new file mode 100644 index 00000000..c580a262 --- /dev/null +++ b/field_friend/vision/zedxmini_camera/zedxmini_camera_provider.py @@ -0,0 +1,48 @@ +import logging + +import rosys + +from .zedxmini_camera import ZedxminiCamera + +SCAN_INTERVAL = 10 + + +class ZedxminiCameraProvider(rosys.vision.CameraProvider[ZedxminiCamera], rosys.persistence.PersistentModule): + + def __init__(self) -> None: + super().__init__() + self.log = logging.getLogger('field_friend.zedxmini_camera_provider') + + rosys.on_shutdown(self.shutdown) + rosys.on_repeat(self.update_device_list, SCAN_INTERVAL) + rosys.on_startup(self.update_device_list) + + def backup(self) -> dict: + for camera in self._cameras.values(): + self.log.info(f'backing up camera: {camera.to_dict()}') + return { + 'cameras': {camera.id: camera.to_dict() for camera in self._cameras.values()} + } + + def restore(self, data: dict[str, dict]) -> None: + for camera_data in data.get('cameras', {}).values(): + self.add_camera(ZedxminiCamera.from_dict(camera_data)) + + async def update_device_list(self) -> None: + if len(self._cameras) == 0: + camera_information = await ZedxminiCamera.get_camera_information('localhost', 8003) + if camera_information is None: + return + self.add_camera(ZedxminiCamera(id=str(camera_information['serial_number']), polling_interval=0.1)) + camera = list(self._cameras.values())[0] + if camera.is_connected: + return + await camera.reconnect() + + async def shutdown(self) -> None: + for camera in self._cameras.values(): + await camera.disconnect() + + @staticmethod + def is_operable() -> bool: + return True diff --git a/sync.py b/sync.py index 2261d8c3..aa46e8fc 100755 --- a/sync.py +++ b/sync.py @@ -7,6 +7,7 @@ parser = argparse.ArgumentParser(description='Sync local code with robot.') parser.add_argument('robot', help='Robot hostname') parser.add_argument('--rosys', action='store_true', default=False, help='Sync rosys') +parser.add_argument('--zedxmini', action='store_true', default=False, help='Sync zedxmini') args = parser.parse_args() touch = 'touch ~/field_friend/main.py' @@ -16,4 +17,8 @@ else: print('Ensuring we have no local rosys on the robot') run_subprocess(f'ssh {args.robot} "rm -rf ~/field_friend/rosys"') + +if args.zedxmini: + folders.append(Folder('../zedxmini', f'{args.robot}:~/zedxmini', on_change=touch)) + sync(*folders)