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.yml b/docker-compose.jetson.orin.yml index 886d2967..7cfab951 100644 --- a/docker-compose.jetson.orin.yml +++ b/docker-compose.jetson.orin.yml @@ -6,7 +6,7 @@ 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" @@ -14,16 +14,13 @@ services: 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" 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/field_provider.py b/field_friend/automations/field_provider.py index 7f7a0767..2b21486b 100644 --- a/field_friend/automations/field_provider.py +++ b/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) diff --git a/field_friend/automations/implements/weeding_implement.py b/field_friend/automations/implements/weeding_implement.py index 09f90b7b..d9859301 100644 --- a/field_friend/automations/implements/weeding_implement.py +++ b/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) \ diff --git a/field_friend/automations/implements/weeding_screw.py b/field_friend/automations/implements/weeding_screw.py index 14669b6b..46119dd3 100644 --- a/field_friend/automations/implements/weeding_screw.py +++ b/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 diff --git a/field_friend/automations/navigation/__init__.py b/field_friend/automations/navigation/__init__.py index 6cabac1f..c0a34c65 100644 --- a/field_friend/automations/navigation/__init__.py +++ b/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', ] 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/navigation/navigation.py b/field_friend/automations/navigation/navigation.py index dab5b836..a6c60dc9 100644 --- a/field_friend/automations/navigation/navigation.py +++ b/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: diff --git a/field_friend/automations/plant_locator.py b/field_friend/automations/plant_locator.py index a23ce1e7..f6092f77 100644 --- a/field_friend/automations/plant_locator.py +++ b/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 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/field_planner.py b/field_friend/interface/components/field_planner.py index 822974ef..86faf8aa 100644 --- a/field_friend/interface/components/field_planner.py +++ b/field_friend/interface/components/field_planner.py @@ -27,7 +27,7 @@ def __init__(self, system: 'System', leaflet: leaflet_map) -> None: self.field_provider = system.field_provider self.odometer = system.odometer self.gnss = system.gnss - self.cultivatable_crops = system.crop_category_names + self.cultivatable_crops = system.plant_locator.crop_category_names self.leaflet_map = leaflet self.tab: TabType = "Plants" self.active_object: ActiveObject | None = None @@ -54,7 +54,7 @@ def __init__(self, system: 'System', leaflet: leaflet_map) -> None: with ui.row(): ui.button('Clear fields', on_click=self.clear_field_dialog.open).props("outline color=warning") \ .tooltip("Delete all fields").classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;") - ui.button("Update reference", on_click=self.field_provider.update_reference).props("outline color=warning") \ + ui.button("Update reference", on_click=self.gnss.update_reference).props("outline color=warning") \ .tooltip("Set current position as geo reference and restart the system").classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;") self.show_field_settings() self.show_object_settings() 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/interface/components/plant_object.py b/field_friend/interface/components/plant_object.py index 3db2265a..eb4be64f 100644 --- a/field_friend/interface/components/plant_object.py +++ b/field_friend/interface/components/plant_object.py @@ -1,18 +1,20 @@ import logging +from typing import TYPE_CHECKING import rosys from nicegui.elements.scene_objects import Group, Sphere -from ...automations import PlantProvider +if TYPE_CHECKING: + from system import System class plant_objects(Group): - def __init__(self, plant_provider: PlantProvider, weed_category_names: list[str]) -> None: + def __init__(self, system: 'System') -> None: super().__init__() - self.plant_provider = plant_provider - self.weed_category_names = weed_category_names + self.plant_provider = system.plant_provider + self.plant_locator = system.plant_locator self.log = logging.getLogger('field_friend.plant_objects') self.update() self.plant_provider.PLANTS_CHANGED.register_ui(self.update) @@ -29,7 +31,7 @@ def update(self) -> None: obj.delete() for id, plant in in_world.items(): if id not in rendered: - if plant.type in self.weed_category_names: + if plant.type in self.plant_locator.weed_category_names: Sphere(0.02).with_name(f'plant_{plant.type}:{id}') \ .material('#ef1208') \ .move(plant.position.x, plant.position.y, 0.02) diff --git a/field_friend/interface/components/robot_scene.py b/field_friend/interface/components/robot_scene.py index 4af25ffa..01b762a6 100644 --- a/field_friend/interface/components/robot_scene.py +++ b/field_friend/interface/components/robot_scene.py @@ -33,8 +33,7 @@ def toggle_lock(): with ui.scene(200, 200, on_click=self.handle_click, grid=False).classes('w-full') as self.scene: field_friend_object(self.system.odometer, self.system.camera_provider, self.system.field_friend) rosys.driving.driver_object(self.system.driver) - plant_objects(self.system.plant_provider, - self.system.big_weed_category_names + self.system.small_weed_category_names) + plant_objects(self.system) visualizer_object(self.system) field_object(self.system.field_provider, self.system.field_navigation.field) self.scene.move_camera(-0.5, -1, 2) diff --git a/field_friend/interface/components/status_dev.py b/field_friend/interface/components/status_dev.py index b8c27b79..dbc503fb 100644 --- a/field_friend/interface/components/status_dev.py +++ b/field_friend/interface/components/status_dev.py @@ -330,8 +330,9 @@ def update_status() -> None: m2_status.text = 'Error in m2' if robot.mower.m2_error else 'No error' gnss_device_label.text = 'No connection' if system.gnss.device is None else 'Connected' - reference_position_label.text = 'No reference' if localization.reference is None else 'Set' - gnss_label.text = str(system.gnss.current.location) if system.gnss.current is not None else 'No position' + reference_position_label.text = 'No reference' if localization.reference is None or ( + localization.reference.lat == 0 and localization.reference.long == 0) else str(localization.reference) + gnss_label.text = 'No position' if system.gnss.current is None else str(system.gnss.current.location) heading_label.text = f'{system.gnss.current.heading:.2f}° {direction_flag}' if system.gnss.current is not None and system.gnss.current.heading is not None else 'No heading' rtk_fix_label.text = f'gps_qual: {system.gnss.current.gps_qual}, mode: {system.gnss.current.mode}' if system.gnss.current is not None else 'No fix' gnss_paused_label.text = str(system.gnss.is_paused) diff --git a/field_friend/interface/pages/main_page.py b/field_friend/interface/pages/main_page.py index b1a6d102..d7bc5886 100644 --- a/field_friend/interface/pages/main_page.py +++ b/field_friend/interface/pages/main_page.py @@ -21,6 +21,7 @@ def page() -> None: def content(self, devmode) -> None: page_height = '50vh' if devmode else 'calc(100vh - 170px)' ui.colors(primary='#6E93D6', secondary='#53B689', accent='#111B1E', positive='#53B689') + self.system.gnss.reference_warning_dialog() with ui.row().style(f'height:{page_height}; width: calc(100vw - 2rem); flex-wrap: nowrap;'): with ui.column().classes('h-full w-1/2 p-2'): leaflet = leaflet_map(self.system, False) diff --git a/field_friend/localization/gnss.py b/field_friend/localization/gnss.py index cc7cc552..36c8657a 100644 --- a/field_friend/localization/gnss.py +++ b/field_friend/localization/gnss.py @@ -1,13 +1,16 @@ from __future__ import annotations import logging +import os from abc import ABC, abstractmethod from copy import deepcopy from dataclasses import dataclass -from typing import Any, Optional +from typing import Any import numpy as np import rosys +from nicegui import ui +from rosys.persistence.registry import backup from .. import localization from .geo_point import GeoPoint @@ -22,7 +25,7 @@ class GNSSRecord: gps_qual: int = 0 altitude: float = 0.0 separation: float = 0.0 - heading: Optional[float] = None + heading: float | None = None speed_kmh: float = 0.0 @@ -30,6 +33,7 @@ class Gnss(rosys.persistence.PersistentModule, ABC): NEEDED_POSES: int = 10 MIN_SECONDS_BETWEEN_UPDATES: float = 10.0 ENSURE_GNSS: bool = False + MAX_DISTANCE_TO_REFERENCE: float = 5000.0 def __init__(self, odometer: rosys.driving.Odometer, antenna_offset: float) -> None: super().__init__() @@ -48,7 +52,7 @@ def __init__(self, odometer: rosys.driving.Odometer, antenna_offset: float) -> N self.GNSS_CONNECTION_LOST = rosys.event.Event() """the GNSS connection was lost""" - self.current: Optional[GNSSRecord] = None + self.current: GNSSRecord | None = None self.device: str | None = None self.antenna_offset = antenna_offset self.is_paused = False @@ -57,6 +61,8 @@ def __init__(self, odometer: rosys.driving.Odometer, antenna_offset: float) -> N self.needed_poses: int = self.NEEDED_POSES self.min_seconds_between_updates: float = self.MIN_SECONDS_BETWEEN_UPDATES self.ensure_gnss: bool = self.ENSURE_GNSS + self.max_distance_to_reference: float = self.MAX_DISTANCE_TO_REFERENCE + self.reference_alert_dialog: ui.dialog self.needs_backup = False rosys.on_repeat(self.check_gnss, 0.01) @@ -88,20 +94,19 @@ async def check_gnss(self) -> None: # TODO also do antenna_offset correction for this event self.ROBOT_GNSS_POSITION_CHANGED.emit(self.current.location) if not self.is_paused and ("R" in self.current.mode or self.current.mode == "SSSS"): - self._on_rtk_fix() + await self._on_rtk_fix() except Exception: self.log.exception('gnss record could not be applied') self.current = None @abstractmethod - async def _create_new_record(self) -> Optional[GNSSRecord]: + async def _create_new_record(self) -> GNSSRecord | None: pass - def _on_rtk_fix(self) -> None: + async def _on_rtk_fix(self) -> None: assert self.current is not None if localization.reference.lat == 0 and localization.reference.long == 0: - self.log.info(f'GNSS reference set to {self.current.location}') - localization.reference = deepcopy(self.current.location) + await self.update_reference() if self.current.heading is not None: yaw = np.deg2rad(-self.current.heading) else: @@ -146,3 +151,26 @@ def restore(self, data: dict[str, Any]) -> None: self.needed_poses = data.get('needed_poses', self.needed_poses) self.min_seconds_between_updates = data.get('min_seconds_between_updates', self.min_seconds_between_updates) self.ensure_gnss = data.get('ensure_gnss', self.ensure_gnss) + + async def update_reference(self) -> None: + if self.current is None: + self.log.warning('No GNSS position available') + return + localization.reference = self.current.location + await backup(force=True) + self.log.info('GNSS reference set to %s', self.current.location) + os.utime('main.py') + + def reference_warning_dialog(self) -> None: + with ui.dialog() as self.reference_alert_dialog, ui.card(): + ui.label('The reference is to far away from the current position which would lead to issues in the navigation. Do you want to set it now?') + with ui.row(): + ui.button("Update reference", on_click=self.update_reference).props("outline color=warning") \ + .tooltip("Set current position as geo reference and restart the system").classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;") + ui.button('Cancel', on_click=self.reference_alert_dialog.close) + + def check_distance_to_reference(self) -> bool: + if self.current is not None and self.current.location.distance(localization.reference) > self.max_distance_to_reference: + self.reference_alert_dialog.open() + return True + return False diff --git a/field_friend/system.py b/field_friend/system.py index c14804e1..58e288ae 100644 --- a/field_friend/system.py +++ b/field_friend/system.py @@ -7,6 +7,8 @@ import psutil import rosys +import config.config_selection as config_selector + from . import localization from .automations import ( AutomationWatcher, @@ -29,6 +31,7 @@ from .automations.navigation import ( ABLineNavigation, CoverageNavigation, + CrossglideDemoNavigation, FieldNavigation, FollowCropsNavigation, Navigation, @@ -47,6 +50,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() @@ -64,7 +68,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: @@ -73,7 +77,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) @@ -81,7 +84,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) @@ -119,17 +121,7 @@ def watch_robot() -> None: self.kpi_provider.increment_on_rising_edge('low_battery', self.field_friend.bms.is_below_percent(10.0)) self.puncher = Puncher(self.field_friend, self.driver, self.kpi_provider) - self.big_weed_category_names = ['big_weed', 'thistle', 'orache', 'kamille', ] - self.small_weed_category_names = ['coin', 'weed',] - self.crop_category_names = [ - 'coin_with_hole', 'sugar_beet', 'onion', 'garlic', 'maize', 'liebstoekel', - 'red_beet', 'kohlrabi', 'schnittlauch', 'petersilie', 'bohnenkraut', 'sauerampfer', - 'oregano', 'pimpinelle', 'borrietsch', 'estragon', 'zitronenmelisse', 'pfefferminze', - 'marokanische_minze', 'thymian', - ] self.plant_locator = PlantLocator(self) - self.plant_locator.weed_category_names = self.big_weed_category_names + self.small_weed_category_names - self.plant_locator.crop_category_names = self.crop_category_names rosys.on_repeat(watch_robot, 1.0) @@ -159,15 +151,15 @@ def watch_robot() -> None: self.straight_line_navigation = StraightLineNavigation(self, self.monitoring) 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.field_navigation = FieldNavigation(self, self.monitoring) + self.crossglide_demo_navigation = CrossglideDemoNavigation(self, self.monitoring) self.navigation_strategies = {n.name: n for n in [self.rows_on_field_navigation, self.straight_line_navigation, self.follow_crops_navigation, self.coverage_navigation, - self.a_b_line_navigation, - self.field_navigation + self.field_navigation, + self.crossglide_demo_navigation, ]} implements: list[Implement] = [self.monitoring] match self.field_friend.implement_name: @@ -249,6 +241,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) diff --git a/tests/test_navigation.py b/tests/test_navigation.py index 198ae826..62bb646e 100644 --- a/tests/test_navigation.py +++ b/tests/test_navigation.py @@ -34,6 +34,7 @@ async def test_straight_line_with_high_angles(system: System): await system.driver.wheels.stop() assert isinstance(system.current_navigation, StraightLineNavigation) system.current_navigation.length = 1.0 + system.gnss.observed_poses.clear() system.automator.start() await forward(until=lambda: system.automator.is_running) await forward(until=lambda: system.automator.is_stopped) diff --git a/tests/test_weeding.py b/tests/test_weeding.py index ac303993..5fd45629 100644 --- a/tests/test_weeding.py +++ b/tests/test_weeding.py @@ -9,7 +9,7 @@ async def test_working_with_weeding_screw(system: System, detector: rosys.vision.DetectorSimulation): detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='maize', position=rosys.geometry.Point3d(x=0.2, y=0.0, z=0))) - detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='thistle', + detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='weed', position=rosys.geometry.Point3d(x=0.2, y=0.05, z=0))) system.current_implement = system.implements['Weed Screw'] system.current_navigation = system.straight_line_navigation @@ -22,7 +22,7 @@ async def test_working_with_weeding_screw(system: System, detector: rosys.vision async def test_keep_crops_safe(system: System, detector: rosys.vision.DetectorSimulation): detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='maize', position=rosys.geometry.Point3d(x=0.2, y=0.0, z=0))) - detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='thistle', + detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='weed', position=rosys.geometry.Point3d(x=0.2, y=system.field_friend.DRILL_RADIUS-0.01, z=0))) system.current_implement = system.implements['Weed Screw'] system.current_navigation = system.straight_line_navigation @@ -32,11 +32,11 @@ async def test_keep_crops_safe(system: System, detector: rosys.vision.DetectorSi assert detector.simulated_objects[0].category_name == 'maize' -@pytest.mark.skip(reason='Needs to be rewritten to ignore specific weeds') +@pytest.mark.skip(reason='We currently do not differentiate between different weed types') async def test_weeding_screw_only_targets_big_weed(system: System, detector: rosys.vision.DetectorSimulation): detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='weed', position=rosys.geometry.Point3d(x=0.2, y=0.0, z=0))) - detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='thistle', + detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='coin', position=rosys.geometry.Point3d(x=0.15, y=0, z=0))) system.current_implement = system.implements['Weed Screw'] system.current_navigation = system.straight_line_navigation @@ -47,9 +47,9 @@ async def test_weeding_screw_only_targets_big_weed(system: System, detector: ros async def test_weeding_screw_does_not_skip_close_weed(system: System, detector: rosys.vision.DetectorSimulation): - detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='thistle', + detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='weed', position=rosys.geometry.Point3d(x=0.2, y=0, z=0))) - detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='thistle', + detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='weed', position=rosys.geometry.Point3d(x=0.2+system.field_friend.DRILL_RADIUS-0.01, y=0.05, z=0))) system.current_implement = system.implements['Weed Screw'] system.current_navigation = system.straight_line_navigation @@ -61,9 +61,9 @@ async def test_weeding_screw_does_not_skip_close_weed(system: System, detector: async def test_weeding_screw_focus_on_weed_close_to_crop(system: System, detector: rosys.vision.DetectorSimulation): detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='maize', position=rosys.geometry.Point3d(x=0.2, y=0.0, z=0))) - detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='thistle', + detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='weed', position=rosys.geometry.Point3d(x=0.1, y=0, z=0))) - detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='thistle', + detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='weed', position=rosys.geometry.Point3d(x=0.16, y=0, z=0))) system.current_implement = system.implements['Weed Screw'] system.current_navigation = system.straight_line_navigation @@ -73,7 +73,7 @@ async def test_weeding_screw_focus_on_weed_close_to_crop(system: System, detecto system.automator.start() await forward(40) assert len(detector.simulated_objects) == 2 - assert detector.simulated_objects[1].category_name == 'thistle' + assert detector.simulated_objects[1].category_name == 'weed' assert detector.simulated_objects[1].position.x == 0.1