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