diff --git a/config/f15_config_f15/camera.py b/config/f15_config_f15/camera.py
index ab04f53c..1f333f70 100644
--- a/config/f15_config_f15/camera.py
+++ b/config/f15_config_f15/camera.py
@@ -1,13 +1,15 @@
-configuration = {'parameters': {
- 'width': 1280,
- 'height': 720,
- 'auto_exposure': True,
- 'fps': 10,
-},
+configuration = {
+ 'type': 'ZedxminiCamera',
+ 'parameters': {
+ 'width': 1280,
+ 'height': 720,
+ 'auto_exposure': True,
+ 'fps': 10,
+ },
'crop': {
- 'left': 60,
- 'right': 200,
- 'up': 20,
- 'down': 0,
-}
+ 'left': 60,
+ 'right': 200,
+ 'up': 20,
+ 'down': 0,
+ }
}
diff --git a/config/f15_config_f15/hardware.py b/config/f15_config_f15/hardware.py
index c8041ca1..61529c16 100644
--- a/config/f15_config_f15/hardware.py
+++ b/config/f15_config_f15/hardware.py
@@ -28,6 +28,7 @@
'min_position': -0.11,
'max_position': 0.11,
'axis_offset': 0.115,
+ 'reversed_direction': False,
},
'z_axis': {
'version': 'axis_d1',
@@ -38,9 +39,10 @@
'profile_acceleration': 500000,
'profile_velocity': 40000,
'profile_deceleration': 500000,
- 'min_position': 0.230,
- 'max_position': 0,
- 'axis_offset': 0.01,
+ 'min_position': -0.230,
+ 'max_position': 0.0,
+ 'axis_offset': -0.01,
+ 'reversed_direction': True,
},
'estop': {
'name': 'estop',
@@ -61,7 +63,11 @@
'status_pin': 13,
},
'flashlight': {
- 'version': 'none',
+ 'version': 'flashlight_pwm_v2',
+ 'name': 'flashlight',
+ 'on_expander': True,
+ 'front_pin': 12,
+ 'back_pin': 23,
},
'bumper': {
'name': 'bumper',
diff --git a/config/f15_config_f15/params.py b/config/f15_config_f15/params.py
index c6937e7a..e2af7b20 100644
--- a/config/f15_config_f15/params.py
+++ b/config/f15_config_f15/params.py
@@ -4,7 +4,8 @@
'pitch': 0.033,
'wheel_distance': 0.47,
'antenna_offset': 0.205,
- 'work_x': 0.0125,
+ 'work_x': -0.06933333,
+ 'work_y': 0.0094166667,
'drill_radius': 0.025,
'tool': 'weed_screw',
}
diff --git a/docker-compose.jetson.orin.zedxmini.yml b/docker-compose.jetson.orin.zedxmini.yml
new file mode 100644
index 00000000..2fc64ad6
--- /dev/null
+++ b/docker-compose.jetson.orin.zedxmini.yml
@@ -0,0 +1,20 @@
+version: "3.9"
+services:
+ zedxmini:
+ restart: always
+ privileged: true
+ runtime: nvidia
+ build:
+ context: ../zedxmini
+ dockerfile: ../zedxmini/Dockerfile
+ volumes:
+ - ../zedxmini:/app
+ - /dev:/dev
+ - /tmp:/tmp
+ - /var/nvidia/nvcam/settings/:/var/nvidia/nvcam/settings
+ - /etc/systemd/system/zed_x_daemon.service:/etc/systemd/system/zed_x_daemon.service
+ - /usr/local/zed/resources:/usr/local/zed/resources
+ ports:
+ - "8003:8003"
+ environment:
+ - TZ=Europe/Amsterdam
diff --git a/docker.sh b/docker.sh
index 7f354cbf..d7678279 100755
--- a/docker.sh
+++ b/docker.sh
@@ -68,6 +68,14 @@ case $os in
esac
export DOCKER_BUILDKIT=1
+if [ -d /usr/local/zed ]; then
+ if [ -d ../zedxmini ]; then
+ compose_args="$compose_args -f docker-compose.jetson.orin.zedxmini.yml"
+ else
+ echo -e "\033[33mWARNING:\033[0m Zed X Mini not found. https://github.com/zauberzeug/zedxmini"
+ fi
+fi
+
cmd=$1
cmd_args=${@:2}
set -x
diff --git a/field_friend.code-workspace b/field_friend.code-workspace
index 6316280b..a5c182e6 100644
--- a/field_friend.code-workspace
+++ b/field_friend.code-workspace
@@ -5,6 +5,9 @@
},
{
"path": "../rosys"
+ },
+ {
+ "path": "../zedxmini"
}
],
"settings": {},
diff --git a/field_friend/automations/implements/weeding_implement.py b/field_friend/automations/implements/weeding_implement.py
index 505c9bfe..d9859301 100644
--- a/field_friend/automations/implements/weeding_implement.py
+++ b/field_friend/automations/implements/weeding_implement.py
@@ -84,6 +84,7 @@ async def deactivate(self):
self.kpi_provider.increment_weeding_kpi('rows_weeded')
async def start_workflow(self) -> None:
+ # TODO: only sleep when moving
await rosys.sleep(2) # wait for robot to stand still
if not self._has_plants_to_handle():
return
@@ -154,8 +155,8 @@ def _has_plants_to_handle(self) -> bool:
safe_weed_position = Point3d.from_point(Point3d.projection(weed_position).polar(
offset, Point3d.projection(crop_position).direction(weed_position)))
upcoming_weed_positions[weed] = safe_weed_position
- self.log.info(f'Moved weed {weed} from {weed_position} to {safe_weed_position} ' +
- f'by {offset} to safe {crop} at {crop_position}')
+ # self.log.info(f'Moved weed {weed} from {weed_position} to {safe_weed_position} ' +
+ # f'by {offset} to safe {crop} at {crop_position}')
# Sort the upcoming positions so nearest comes first
sorted_weeds = dict(sorted(upcoming_weed_positions.items(), key=lambda item: item[1].x))
diff --git a/field_friend/automations/navigation/__init__.py b/field_friend/automations/navigation/__init__.py
index 794c902f..23f0f50e 100644
--- a/field_friend/automations/navigation/__init__.py
+++ b/field_friend/automations/navigation/__init__.py
@@ -5,6 +5,7 @@
from .navigation import Navigation, WorkflowException
from .row_on_field_navigation import RowsOnFieldNavigation
from .straight_line_navigation import StraightLineNavigation
+from .crossglide_demo_navigation import CrossglideDemoNavigation
__all__ = [
'Navigation',
@@ -14,4 +15,5 @@
'FollowCropsNavigation',
'CoverageNavigation',
'ABLineNavigation',
+ 'CrossglideDemoNavigation',
]
diff --git a/field_friend/automations/navigation/crossglide_demo_navigation.py b/field_friend/automations/navigation/crossglide_demo_navigation.py
new file mode 100644
index 00000000..60d88225
--- /dev/null
+++ b/field_friend/automations/navigation/crossglide_demo_navigation.py
@@ -0,0 +1,79 @@
+from typing import TYPE_CHECKING, Any
+
+import numpy as np
+import rosys
+
+from ...automations.implements.implement import Implement
+from .navigation import Navigation
+
+if TYPE_CHECKING:
+ from system import System
+
+
+class WorkflowException(Exception):
+ pass
+
+
+class CrossglideDemoNavigation(Navigation):
+
+ def __init__(self, system: 'System', tool: Implement) -> None:
+ super().__init__(system, tool)
+ self.MAX_STRETCH_DISTANCE: float = 5.0
+ self.detector = system.detector
+ self.name = 'Crossglide Demo'
+ self.origin: rosys.geometry.Point
+ self.target: rosys.geometry.Point
+
+ async def prepare(self) -> bool:
+ await super().prepare()
+ self.log.info(f'Activating {self.implement.name}...')
+ await self.implement.activate()
+ return True
+
+ async def start(self) -> None:
+ try:
+ await self.implement.stop_workflow()
+ if not await self.implement.prepare():
+ self.log.error('Tool-Preparation failed')
+ return
+ if not await self.prepare():
+ self.log.error('Preparation failed')
+ return
+ if isinstance(self.driver.wheels, rosys.hardware.WheelsSimulation) and not rosys.is_test:
+ self.create_simulation()
+ self.log.info('Navigation started')
+ while not self._should_finish():
+ self.implement.next_punch_y_position = np.random.uniform(-0.11, 0.1)
+ await self.implement.start_workflow()
+ except WorkflowException as e:
+ self.kpi_provider.increment_weeding_kpi('automation_stopped')
+ self.log.error(f'WorkflowException: {e}')
+ finally:
+ self.kpi_provider.increment_weeding_kpi('weeding_completed')
+ await self.implement.finish()
+ await self.finish()
+ await self.driver.wheels.stop()
+
+ async def finish(self) -> None:
+ await super().finish()
+ await self.implement.deactivate()
+
+ async def _drive(self, distance: float) -> None:
+ pass
+
+ def _should_finish(self) -> bool:
+ return False
+
+ def create_simulation(self):
+ pass
+ # TODO: implement create_simulation
+
+ def settings_ui(self) -> None:
+ super().settings_ui()
+
+ def backup(self) -> dict:
+ return super().backup() | {
+ }
+
+ def restore(self, data: dict[str, Any]) -> None:
+ super().restore(data)
diff --git a/field_friend/automations/plant_locator.py b/field_friend/automations/plant_locator.py
index b7b5e255..25088a78 100644
--- a/field_friend/automations/plant_locator.py
+++ b/field_friend/automations/plant_locator.py
@@ -4,8 +4,11 @@
import aiohttp
import rosys
from nicegui import ui
+from rosys.geometry import Point3d
from rosys.vision import Autoupload
+from ..vision import CalibratableUsbCamera
+from ..vision.zedxmini_camera import StereoCamera
from .plant import Plant
WEED_CATEGORY_NAME = ['coin', 'weed', 'big_weed', 'weedy_area', ]
@@ -100,7 +103,20 @@ async def _detect_plants(self) -> None:
if d.cx < dead_zone or d.cx > new_image.size.width - dead_zone or d.cy < dead_zone:
continue
image_point = rosys.geometry.Point(x=d.cx, y=d.cy)
- world_point_3d = camera.calibration.project_from_image(image_point)
+ world_point_3d: rosys.geometry.Point3d | None = None
+ if isinstance(camera, StereoCamera):
+ world_point_3d = camera.calibration.project_from_image(image_point)
+ # TODO: 3d detection
+ # camera_point_3d: Point3d | None = await camera.get_point(
+ # int(d.cx), int(d.cy))
+ # if camera_point_3d is None:
+ # self.log.error('could not get a depth value for detection')
+ # continue
+ # camera.calibration.extrinsics = camera.calibration.extrinsics.as_frame(
+ # 'zedxmini').in_frame(self.odometer.prediction_frame)
+ # world_point_3d = camera_point_3d.in_frame(camera.calibration.extrinsics).resolve()
+ else:
+ world_point_3d = camera.calibration.project_from_image(image_point)
if world_point_3d is None:
self.log.error('could not generate world point of detection, calibration error')
continue
diff --git a/field_friend/automations/puncher.py b/field_friend/automations/puncher.py
index 0291f01d..f6b6ed93 100644
--- a/field_friend/automations/puncher.py
+++ b/field_friend/automations/puncher.py
@@ -63,6 +63,7 @@ async def punch(self,
turns: float = 2.0,
with_open_tornado: bool = False,
) -> None:
+ y += self.field_friend.WORK_Y
self.log.info(f'Punching at {y} with depth {depth}...')
rest_position = 'reference'
if self.field_friend.y_axis is None or self.field_friend.z_axis is None:
@@ -78,7 +79,7 @@ async def punch(self,
rosys.notify('homing failed!', type='negative')
self.log.error('homing failed!')
raise PuncherException('homing failed')
- await rosys.sleep(0.5)
+ # await rosys.sleep(0.5)
if isinstance(self.field_friend.y_axis, ChainAxis):
if not self.field_friend.y_axis.min_position <= y <= self.field_friend.y_axis.max_position:
rosys.notify('y position out of range', type='negative')
@@ -120,7 +121,7 @@ async def clear_view(self) -> None:
await self.field_friend.y_axis.return_to_reference()
return
elif isinstance(self.field_friend.y_axis, Axis):
- if isinstance(self.field_friend.z_axis,Axis):
+ if isinstance(self.field_friend.z_axis, Axis):
if self.field_friend.z_axis.position != 0:
await self.field_friend.z_axis.return_to_reference()
y = self.field_friend.y_axis.min_position if self.field_friend.y_axis.position <= 0 else self.field_friend.y_axis.max_position
diff --git a/field_friend/hardware/axis.py b/field_friend/hardware/axis.py
index b0aedfdd..75e7e29c 100644
--- a/field_friend/hardware/axis.py
+++ b/field_friend/hardware/axis.py
@@ -57,14 +57,14 @@ async def try_reference(self) -> bool:
return True
def compute_steps(self, position: float) -> int:
- """Compute the number of steps to move the y axis to the given position.
+ """Compute the number of steps to move the axis to the given position.
The position is given in meters.
"""
return int((position + self.axis_offset) * self.steps_per_m) * (-1 if self.reversed_direction else 1)
def compute_position(self, steps: int) -> float:
- return steps / self.steps_per_m - self.axis_offset * (-1 if self.reversed_direction else 1)
+ return steps / self.steps_per_m * (-1 if self.reversed_direction else 1) - self.axis_offset
@property
def position(self) -> float:
diff --git a/field_friend/hardware/axis_D1.py b/field_friend/hardware/axis_D1.py
index aa61f62c..b2988d15 100644
--- a/field_friend/hardware/axis_D1.py
+++ b/field_friend/hardware/axis_D1.py
@@ -19,6 +19,7 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
profile_velocity: int = 20,
profile_acceleration: int = 200,
profile_deceleration: int = 400,
+ reverse_direction: bool = False,
** kwargs
@@ -32,7 +33,7 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
"""
self.name = name
self.statusword: int = 0
- self._position: int = 0
+ self.steps: int = 0
self.velocity: int = 0
# flags of the Statusword for more information refer to the CANopen standard and D1 manual
@@ -56,7 +57,7 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
{self.name}_motor.homing_acceleration = {homing_acceleration}
{self.name}_motor.switch_search_speed = {homing_velocity}
{self.name}_motor.zero_search_speed = {homing_velocity}
- {self.name}_motor.profile_acceleration = {profile_acceleration}
+ {self.name}_motor.profile_acceleration = {profile_acceleration}
{self.name}_motor.profile_deceleration = {profile_deceleration}
{self.name}_motor.profile_velocity = {profile_velocity}
''')
@@ -75,24 +76,18 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *,
min_position=min_position,
axis_offset=axis_offset,
reference_speed=homing_velocity,
- steps_per_m=0,
- reversed_direction=False,
+ steps_per_m=D1_STEPS_P_M,
+ reversed_direction=reverse_direction,
**kwargs)
- @property
- def position(self) -> float:
- return (self._position / D1_STEPS_P_M) - self.axis_offset
-
async def stop(self):
pass
async def move_to(self, position: float, speed: int | None = None) -> None:
- if self.is_referenced:
- await self.robot_brain.send(f'{self.name}_motor.profile_position({self._compute_steps(position)});')
- if not self.is_referenced:
- self.log.error(f'AxisD1 {self.name} is not refernced')
- return
- while abs(self.position - position) > 0.02:
+ await super().move_to(position)
+ while (abs(self.position - position)) > 0.01:
+ # sometimes the moving command is not executed, so it is send in each loop (for demo purposes)
+ await self.robot_brain.send(f'{self.name}_motor.profile_position({self.compute_steps(position)});')
await rosys.sleep(0.1)
async def enable_motor(self):
@@ -104,25 +99,29 @@ async def enable_motor(self):
async def reset_error(self):
if self.fault:
await self.robot_brain.send(f'{self.name}_motor.reset()')
- else: self.log.error(f'AxisD1 {self.name} is not in fault state')
+ else:
+ self.log.error(f'AxisD1 {self.name} is not in fault state')
- async def try_reference(self):
+ async def try_reference(self) -> bool:
if not self._valid_status():
await self.enable_motor()
if self.is_referenced:
self.log.error(f'AxisD1 {self.name} is already referenced')
else:
- #due to some timing issues, the homing command is sent twice
+ # due to some timing issues, the homing command is sent twice
await self.robot_brain.send(f'{self.name}_motor.home()')
await self.robot_brain.send(f'{self.name}_motor.home()')
+ while not self.is_referenced:
+ await rosys.sleep(0.1)
+ return self.is_referenced
async def speed_Mode(self, speed: int):
- #due to some timing issues, the speed command is sent twice
+ # due to some timing issues, the speed command is sent twice
await self.robot_brain.send(f'{self.name}_motor.profile_velocity({speed});')
await self.robot_brain.send(f'{self.name}_motor.profile_velocity({speed});')
def handle_core_output(self, time: float, words: list[str]) -> None:
- self._position = int(words.pop(0))
+ self.steps = int(words.pop(0))
self.velocity = int(words.pop(0))
self.statusword = int(words.pop(0))
self.is_referenced = int(words.pop(0)) == 1
@@ -131,9 +130,6 @@ def handle_core_output(self, time: float, words: list[str]) -> None:
def _valid_status(self) -> bool:
return self.ready_to_switch_on and self.switched_on and self.operation_enabled and self.quick_stop
- def _compute_steps(self, position: float) -> int:
- return int(abs(position + self.axis_offset) * D1_STEPS_P_M)
-
def _split_statusword(self) -> None:
self.ready_to_switch_on = ((self.statusword >> 0) & 1) == 1
self.switched_on = ((self.statusword >> 1) & 1) == 1
diff --git a/field_friend/hardware/field_friend.py b/field_friend/hardware/field_friend.py
index 2ec9f3b6..8546b579 100644
--- a/field_friend/hardware/field_friend.py
+++ b/field_friend/hardware/field_friend.py
@@ -19,6 +19,7 @@ class FieldFriend(rosys.hardware.Robot):
WHEEL_DIAMETER = 0.041 * 17 / np.pi
M_PER_TICK = WHEEL_DIAMETER * np.pi / MOTOR_GEAR_RATIO
WORK_X = 0.118
+ WORK_Y = 0.0
DRILL_RADIUS = 0.025
CHOP_RADIUS = 0.07
WORK_X_CHOP = 0.04
diff --git a/field_friend/hardware/field_friend_hardware.py b/field_friend/hardware/field_friend_hardware.py
index 9849dde7..d3f99287 100644
--- a/field_friend/hardware/field_friend_hardware.py
+++ b/field_friend/hardware/field_friend_hardware.py
@@ -45,6 +45,8 @@ def __init__(self) -> None:
implement: str = config_params['tool']
if implement in ['tornado', 'weed_screw', 'none']:
self.WORK_X: float = config_params['work_x']
+ if 'work_y' in config_params:
+ self.WORK_Y: float = config_params['work_y']
self.DRILL_RADIUS: float = config_params['drill_radius']
elif implement in ['dual_mechanism']:
self.WORK_X_CHOP: float = config_params['work_x_chop']
@@ -59,7 +61,8 @@ def __init__(self) -> None:
communication = rosys.hardware.SerialCommunication()
if 'enable_esp_on_startup' in config_robotbrain['robot_brain']:
- robot_brain = rosys.hardware.RobotBrain(communication,enable_esp_on_startup=config_robotbrain['robot_brain']['enable_esp_on_startup'])
+ robot_brain = rosys.hardware.RobotBrain(
+ communication, enable_esp_on_startup=config_robotbrain['robot_brain']['enable_esp_on_startup'])
else:
robot_brain = rosys.hardware.RobotBrain(communication)
robot_brain.lizard_firmware.flash_params += config_robotbrain['robot_brain']['flash_params']
@@ -121,7 +124,8 @@ def __init__(self) -> None:
profile_deceleration=config_hardware['y_axis']['profile_deceleration'],
max_position=config_hardware['y_axis']['max_position'],
min_position=config_hardware['y_axis']['min_position'],
- axis_offset=config_hardware['y_axis']['axis_offset'],)
+ axis_offset=config_hardware['y_axis']['axis_offset'],
+ reverse_direction=config_hardware['y_axis']['reversed_direction'],)
elif config_hardware['y_axis']['version'] == 'chain_axis':
y_axis = ChainAxisHardware(robot_brain,
expander=expander,
@@ -220,7 +224,8 @@ def __init__(self) -> None:
profile_deceleration=config_hardware['z_axis']['profile_deceleration'],
max_position=config_hardware['z_axis']['max_position'],
min_position=config_hardware['z_axis']['min_position'],
- axis_offset=config_hardware['z_axis']['axis_offset'],)
+ axis_offset=config_hardware['z_axis']['axis_offset'],
+ reverse_direction=config_hardware['z_axis']['reversed_direction'],)
elif config_hardware['z_axis']['version'] == 'tornado':
z_axis = TornadoHardware(robot_brain,
expander=expander,
diff --git a/field_friend/hardware/field_friend_simulation.py b/field_friend/hardware/field_friend_simulation.py
index f94860de..f13cd1f0 100644
--- a/field_friend/hardware/field_friend_simulation.py
+++ b/field_friend/hardware/field_friend_simulation.py
@@ -29,6 +29,8 @@ def __init__(self, robot_id) -> None:
tool = config_params['tool']
if tool in ['tornado', 'weed_screw', 'none']:
self.WORK_X = config_params['work_x']
+ if 'work_y' in config_params:
+ self.WORK_Y = config_params['work_y']
self.DRILL_RADIUS = config_params['drill_radius']
elif tool in ['dual_mechanism']:
self.WORK_X_CHOP = config_params['work_x_chop']
diff --git a/field_friend/interface/components/calibration_dialog.py b/field_friend/interface/components/calibration_dialog.py
index 85d240c6..0018df5c 100644
--- a/field_friend/interface/components/calibration_dialog.py
+++ b/field_friend/interface/components/calibration_dialog.py
@@ -10,7 +10,8 @@
from rosys.geometry import Point3d
from rosys.vision import Calibration, Camera, CameraProvider, SimulatedCalibratableCamera
-from ...vision import DOT_DISTANCE, Dot, Network
+from ...vision import DOT_DISTANCE, CalibratableUsbCamera, Dot, Network
+from ...vision.zedxmini_camera import ZedxminiCamera
class calibration_dialog(ui.dialog):
@@ -150,7 +151,7 @@ def handle_mouse(self, e: events.MouseEventArguments) -> None:
if self.moving_dot:
self.moving_dot.x = e.image_x
self.moving_dot.y = e.image_y
- self.render()
+ # self.render()
if e.type == 'mouseup':
if self.moving_dot is not None:
self.network.try_refine(self.moving_dot)
@@ -193,7 +194,12 @@ def apply_calibration(self) -> None:
try:
if not self.calibration:
raise ValueError('No calibration created')
- self.camera.calibration = self.calibration
+ if isinstance(self.camera, CalibratableUsbCamera):
+ self.camera.calibration = self.calibration
+ elif isinstance(self.camera, ZedxminiCamera):
+ self.camera.setup_calibration(self.camera.camera_information)
+ self.camera.calibration.extrinsics = self.calibration.extrinsics
+ self.camera.calibration.extrinsics.as_frame(self.camera.id)
except Exception as e:
self.camera.calibration = None
ui.notify(str(e))
diff --git a/field_friend/interface/components/camera_card.py b/field_friend/interface/components/camera_card.py
index 2ecb2067..f8046c7a 100644
--- a/field_friend/interface/components/camera_card.py
+++ b/field_friend/interface/components/camera_card.py
@@ -10,8 +10,9 @@
from field_friend.automations.implements.weeding_implement import WeedingImplement
-from ...automations import PlantLocator, Puncher
from ...hardware import Axis, FlashlightPWM, FlashlightPWMV2, Tornado
+from ...vision import CalibratableUsbCamera
+from ...vision.zedxmini_camera import StereoCamera
from .calibration_dialog import calibration_dialog
if TYPE_CHECKING:
@@ -32,7 +33,7 @@ def __init__(self, system: 'System') -> None:
self.puncher = system.puncher
self.system = system
self.punching_enabled: bool = False
- self.shrink_factor: float = 2.0
+ self.shrink_factor: float = 4.0
self.show_weeds_to_handle: bool = False
self.camera: Optional[rosys.vision.CalibratableCamera] = None
self.image_view: Optional[ui.interactive_image] = None
@@ -118,9 +119,11 @@ def update_content(self) -> None:
url = f'{active_camera.get_latest_image_url()}?shrink={self.shrink_factor}'
else:
url = active_camera.get_latest_image_url()
- if self.image_view is None or self.camera.calibration is None:
+ if self.image_view is None:
return
self.image_view.set_source(url)
+ if self.camera.calibration is None:
+ return
image = active_camera.latest_detected_image
svg = ''
if image and image.detections:
@@ -132,19 +135,31 @@ def update_content(self) -> None:
def on_mouse_move(self, e: MouseEventArguments):
if self.camera is None:
return
- if e.type == 'mousemove':
- point2d = Point(x=e.image_x, y=e.image_y)
- if self.camera.calibration is None:
- self.debug_position.set_text(f'{point2d} no calibration')
- return
+
+ point2d = Point(x=e.image_x, y=e.image_y)
+ if self.camera.calibration is None:
+ self.debug_position.set_text(f'{point2d} no calibration')
+ return
+ point3d: rosys.geometry.Point3d | None = None
+ if isinstance(self.camera, CalibratableUsbCamera):
point3d = self.camera.calibration.project_from_image(point2d)
+ elif isinstance(self.camera, StereoCamera):
+ # TODO: too many calls
+ # camera_point_3d: Point3d | None = self.camera.get_point(
+ # int(point2d.x), int(point2d.y))
+ # if camera_point_3d is None:
+ # return
+ # camera_point_3d = camera_point_3d.in_frame(self.odometer.prediction_frame)
+ # world_point_3d = camera_point_3d.resolve()
+ # self.log.info(f'camera_point_3d: {camera_point_3d} -> world_point_3d: {world_point_3d.tuple}')
+ pass
+
+ if e.type == 'mousemove' and point3d is not None:
self.debug_position.set_text(f'screen {point2d} -> local {point3d}')
if e.type == 'mouseup':
- point2d = Point(x=e.image_x, y=e.image_y)
if self.camera.calibration is None:
self.debug_position.set_text(f'last punch: {point2d}')
return
- point3d = self.camera.calibration.project_from_image(point2d)
if point3d is not None:
self.debug_position.set_text(f'last punch: {point2d} -> {point3d}')
if self.puncher is not None and self.punching_enabled:
@@ -220,11 +235,11 @@ def build_svg_for_plant_provider(self) -> str:
if self.camera is None or self.camera.calibration is None:
return ''
position = rosys.geometry.Point3d(x=self.camera.calibration.extrinsics.translation[0],
- y=self.camera.calibration.extrinsics.translation[1])
+ y=self.camera.calibration.extrinsics.translation[1],
+ z=0)
svg = ''
for plant in self.plant_provider.get_relevant_weeds(position):
- position_3d = rosys.geometry.Point3d(x=plant.position.x, y=plant.position.y, z=0)
- screen = self.camera.calibration.project_to_image(position_3d)
+ screen = self.camera.calibration.project_to_image(plant.position)
if screen is not None:
svg += f''
svg += f'{plant.id[:4]}'
diff --git a/field_friend/interface/components/hardware_control.py b/field_friend/interface/components/hardware_control.py
index 33f796bd..b6eed5c8 100644
--- a/field_friend/interface/components/hardware_control.py
+++ b/field_friend/interface/components/hardware_control.py
@@ -3,8 +3,8 @@
from nicegui.events import ValueChangeEventArguments
from ...automations import Puncher
-from ...hardware import (ChainAxis, FieldFriend, FieldFriendHardware, Flashlight, FlashlightPWM, FlashlightPWMV2,
- FlashlightV2, Mower, MowerHardware, MowerSimulation, Tornado, Axis, YAxisCanOpenHardware,
+from ...hardware import (Axis, ChainAxis, FieldFriend, FieldFriendHardware, Flashlight, FlashlightPWM, FlashlightPWMV2,
+ FlashlightV2, Mower, MowerHardware, MowerSimulation, Tornado, YAxisCanOpenHardware,
ZAxisCanOpenHardware)
from .confirm_dialog import ConfirmDialog as confirm_dialog
from .status_bulb import StatusBulb as status_bulb
@@ -142,7 +142,7 @@ async def toggle_flashlight(e: ValueChangeEventArguments) -> None:
puncher.punch(field_friend.y_axis.max_position, depth=depth.value)))
ui.button(on_click=lambda: automator.start(puncher.punch(0, depth=depth.value)))
ui.button(on_click=lambda: automator.start(
- puncher.punch(field_friend.y_axis.min_position, depth=depth.value)))
+ puncher.punch(field_friend.y_axis.min_position - field_friend.WORK_Y, depth=depth.value)))
if isinstance(field_friend.mower, Mower):
with ui.column():
diff --git a/field_friend/system.py b/field_friend/system.py
index 70898fdb..839d7ba0 100644
--- a/field_friend/system.py
+++ b/field_friend/system.py
@@ -7,12 +7,14 @@
import psutil
import rosys
+import config.config_selection as config_selector
+
from . import localization
from .automations import (AutomationWatcher, BatteryWatcher, FieldProvider, KpiProvider, PathProvider, PlantLocator,
PlantProvider, Puncher)
from .automations.implements import ChopAndScrew, ExternalMower, Implement, Recorder, Tornado, WeedingScrew
-from .automations.navigation import (ABLineNavigation, CoverageNavigation, FollowCropsNavigation, Navigation,
- RowsOnFieldNavigation, StraightLineNavigation)
+from .automations.navigation import (ABLineNavigation, CoverageNavigation, CrossglideDemoNavigation,
+ FollowCropsNavigation, Navigation, RowsOnFieldNavigation, StraightLineNavigation)
from .hardware import FieldFriend, FieldFriendHardware, FieldFriendSimulation, TeltonikaRouter
from .interface.components.info import Info
from .kpi_generator import generate_kpis
@@ -20,6 +22,7 @@
from .localization.gnss_hardware import GnssHardware
from .localization.gnss_simulation import GnssSimulation
from .vision import CalibratableUsbCameraProvider, CameraConfigurator
+from .vision.zedxmini_camera import ZedxminiCameraProvider
icecream.install()
@@ -37,7 +40,7 @@ def __init__(self) -> None:
self.is_real = rosys.hardware.SerialCommunication.is_possible()
self.AUTOMATION_CHANGED = rosys.event.Event()
- self.camera_provider: CalibratableUsbCameraProvider | rosys.vision.SimulatedCameraProvider
+ self.camera_provider = self.setup_camera_provider()
self.detector: rosys.vision.DetectorHardware | rosys.vision.DetectorSimulation
self.field_friend: FieldFriend
if self.is_real:
@@ -46,7 +49,6 @@ def __init__(self) -> None:
self.teltonika_router = TeltonikaRouter()
except Exception:
self.log.exception(f'failed to initialize FieldFriendHardware {self.version}')
- self.camera_provider = CalibratableUsbCameraProvider()
self.mjpeg_camera_provider = rosys.vision.MjpegCameraProvider(username='root', password='zauberzg!')
self.detector = rosys.vision.DetectorHardware(port=8004)
self.monitoring_detector = rosys.vision.DetectorHardware(port=8005)
@@ -54,7 +56,6 @@ def __init__(self) -> None:
self.camera_configurator = CameraConfigurator(self.camera_provider, odometer=self.odometer)
else:
self.field_friend = FieldFriendSimulation(robot_id=self.version)
- self.camera_provider = rosys.vision.SimulatedCameraProvider()
# NOTE we run this in rosys.startup to enforce setup AFTER the persistence is loaded
rosys.on_startup(self.setup_simulated_usb_camera)
self.detector = rosys.vision.DetectorSimulation(self.camera_provider)
@@ -122,11 +123,13 @@ def watch_robot() -> None:
self.follow_crops_navigation = FollowCropsNavigation(self, self.monitoring)
self.coverage_navigation = CoverageNavigation(self, self.monitoring)
self.a_b_line_navigation = ABLineNavigation(self, self.monitoring)
+ self.crossglide_demo_navigation = CrossglideDemoNavigation(self, self.monitoring)
self.navigation_strategies = {n.name: n for n in [self.field_navigation,
self.straight_line_navigation,
self.follow_crops_navigation,
self.coverage_navigation,
- self.a_b_line_navigation
+ self.a_b_line_navigation,
+ self.crossglide_demo_navigation
]}
implements: list[Implement] = [self.monitoring]
match self.field_friend.implement_name:
@@ -208,6 +211,17 @@ def current_navigation(self, navigation: Navigation) -> None:
self.AUTOMATION_CHANGED.emit(navigation.name)
self.request_backup()
+ def setup_camera_provider(self) -> CalibratableUsbCameraProvider | rosys.vision.SimulatedCameraProvider | ZedxminiCameraProvider:
+ if not self.is_real:
+ return rosys.vision.SimulatedCameraProvider()
+ camera_config = config_selector.import_config(module='camera')
+ camera_type = camera_config.get('type', 'CalibratableUsbCamera')
+ if camera_type == 'CalibratableUsbCamera':
+ return CalibratableUsbCameraProvider()
+ if camera_type == 'ZedxminiCamera':
+ return ZedxminiCameraProvider()
+ raise NotImplementedError(f'Unknown camera type: {camera_type}')
+
async def setup_simulated_usb_camera(self):
self.camera_provider.remove_all_cameras()
camera = rosys.vision.SimulatedCalibratableCamera.create_calibrated(id='bottom_cam',
diff --git a/field_friend/vision/zedxmini_camera/__init__.py b/field_friend/vision/zedxmini_camera/__init__.py
new file mode 100644
index 00000000..457fa50c
--- /dev/null
+++ b/field_friend/vision/zedxmini_camera/__init__.py
@@ -0,0 +1,8 @@
+from .zedxmini_camera import StereoCamera, ZedxminiCamera
+from .zedxmini_camera_provider import ZedxminiCameraProvider
+
+__all__ = [
+ 'StereoCamera',
+ 'ZedxminiCamera',
+ 'ZedxminiCameraProvider',
+]
diff --git a/field_friend/vision/zedxmini_camera/zedxmini_camera.py b/field_friend/vision/zedxmini_camera/zedxmini_camera.py
new file mode 100644
index 00000000..b1f7f23c
--- /dev/null
+++ b/field_friend/vision/zedxmini_camera/zedxmini_camera.py
@@ -0,0 +1,167 @@
+import abc
+import asyncio
+import logging
+from typing import Any, Self
+
+import aiohttp
+import rosys
+from rosys import persistence
+from rosys.geometry import Point3d
+from rosys.vision import CalibratableCamera, Calibration, Image, ImageSize, Intrinsics
+
+
+class StereoCamera(CalibratableCamera, abc.ABC):
+ @abc.abstractmethod
+ async def get_point(self, x, y) -> Point3d | None:
+ pass
+
+
+class ZedxminiCamera(StereoCamera):
+ ip: str = 'localhost'
+ port: int = 8003
+
+ def __init__(self, ip: str | None = None, port: int | None = None, focal_length: float = 747.0735473632812, **kwargs) -> None:
+ self.MAX_IMAGES = 10
+ super().__init__(**kwargs)
+ if ip is not None:
+ self.ip = ip
+ if port is not None:
+ self.port = port
+ self.focal_length = focal_length
+ self.connected: bool = False
+ self.log = logging.getLogger(self.name)
+ self.log.setLevel(logging.DEBUG)
+ self.camera_information: dict[str, Any] = {}
+
+ async def connect(self) -> None:
+ await super().connect()
+ self.connected = await self.setup_camera_information()
+
+ @staticmethod
+ async def get_camera_information(ip: str | None = None, port: int | None = None) -> dict[str, Any] | None:
+ ip = ZedxminiCamera.ip if ip is None else ip
+ port = ZedxminiCamera.port if port is None else port
+ url = f'http://{ip}:{port}/information'
+ data: dict[str, Any] | None = None
+ async with aiohttp.ClientSession() as session:
+ try:
+ async with session.get(url, timeout=2.0) as response:
+ if response.status != 200:
+ logging.warning(f"response.status: {response.status}")
+ return None
+ data = await response.json()
+ except aiohttp.ClientError as e:
+ logging.error(f"Error capturing image: {str(e)}")
+ return None
+ except asyncio.TimeoutError:
+ logging.error("Request timed out")
+ return None
+ if data is None:
+ return
+ return data
+
+ async def setup_camera_information(self) -> bool:
+ camera_information = await self.get_camera_information(self.ip, self.port)
+ if camera_information is None:
+ return False
+ assert 'calibration' in camera_information
+ assert 'left_cam' in camera_information['calibration']
+ assert 'fx' in camera_information['calibration']['left_cam']
+ self.camera_information = camera_information
+ self.focal_length = camera_information['calibration']['left_cam']['fy']
+ return True
+
+ async def capture_image(self) -> None:
+ if not self.connected:
+ return
+ url = f'http://{self.ip}:{self.port}/image'
+ data: dict[str, Any] | None = None
+ async with aiohttp.ClientSession() as session:
+ try:
+ async with session.get(url, timeout=2.0) as response:
+ if response.status != 200:
+ self.log.warning(f"response.status: {response.status}")
+ return
+ data = await response.json()
+ except aiohttp.ClientError as e:
+ self.log.error(f"Error capturing image: {str(e)}")
+ except asyncio.TimeoutError:
+ self.log.error("Request timed out")
+ if data is None:
+ return
+ assert 'image' in data
+ image_bytes = await rosys.run.cpu_bound(bytes.fromhex, data['image'])
+ image = Image(
+ camera_id=data['camera_id'],
+ size=ImageSize(width=data['width'], height=data['height']),
+ time=data['time'],
+ data=image_bytes,
+ is_broken=data['is_broken'],
+ tags=set(data['tags']),
+ )
+ self._add_image(image)
+
+ async def get_point(self, x, y) -> Point3d | None:
+ url = f'http://{self.ip}:{self.port}/point?x={x}&y={y}'
+ data: dict[str, Any] | None = None
+ async with aiohttp.ClientSession() as session:
+ try:
+ async with session.get(url, timeout=2.0) as response:
+ if response.status != 200:
+ self.log.warning(f"response.status: {response.status}")
+ return None
+ data = await response.json()
+ except aiohttp.ClientError as e:
+ self.log.error(f"Error capturing image: {str(e)}")
+ except asyncio.TimeoutError:
+ self.log.error("Request timed out")
+ if data is None:
+ return None
+ assert 'x' in data
+ assert 'y' in data
+ assert 'z' in data
+ return Point3d(**data)
+
+ @property
+ def is_connected(self) -> bool:
+ # TODO: check it in capture_image
+ return self.connected
+
+ def setup_calibration(self, camera_dict: dict) -> None:
+ assert 'resolution' in camera_dict
+ assert 'calibration' in camera_dict
+ assert 'left_cam' in camera_dict['calibration']
+ width = camera_dict['resolution'][0]
+ height = camera_dict['resolution'][1]
+ fx = camera_dict['calibration']['left_cam']['fx']
+ fy = camera_dict['calibration']['left_cam']['fy']
+ self.focal_length = (fx + fy) / 2.0
+ fy = camera_dict['calibration']['left_cam']['fy']
+ cx = camera_dict['calibration']['left_cam']['cx']
+ cy = camera_dict['calibration']['left_cam']['cy']
+ k1 = camera_dict['calibration']['left_cam']['k1']
+ k2 = camera_dict['calibration']['left_cam']['k2']
+ p1 = camera_dict['calibration']['left_cam']['p1']
+ p2 = camera_dict['calibration']['left_cam']['p2']
+ k3 = camera_dict['calibration']['left_cam']['k3']
+
+ size = ImageSize(width=width, height=height)
+ K: list[list[float]] = [[fx, 0.0, cx],
+ [0.0, fy, cy],
+ [0.0, 0.0, 1.0]]
+ D: list[float] = [k1, k2, p1, p2, k3]
+ intrinsics = Intrinsics(matrix=K, distortion=D, size=size)
+ self.calibration = Calibration(intrinsics=intrinsics)
+
+ @classmethod
+ def from_dict(cls, data: dict[str, Any]) -> Self:
+ logging.info(f'zedxmini - from_dict - data: {data}')
+ return cls(**(data | {
+ 'calibration': persistence.from_dict(rosys.vision.Calibration, data['calibration']) if data.get('calibration') else None,
+ }))
+
+ def to_dict(self) -> dict:
+ logging.info('zedxmini - to_dict')
+ return super().to_dict() | {
+ 'focal_length': self.focal_length,
+ }
diff --git a/field_friend/vision/zedxmini_camera/zedxmini_camera_provider.py b/field_friend/vision/zedxmini_camera/zedxmini_camera_provider.py
new file mode 100644
index 00000000..c580a262
--- /dev/null
+++ b/field_friend/vision/zedxmini_camera/zedxmini_camera_provider.py
@@ -0,0 +1,48 @@
+import logging
+
+import rosys
+
+from .zedxmini_camera import ZedxminiCamera
+
+SCAN_INTERVAL = 10
+
+
+class ZedxminiCameraProvider(rosys.vision.CameraProvider[ZedxminiCamera], rosys.persistence.PersistentModule):
+
+ def __init__(self) -> None:
+ super().__init__()
+ self.log = logging.getLogger('field_friend.zedxmini_camera_provider')
+
+ rosys.on_shutdown(self.shutdown)
+ rosys.on_repeat(self.update_device_list, SCAN_INTERVAL)
+ rosys.on_startup(self.update_device_list)
+
+ def backup(self) -> dict:
+ for camera in self._cameras.values():
+ self.log.info(f'backing up camera: {camera.to_dict()}')
+ return {
+ 'cameras': {camera.id: camera.to_dict() for camera in self._cameras.values()}
+ }
+
+ def restore(self, data: dict[str, dict]) -> None:
+ for camera_data in data.get('cameras', {}).values():
+ self.add_camera(ZedxminiCamera.from_dict(camera_data))
+
+ async def update_device_list(self) -> None:
+ if len(self._cameras) == 0:
+ camera_information = await ZedxminiCamera.get_camera_information('localhost', 8003)
+ if camera_information is None:
+ return
+ self.add_camera(ZedxminiCamera(id=str(camera_information['serial_number']), polling_interval=0.1))
+ camera = list(self._cameras.values())[0]
+ if camera.is_connected:
+ return
+ await camera.reconnect()
+
+ async def shutdown(self) -> None:
+ for camera in self._cameras.values():
+ await camera.disconnect()
+
+ @staticmethod
+ def is_operable() -> bool:
+ return True
diff --git a/sync.py b/sync.py
index 2261d8c3..aa46e8fc 100755
--- a/sync.py
+++ b/sync.py
@@ -7,6 +7,7 @@
parser = argparse.ArgumentParser(description='Sync local code with robot.')
parser.add_argument('robot', help='Robot hostname')
parser.add_argument('--rosys', action='store_true', default=False, help='Sync rosys')
+parser.add_argument('--zedxmini', action='store_true', default=False, help='Sync zedxmini')
args = parser.parse_args()
touch = 'touch ~/field_friend/main.py'
@@ -16,4 +17,8 @@
else:
print('Ensuring we have no local rosys on the robot')
run_subprocess(f'ssh {args.robot} "rm -rf ~/field_friend/rosys"')
+
+if args.zedxmini:
+ folders.append(Folder('../zedxmini', f'{args.robot}:~/zedxmini', on_change=touch))
+
sync(*folders)