diff --git a/config/config_selection.py b/config/config_selection.py index d2874da2..1ca7fd46 100644 --- a/config/config_selection.py +++ b/config/config_selection.py @@ -57,4 +57,6 @@ def find_matching_config_folders(hostname): matching_folders = glob.glob(pattern) # Filter out the ones that are not directories matching_dirs = [folder for folder in matching_folders if os.path.isdir(folder)] + assert len(matching_dirs) == 1, \ + f"Expected exactly one matching folder, but got {len(matching_dirs)} for {pattern}: {matching_dirs}" return matching_dirs[0].replace("/", ".") diff --git a/config/f15_config_f15/camera.py b/config/f15_config_f15/camera.py new file mode 100644 index 00000000..ab04f53c --- /dev/null +++ b/config/f15_config_f15/camera.py @@ -0,0 +1,13 @@ +configuration = {'parameters': { + 'width': 1280, + 'height': 720, + 'auto_exposure': True, + 'fps': 10, +}, + 'crop': { + 'left': 60, + 'right': 200, + 'up': 20, + 'down': 0, +} +} diff --git a/config/f15_config_f15/hardware.py b/config/f15_config_f15/hardware.py new file mode 100644 index 00000000..c8041ca1 --- /dev/null +++ b/config/f15_config_f15/hardware.py @@ -0,0 +1,96 @@ +configuration = { + 'wheels': { + 'version': 'double_wheels', + 'name': 'wheels', + 'left_back_can_address': 0x100, + 'left_front_can_address': 0x000, + 'right_back_can_address': 0x300, + 'right_front_can_address': 0x200, + 'is_left_reversed': False, + 'is_right_reversed': True, + 'odrive_version': 6, + }, + # # FIXME: PIN checken + # 'eyes': { + # 'name': 'eyes', + # 'on_expander': True, + # 'eyes_pin': 12, + # }, + 'y_axis': { + 'version': 'axis_d1', + 'name': 'yaxis', + 'can_address': 0x60, + 'homing_acceleration': 2000, + 'homing_velocity': 400, + 'profile_acceleration': 500000, + 'profile_velocity': 40000, + 'profile_deceleration': 500000, + 'min_position': -0.11, + 'max_position': 0.11, + 'axis_offset': 0.115, + }, + 'z_axis': { + 'version': 'axis_d1', + 'name': 'zaxis', + 'can_address': 0x70, + 'homing_acceleration': 1000, + 'homing_velocity': 500, + 'profile_acceleration': 500000, + 'profile_velocity': 40000, + 'profile_deceleration': 500000, + 'min_position': 0.230, + 'max_position': 0, + 'axis_offset': 0.01, + }, + 'estop': { + 'name': 'estop', + 'pins': {'1': 34, '2': 35}, + }, + 'bms': { + 'name': 'bms', + 'on_expander': True, + 'rx_pin': 26, + 'tx_pin': 27, + 'baud': 9600, + 'num': 2, + }, + 'battery_control': { + 'name': 'battery_control', + 'on_expander': True, + 'reset_pin': 15, + 'status_pin': 13, + }, + 'flashlight': { + 'version': 'none', + }, + 'bumper': { + 'name': 'bumper', + 'on_expander': True, + 'pins': {'front_top': 18, 'front_bottom': 35, 'back': 21}, + }, + 'status_control': { + 'name': 'status_control', + }, + 'bluetooth': { + 'name': 'fieldfriend-f15', + }, + 'serial': { + 'name': 'serial', + 'rx_pin': 26, + 'tx_pin': 27, + 'baud': 115200, + 'num': 1, + }, + 'expander': { + 'name': 'p0', + 'boot': 25, + 'enable': 14, + }, + 'can': { + 'name': 'can', + 'on_expander': False, + 'rx_pin': 32, + 'tx_pin': 33, + 'baud': 1_000_000, + }, +} diff --git a/config/f15_config_f15/params.py b/config/f15_config_f15/params.py new file mode 100644 index 00000000..c6937e7a --- /dev/null +++ b/config/f15_config_f15/params.py @@ -0,0 +1,10 @@ +configuration = { + 'motor_gear_ratio': 12.52, + 'thooth_count': 15, + 'pitch': 0.033, + 'wheel_distance': 0.47, + 'antenna_offset': 0.205, + 'work_x': 0.0125, + 'drill_radius': 0.025, + 'tool': 'weed_screw', +} diff --git a/config/f15_config_f15/robotbrain.py b/config/f15_config_f15/robotbrain.py new file mode 100644 index 00000000..33b7e534 --- /dev/null +++ b/config/f15_config_f15/robotbrain.py @@ -0,0 +1,4 @@ +configuration = {'robot_brain': { + 'flash_params': ['orin', 'v05', 'nand'], + 'enable_esp_on_startup':False, +}} \ No newline at end of file diff --git a/config/testbrain_config_rb19/hardware.py b/config/testbrain_config_rb19/hardware.py index 0a141e93..cb504935 100644 --- a/config/testbrain_config_rb19/hardware.py +++ b/config/testbrain_config_rb19/hardware.py @@ -10,20 +10,20 @@ 'is_right_reversed': False, }, 'y_axis': { - 'version': 'none', + 'version': 'axis_d1', 'name': 'yaxis', - 'can_address': 0x70, - 'max_speed': 2000, - 'reference_speed': 30, - 'min_position': -0.125, - 'max_position': 0.125, - 'axis_offset': 0.13, - 'steps_per_m': 1_666_666.667, # 4000steps/turn motor; 1/10 gear; 0.024m/u - 'end_r_pin': 12, - 'end_l_pin': 25, - 'motor_on_expander': False, - 'end_stops_on_expander': True, - 'reversed_direction': False, + 'can_address': 0x60, + 'homing_acceleration': 2000, + 'homing_velocity': 40, + 'profile_acceleration': 4000, + 'profile_velocity': 400, + 'profile_deceleration': 5000, + 'min_position': 0, + 'max_position': 23000, + 'axis_offset': 23000, + }, + 'z_axis': { + 'version': 'none', }, 'estop': { 'name': 'estop', @@ -40,22 +40,7 @@ 'flashlight': { 'version': 'none', }, - 'z_axis': { - 'version': 'none', - 'name': 'zaxis', - 'can_address': 0x60, - 'max_speed': 2000, - 'reference_speed': 30, - 'min_position': -0.197, - 'max_position': 0.0, - 'axis_offset': 0.0, - 'steps_per_m': 4_000_000, # 4000steps/turn motor; 1/20 gear; 0.02m/u - 'end_t_pin': 22, - 'end_b_pin': 23, - 'motor_on_expander': False, - 'end_stops_on_expander': True, - 'reversed_direction': False, - }, + 'bluetooth': { 'name': 'TestBrain', }, diff --git a/config/testbrain_config_rb19/params.py b/config/testbrain_config_rb19/params.py index a4cb1569..c81beaca 100644 --- a/config/testbrain_config_rb19/params.py +++ b/config/testbrain_config_rb19/params.py @@ -6,4 +6,5 @@ 'work_x': 0.0125, 'drill_radius': 0.025, 'tool': 'none', + 'antenna_offset': 0.205, } diff --git a/config/testbrain_config_rb19/robotbrain.py b/config/testbrain_config_rb19/robotbrain.py index 3dc97482..49c9ee58 100644 --- a/config/testbrain_config_rb19/robotbrain.py +++ b/config/testbrain_config_rb19/robotbrain.py @@ -1,4 +1,4 @@ configuration = {'robot_brain': { - 'flash_params': [''] + 'flash_params': ['nand'] }} # test diff --git a/field_friend/automations/field_provider.py b/field_friend/automations/field_provider.py index 030b2aa2..33fcd2b3 100644 --- a/field_friend/automations/field_provider.py +++ b/field_friend/automations/field_provider.py @@ -172,12 +172,10 @@ def add_obstacle_point(self, field: Field, obstacle: FieldObstacle, point: Optio if positioning is None or positioning.lat == 0 or positioning.long == 0: rosys.notify("No GNSS position.") return - if not("R" in self.gnss.current.mode or self.gnss.current.mode == "SSSS"): + if not ("R" in self.gnss.current.mode or self.gnss.current.mode == "SSSS"): rosys.notify("GNSS position is not accurate enough.") return new_point = positioning - if self.gnss.device != 'simulation': - self.ensure_field_reference(field) if point is not None: index = obstacle.points.index(point) obstacle.points[index] = new_point @@ -201,7 +199,7 @@ def add_row_point(self, field: Field, row: Row, point: Optional[GeoPoint] = None if positioning is None or positioning.lat == 0 or positioning.long == 0: rosys.notify("No GNSS position.") return - if not("R" in self.gnss.current.mode or self.gnss.current.mode == "SSSS"): + if not ("R" in self.gnss.current.mode or self.gnss.current.mode == "SSSS"): rosys.notify("GNSS position is not accurate enough.") return new_point = positioning diff --git a/field_friend/automations/implements/weeding_implement.py b/field_friend/automations/implements/weeding_implement.py index 4e53cd47..09f90b7b 100644 --- a/field_friend/automations/implements/weeding_implement.py +++ b/field_friend/automations/implements/weeding_implement.py @@ -5,7 +5,7 @@ import rosys from nicegui import ui -from rosys.geometry import Point, Pose +from rosys.geometry import Point3d, Pose from ...hardware import ChainAxis from .implement import Implement @@ -31,6 +31,7 @@ def __init__(self, name: str, system: 'System', persistence_key: str = 'weeding self.system = system self.kpi_provider = system.kpi_provider self.puncher = system.puncher + self.record_video = False self.cultivated_crop: str | None = None self.crop_safety_distance: float = 0.01 @@ -43,9 +44,9 @@ def __init__(self, name: str, system: 'System', persistence_key: str = 'weeding self.start_time: Optional[float] = None self.last_pose: Optional[Pose] = None self.driven_distance: float = 0.0 - self.crops_to_handle: dict[str, Point] = {} - self.weeds_to_handle: dict[str, Point] = {} - self.last_punches: deque[rosys.geometry.Point] = deque(maxlen=5) + self.crops_to_handle: dict[str, Point3d] = {} + self.weeds_to_handle: dict[str, Point3d] = {} + self.last_punches: deque[Point3d] = deque(maxlen=5) self.next_punch_y_position: float = 0 rosys.on_repeat(self._update_time_and_distance, 0.1) @@ -63,17 +64,21 @@ async def prepare(self) -> bool: async def finish(self) -> None: self.system.plant_locator.pause() await self.system.field_friend.stop() + await self.system.timelapse_recorder.compress_video() await super().finish() async def activate(self): await self.system.field_friend.flashlight.turn_on() await self.puncher.clear_view() - self.system.plant_locator.resume() await rosys.sleep(3) + self.system.plant_locator.resume() + if self.record_video: + self.system.timelapse_recorder.camera = self.system.camera_provider.first_connected_camera await super().activate() async def deactivate(self): await super().deactivate() + self.system.timelapse_recorder.camera = None await self.system.field_friend.flashlight.turn_off() self.system.plant_locator.pause() self.kpi_provider.increment_weeding_kpi('rows_weeded') @@ -94,7 +99,7 @@ async def _check_hardware_ready(self) -> bool: rosys.notify('E-Stop is active, aborting', 'negative') self.log.error('E-Stop is active, aborting') return False - camera = next((camera for camera in self.system.camera_provider.cameras.values() if camera.is_connected), None) + camera = self.system.camera_provider.first_connected_camera if not camera: rosys.notify('no camera connected') return False @@ -118,8 +123,8 @@ async def _check_hardware_ready(self) -> bool: def _has_plants_to_handle(self) -> bool: relative_crop_positions = { - c.id: self.system.odometer.prediction.relative_point(c.position) - for c in self.system.plant_provider.get_relevant_crops(self.system.odometer.prediction.point) + c.id: Point3d.from_point(self.system.odometer.prediction.relative_point(c.position)) + for c in self.system.plant_provider.get_relevant_crops(self.system.odometer.prediction.point_3d()) if self.cultivated_crop is None or c.type == self.cultivated_crop } upcoming_crop_positions = { @@ -131,8 +136,8 @@ def _has_plants_to_handle(self) -> bool: self.crops_to_handle = sorted_crops relative_weed_positions = { - w.id: self.system.odometer.prediction.relative_point(w.position) - for w in self.system.plant_provider.get_relevant_weeds(self.system.odometer.prediction.point) + w.id: Point3d.from_point(self.system.odometer.prediction.relative_point(w.position)) + for w in self.system.plant_provider.get_relevant_weeds(self.system.odometer.prediction.point_3d()) if w.type in self.relevant_weeds } upcoming_weed_positions = { @@ -146,7 +151,8 @@ def _has_plants_to_handle(self) -> bool: offset = self.system.field_friend.DRILL_RADIUS + \ self.crop_safety_distance - crop_position.distance(weed_position) if offset > 0: - safe_weed_position = weed_position.polar(offset, crop_position.direction(weed_position)) + 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}') @@ -166,7 +172,8 @@ def backup(self) -> dict: 'with_chopping': self.with_chopping, 'chop_if_no_crops': self.chop_if_no_crops, 'cultivated_crop': self.cultivated_crop, - 'crop_safety_distance': self.crop_safety_distance + 'crop_safety_distance': self.crop_safety_distance, + 'record_video': self.record_video, } def restore(self, data: dict[str, Any]) -> None: @@ -175,6 +182,7 @@ def restore(self, data: dict[str, Any]) -> None: self.chop_if_no_crops = data.get('chop_if_no_crops', self.chop_if_no_crops) self.cultivated_crop = data.get('cultivated_crop', self.cultivated_crop) self.crop_safety_distance = data.get('crop_safety_distance', self.crop_safety_distance) + self.record_video = data.get('record_video', self.record_video) def clear(self) -> None: self.crops_to_handle = {} @@ -209,3 +217,6 @@ def settings_ui(self): .classes('w-24') \ .bind_value(self, 'crop_safety_distance') \ .tooltip('Set the crop safety distance for the weeding automation') + ui.checkbox('record video', on_change=self.request_backup) \ + .bind_value(self, 'record_video') \ + .tooltip('Set the weeding automation to record video') diff --git a/field_friend/automations/implements/weeding_screw.py b/field_friend/automations/implements/weeding_screw.py index 7865a9e7..14669b6b 100644 --- a/field_friend/automations/implements/weeding_screw.py +++ b/field_friend/automations/implements/weeding_screw.py @@ -22,11 +22,11 @@ def __init__(self, system: 'System') -> None: async def start_workflow(self) -> None: await super().start_workflow() try: - punch_position = self.system.odometer.prediction.transform( - rosys.geometry.Point(x=self.system.field_friend.WORK_X, y=self.next_punch_y_position)) + punch_position = self.system.odometer.prediction.transform3d( + rosys.geometry.Point3d(x=self.system.field_friend.WORK_X, y=self.next_punch_y_position, z=0)) self.last_punches.append(punch_position) await self.system.puncher.punch(y=self.next_punch_y_position, depth=self.weed_screw_depth) - punched_weeds = [weed.id for weed in self.system.plant_provider.get_relevant_weeds(self.system.odometer.prediction.point) + punched_weeds = [weed.id for weed in self.system.plant_provider.get_relevant_weeds(self.system.odometer.prediction.point_3d()) if weed.position.distance(punch_position) <= self.system.field_friend.DRILL_RADIUS] for weed_id in punched_weeds: self.system.plant_provider.remove_weed(weed_id) @@ -52,8 +52,8 @@ async def get_stretch(self, max_distance: float) -> float: self.log.info(f'Found {len(weeds_in_range)} weeds in range: {weeds_in_range}') for next_weed_id, next_weed_position in weeds_in_range.items(): # next_weed_position.x += 0.01 # NOTE somehow this helps to mitigate an offset we experienced in the tests - weed_world_position = self.system.odometer.prediction.transform(next_weed_position) - crops = self.system.plant_provider.get_relevant_crops(self.system.odometer.prediction.point) + weed_world_position = self.system.odometer.prediction.transform3d(next_weed_position) + crops = self.system.plant_provider.get_relevant_crops(self.system.odometer.prediction.point_3d()) if self.cultivated_crop and not any(c.position.distance(weed_world_position) < self.max_crop_distance for c in crops): self.log.info('Skipping weed because it is to far from the cultivated crops') continue diff --git a/field_friend/automations/navigation/follow_crops_navigation.py b/field_friend/automations/navigation/follow_crops_navigation.py index 3d2190a6..e03ea778 100644 --- a/field_friend/automations/navigation/follow_crops_navigation.py +++ b/field_friend/automations/navigation/follow_crops_navigation.py @@ -47,7 +47,7 @@ def update_target(self) -> None: self.target = self.odometer.prediction.transform(rosys.geometry.Point(x=distance, y=0)) async def _drive(self, distance: float) -> None: - row = self.plant_provider.get_relevant_crops(self.odometer.prediction.point, max_distance=1.0) + row = self.plant_provider.get_relevant_crops(point=self.odometer.prediction.point_3d(), max_distance=1.0) if len(row) >= 3: points_array = np.array([(p.position.x, p.position.y) for p in row]) # Fit a line using least squares diff --git a/field_friend/automations/plant.py b/field_friend/automations/plant.py index 762e0056..82577e7d 100644 --- a/field_friend/automations/plant.py +++ b/field_friend/automations/plant.py @@ -1,10 +1,9 @@ -import uuid from collections import deque from dataclasses import dataclass, field from typing import Optional from uuid import uuid4 -from rosys.geometry import Point +from rosys.geometry import Point3d from rosys.vision import Image @@ -12,21 +11,23 @@ class Plant: id: str = field(default_factory=lambda: str(uuid4())) type: str - positions: deque[Point] = field(default_factory=lambda: deque(maxlen=20)) + positions: deque[Point3d] = field(default_factory=lambda: deque(maxlen=20)) detection_time: float confidences: deque[float] = field(default_factory=lambda: deque(maxlen=20)) detection_image: Optional[Image] = None @property - def position(self) -> Point: + def position(self) -> Point3d: """Calculate the middle position of all points""" - total_x = sum(point.x for point in self.positions) - total_y = sum(point.y for point in self.positions) + total_x = sum(point3d.x for point3d in self.positions) + total_y = sum(point3d.y for point3d in self.positions) + total_z = sum(point3d.z for point3d in self.positions) middle_x = total_x / len(self.positions) middle_y = total_y / len(self.positions) + middle_z = total_z / len(self.positions) - return Point(x=middle_x, y=middle_y) + return Point3d(x=middle_x, y=middle_y, z=middle_z) @property def confidence(self) -> float: diff --git a/field_friend/automations/plant_locator.py b/field_friend/automations/plant_locator.py index 59ef3148..a23ce1e7 100644 --- a/field_friend/automations/plant_locator.py +++ b/field_friend/automations/plant_locator.py @@ -46,6 +46,7 @@ def __init__(self, system: 'System') -> None: if system.is_real: self.teltonika_router = system.teltonika_router self.teltonika_router.CONNECTION_CHANGED.register(self.set_upload_images) + self.teltonika_router.MOBILE_UPLOAD_PERMISSION_CHANGED.register(self.set_upload_images) def backup(self) -> dict: self.log.info(f'backup: autoupload: {self.autoupload}') @@ -102,11 +103,10 @@ async def _detect_plants(self) -> None: if world_point_3d is None: self.log.error('could not generate world point of detection, calibration error') continue - world_point = world_point_3d.projection() plant = Plant(type=d.category_name, detection_time=rosys.time(), detection_image=new_image) - plant.positions.append(world_point) + plant.positions.append(world_point_3d) plant.confidences.append(d.confidence) if d.category_name in self.weed_category_names and d.confidence >= self.minimum_weed_confidence: # self.log.info('weed found') @@ -164,9 +164,6 @@ def settings_ui(self) -> None: ui.select(options, label='Autoupload', on_change=self.request_backup) \ .bind_value(self, 'autoupload') \ .classes('w-24').tooltip('Set the autoupload for the weeding automation') - if isinstance(self.detector, rosys.vision.DetectorHardware): - ui.checkbox('Upload images', on_change=self.request_backup).bind_value(self, 'upload_images') \ - .on('click', lambda: self.set_outbox_mode(value=self.upload_images, port=self.detector.port)) @ui.refreshable def chips(): @@ -190,7 +187,10 @@ def add_chip(): ui.button(icon='add', on_click=add_chip).props('round dense flat') def set_upload_images(self): - if self.teltonika_router.current_connection == 'wifi' or self.teltonika_router.current_connection == 'ether': + if self.teltonika_router.mobile_upload_permission: self.upload_images = True else: - self.upload_images = False + if self.teltonika_router.current_connection == 'wifi' or self.teltonika_router.current_connection == 'ether': + self.upload_images = True + else: + self.upload_images = False diff --git a/field_friend/automations/plant_provider.py b/field_friend/automations/plant_provider.py index bc018bc0..ff691c9c 100644 --- a/field_friend/automations/plant_provider.py +++ b/field_friend/automations/plant_provider.py @@ -3,7 +3,7 @@ import rosys from nicegui import ui -from rosys.geometry import Point +from rosys.geometry import Point3d from .plant import Plant @@ -129,18 +129,18 @@ def _add_crop_prediction(self, plant: Plant) -> None: crop_1 = sorted_crops[0] crop_2 = sorted_crops[1] - yaw = crop_2.position.direction(crop_1.position) - prediction = crop_1.position.polar(self.crop_spacing, yaw) + yaw = crop_2.position.projection().direction(crop_1.position.projection()) + prediction = crop_1.position.projection().polar(self.crop_spacing, yaw) - if plant.position.distance(prediction) > self.match_distance: + if plant.position.projection().distance(prediction) > self.match_distance: return - plant.positions.append(prediction) + plant.positions.append(Point3d.from_point(prediction, 0)) plant.confidences.append(self.prediction_confidence) - def get_relevant_crops(self, point: Point, *, max_distance=0.5) -> list[Plant]: + def get_relevant_crops(self, point: Point3d, *, max_distance=0.5) -> list[Plant]: return [c for c in self.crops if c.position.distance(point) <= max_distance and c.confidence >= self.minimum_combined_crop_confidence] - def get_relevant_weeds(self, point: Point, *, max_distance=0.5) -> list[Plant]: + def get_relevant_weeds(self, point: Point3d, *, max_distance=0.5) -> list[Plant]: return [w for w in self.weeds if w.position.distance(point) <= max_distance and w.confidence >= self.minimum_combined_weed_confidence] def settings_ui(self) -> None: diff --git a/field_friend/automations/puncher.py b/field_friend/automations/puncher.py index 5facf67d..0291f01d 100644 --- a/field_friend/automations/puncher.py +++ b/field_friend/automations/puncher.py @@ -5,7 +5,7 @@ from rosys.driving import Driver from rosys.geometry import Point -from ..hardware import ChainAxis, FieldFriend, Tornado, YAxis, ZAxis +from ..hardware import Axis, ChainAxis, FieldFriend, Tornado from .kpi_provider import KpiProvider @@ -83,7 +83,7 @@ async def punch(self, 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') raise PuncherException('y position out of range') - elif isinstance(self.field_friend.y_axis, YAxis): + elif isinstance(self.field_friend.y_axis, Axis): 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') raise PuncherException('y position out of range') @@ -92,7 +92,7 @@ async def punch(self, await self.field_friend.y_axis.move_to(y) await self.tornado_drill(angle=angle, turns=turns, with_open_drill=with_open_tornado) - elif isinstance(self.field_friend.z_axis, ZAxis): + elif isinstance(self.field_friend.z_axis, Axis): if self.is_demo: self.log.warning('punching with demo mode is not yet implemented for z axis') await self.field_friend.y_axis.move_to(y) @@ -119,7 +119,10 @@ async def clear_view(self) -> None: if isinstance(self.field_friend.y_axis, ChainAxis): await self.field_friend.y_axis.return_to_reference() return - elif isinstance(self.field_friend.y_axis, YAxis): + elif isinstance(self.field_friend.y_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 await self.field_friend.y_axis.move_to(y, speed=self.field_friend.y_axis.max_speed) await self.field_friend.y_axis.stop() diff --git a/field_friend/hardware/__init__.py b/field_friend/hardware/__init__.py index b6fda9c3..ee7322c1 100644 --- a/field_friend/hardware/__init__.py +++ b/field_friend/hardware/__init__.py @@ -1,3 +1,5 @@ +from .axis import Axis, AxisSimulation +from .axis_D1 import AxisD1 from .can_open_master import CanOpenMasterHardware from .chain_axis import ChainAxis, ChainAxisHardware, ChainAxisSimulation from .double_wheels import DoubleWheelsHardware @@ -6,19 +8,25 @@ from .field_friend_hardware import FieldFriendHardware from .field_friend_simulation import FieldFriendSimulation from .flashlight import Flashlight, FlashlightHardware, FlashlightSimulation -from .flashlight_pwm import FlashlightPWM, FlashlightPWMHardware, FlashlightPWMSimulation -from .flashlight_pwm_v2 import FlashlightPWMHardwareV2, FlashlightPWMSimulationV2, FlashlightPWMV2 +from .flashlight_pwm import ( + FlashlightPWM, + FlashlightPWMHardware, + FlashlightPWMSimulation, +) +from .flashlight_pwm_v2 import ( + FlashlightPWMHardwareV2, + FlashlightPWMSimulationV2, + FlashlightPWMV2, +) from .flashlight_v2 import FlashlightHardwareV2, FlashlightSimulationV2, FlashlightV2 from .imu import IMUHardware from .led_eyes import LedEyesHardware from .safety import Safety, SafetyHardware, SafetySimulation from .safety_small import SmallSafetyHardware from .status_control import StatusControlHardware +from .teltonika_router import TeltonikaRouter from .tornado import Tornado, TornadoHardware, TornadoSimulation -from .y_axis import YAxis, YAxisSimulation from .y_axis_canopen_hardware import YAxisCanOpenHardware from .y_axis_stepper_hardware import YAxisStepperHardware -from .z_axis import ZAxis, ZAxisSimulation from .z_axis_canopen_hardware import ZAxisCanOpenHardware from .z_axis_stepper_hardware import ZAxisStepperHardware -from .teltonika_router import TeltonikaRouter diff --git a/field_friend/hardware/z_axis.py b/field_friend/hardware/axis.py similarity index 90% rename from field_friend/hardware/z_axis.py rename to field_friend/hardware/axis.py index ca05464f..b0aedfdd 100644 --- a/field_friend/hardware/z_axis.py +++ b/field_friend/hardware/axis.py @@ -4,7 +4,7 @@ import rosys -class ZAxis(rosys.hardware.Module, abc.ABC): +class Axis(rosys.hardware.Module, abc.ABC): def __init__(self, max_speed, @@ -25,12 +25,14 @@ def __init__(self, self.steps_per_m: float = steps_per_m self.reversed_direction: bool = reversed_direction - self.is_referenced: bool = False self.steps: int = 0 + self.is_referenced: bool = False self.alarm: bool = False self.idle: bool = False self.end_t: bool = False self.end_b: bool = False + self.end_l: bool = False + self.end_r: bool = False rosys.on_shutdown(self.stop) @@ -45,9 +47,10 @@ async def move_to(self, position: float, speed: int | None = None) -> None: if not self.is_referenced: raise RuntimeError('zaxis is not referenced, reference first') if speed > self.max_speed: - raise RuntimeError('zaxis speed is too high') + raise RuntimeError(f'axis speed is too high, max speed is {self.max_speed}') if not self.min_position <= position <= self.max_position: - raise RuntimeError('zaxis depth is out of range') + raise RuntimeError( + f'target position {position} ist out of axis range {self.min_position} to {self.max_position}') @abc.abstractmethod async def try_reference(self) -> bool: @@ -74,7 +77,7 @@ async def return_to_reference(self) -> None: self.log.error(f'could not return zaxis to reference because of {e}') -class ZAxisSimulation(ZAxis, rosys.hardware.ModuleSimulation): +class AxisSimulation(Axis, rosys.hardware.ModuleSimulation): '''The z axis simulation module is a simple example for a representation of simulated robot hardware. ''' @@ -110,7 +113,7 @@ async def move_to(self, position: float, speed: int | None = None) -> None: try: await super().move_to(position, speed) except RuntimeError as e: - self.log.error(f'could not move zaxis to {position} because of {e}') + self.log.error(f'could not move axis to {position} because of {e}') return self.target_steps = self.compute_steps(position) self.speed = speed if self.target_steps > self.steps else speed * -1 diff --git a/field_friend/hardware/axis_D1.py b/field_friend/hardware/axis_D1.py new file mode 100644 index 00000000..aa61f62c --- /dev/null +++ b/field_friend/hardware/axis_D1.py @@ -0,0 +1,151 @@ +import rosys +from rosys.helpers import remove_indentation + +from .axis import Axis + +D1_STEPS_P_M = 100000 + + +class AxisD1(Axis, rosys.hardware.ModuleHardware): + def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, + name: str = 'axis_D1', + can: rosys.hardware.CanHardware, + max_position: int = -0.1, + min_position: int = 0.1, + axis_offset: int = 0, + can_address: int = 0x60, + homing_acceleration: int = 100, + homing_velocity: int = 20, + profile_velocity: int = 20, + profile_acceleration: int = 200, + profile_deceleration: int = 400, + + + ** kwargs + ) -> None: + """Rosys module to control the Igus D1 motor controller. + + :param: robot_brain: The RobotBrain object. + :param name: The name of the axis (default: 'axis_D1'). + :param can: The CAN hardware object. + :param can_address: The CAN address of the axis (default: 0x60). + """ + self.name = name + self.statusword: int = 0 + self._position: int = 0 + self.velocity: int = 0 + + # flags of the Statusword for more information refer to the CANopen standard and D1 manual + self.ready_to_switch_on: bool = False + self.switched_on: bool = False + self.operation_enabled: bool = False + self.fault: bool = False + self.voltage_enabled: bool = False + self.quick_stop: bool = False + self.switch_on_disabled: bool = False + self.warning: bool = False + self.manufacturer_specific: bool = False # this has no funktion in the D1 + self.remote_enable: bool = False + self.target_reached: bool = False + self.internal_limit_active: bool = False + self.operation_mode_specific: bool = False + self.manufacturer_specific2: bool = False + + lizard_code = remove_indentation(f''' + {self.name}_motor = D1Motor({can.name}, {can_address}) + {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_deceleration = {profile_deceleration} + {self.name}_motor.profile_velocity = {profile_velocity} + ''') + core_message_fields = [ + f'{name}_motor.position', + f'{name}_motor.velocity', + f'{name}_motor.status_word', + f'{name}_motor.status_flags', + ] + super().__init__( + robot_brain=robot_brain, + lizard_code=lizard_code, + core_message_fields=core_message_fields, + max_speed=profile_velocity, + max_position=max_position, + min_position=min_position, + axis_offset=axis_offset, + reference_speed=homing_velocity, + steps_per_m=0, + reversed_direction=False, + **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 rosys.sleep(0.1) + + async def enable_motor(self): + if self.fault: + await self.reset_error() + await rosys.sleep(0.5) + await self.robot_brain.send(f'{self.name}_motor.setup()') + + 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') + + async def try_reference(self): + 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 + await self.robot_brain.send(f'{self.name}_motor.home()') + await self.robot_brain.send(f'{self.name}_motor.home()') + + async def speed_Mode(self, speed: int): + #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.velocity = int(words.pop(0)) + self.statusword = int(words.pop(0)) + self.is_referenced = int(words.pop(0)) == 1 + self._split_statusword() + + 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 + self.operation_enabled = ((self.statusword >> 2) & 1) == 1 + self.fault = ((self.statusword >> 3) & 1) == 1 + self.voltage_enabled = ((self.statusword >> 4) & 1) == 1 + self.quick_stop = ((self.statusword >> 5) & 1) == 1 + self.switch_on_disabled = ((self.statusword >> 6) & 1) == 1 + self.warning = ((self.statusword >> 7) & 1) == 1 + self.manufacturer_specific = ((self.statusword >> 8) & 1) == 1 # No function in D1 + self.remote_enable = ((self.statusword >> 9) & 1) == 1 + self.target_reached = ((self.statusword >> 10) & 1) == 1 + self.internal_limit_active = ((self.statusword >> 11) & 1) == 1 + self.operation_mode_specific = ((self.statusword >> 12) & 1) == 1 + self.manufacturer_specific2 = ((self.statusword >> 13) & 1) == 1 diff --git a/field_friend/hardware/field_friend.py b/field_friend/hardware/field_friend.py index a42a799b..2ec9f3b6 100644 --- a/field_friend/hardware/field_friend.py +++ b/field_friend/hardware/field_friend.py @@ -11,8 +11,7 @@ from .flashlight_v2 import FlashlightV2 from .safety import Safety from .tornado import Tornado -from .y_axis import YAxis -from .z_axis import ZAxis +from .axis import Axis class FieldFriend(rosys.hardware.Robot): @@ -33,8 +32,8 @@ def __init__( implement_name: str, wheels: rosys.hardware.Wheels, flashlight: Union[Flashlight, FlashlightV2, FlashlightPWM, None], - y_axis: Union[YAxis, ChainAxis, None], - z_axis: Union[ZAxis, Tornado, None], + y_axis: Union[Axis, ChainAxis, None], + z_axis: Union[Axis, Tornado, None], mower: Union[Mower, None], estop: rosys.hardware.EStop, bumper: Union[rosys.hardware.Bumper, None], @@ -69,7 +68,7 @@ def can_reach(self, local_point: rosys.geometry.Point, second_tool: bool = False The point is given in local coordinates, i.e. the origin is the center of the tool. """ - if self.implement_name in ['weed_screw', 'tornado'] and isinstance(self.y_axis, YAxis): + if self.implement_name in ['weed_screw', 'tornado'] and isinstance(self.y_axis, Axis): return self.y_axis.min_position <= local_point.y <= self.y_axis.max_position elif self.implement_name in ['dual_mechanism'] and isinstance(self.y_axis, ChainAxis): if second_tool: diff --git a/field_friend/hardware/field_friend_hardware.py b/field_friend/hardware/field_friend_hardware.py index ff906ab1..9849dde7 100644 --- a/field_friend/hardware/field_friend_hardware.py +++ b/field_friend/hardware/field_friend_hardware.py @@ -5,6 +5,7 @@ import config.config_selection as config_selector +from .axis_D1 import AxisD1 from .can_open_master import CanOpenMasterHardware from .chain_axis import ChainAxisHardware from .double_wheels import DoubleWheelsHardware @@ -57,7 +58,10 @@ def __init__(self) -> None: raise NotImplementedError(f'Unknown FieldFriend implement: {implement}') communication = rosys.hardware.SerialCommunication() - robot_brain = rosys.hardware.RobotBrain(communication) + 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']) + else: + robot_brain = rosys.hardware.RobotBrain(communication) robot_brain.lizard_firmware.flash_params += config_robotbrain['robot_brain']['flash_params'] bluetooth = rosys.hardware.BluetoothHardware(robot_brain, name=config_hardware['bluetooth']['name'], @@ -100,12 +104,25 @@ def __init__(self) -> None: else: raise NotImplementedError(f'Unknown wheels version: {config_hardware["wheels"]["version"]}') - if config_hardware['y_axis']['version'] == 'y_axis_canopen' or config_hardware['z_axis']['version'] == 'z_axis_canopen': + if config_hardware['y_axis']['version'] in ('y_axis_canopen', 'axis_d1') or config_hardware['z_axis']['version'] in ('y_axis_canopen', 'axis_d1'): can_open_master = CanOpenMasterHardware(robot_brain, can=can, name='master') else: can_open_master = None - y_axis: ChainAxisHardware | YAxisStepperHardware | YAxisCanOpenHardware | None - if config_hardware['y_axis']['version'] == 'chain_axis': + y_axis: ChainAxisHardware | YAxisStepperHardware | YAxisCanOpenHardware | AxisD1 | None + if config_hardware['y_axis']['version'] == 'axis_d1': + y_axis = AxisD1(robot_brain, + can=can, + can_address=config_hardware['y_axis']['can_address'], + name=config_hardware['y_axis']['name'], + homing_acceleration=config_hardware['y_axis']['homing_acceleration'], + homing_velocity=config_hardware['y_axis']['homing_velocity'], + profile_acceleration=config_hardware['y_axis']['profile_acceleration'], + profile_velocity=config_hardware['y_axis']['profile_velocity'], + 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'],) + elif config_hardware['y_axis']['version'] == 'chain_axis': y_axis = ChainAxisHardware(robot_brain, expander=expander, name=config_hardware['y_axis']['name'], @@ -170,7 +187,7 @@ def __init__(self) -> None: else: raise NotImplementedError(f'Unknown y_axis version: {config_hardware["y_axis"]["version"]}') - z_axis: TornadoHardware | ZAxisCanOpenHardware | ZAxisStepperHardware | None + z_axis: TornadoHardware | ZAxisCanOpenHardware | ZAxisStepperHardware | AxisD1 | None if config_hardware['z_axis']['version'] == 'z_axis_stepper': z_axis = ZAxisStepperHardware(robot_brain, expander=expander, @@ -191,6 +208,19 @@ def __init__(self) -> None: reversed_direction=config_hardware['z_axis']['reversed_direction'], end_stops_inverted=config_hardware['z_axis']['end_stops_inverted'], ) + elif config_hardware['z_axis']['version'] == 'axis_d1': + z_axis = AxisD1(robot_brain, + can=can, + can_address=config_hardware['z_axis']['can_address'], + name=config_hardware['z_axis']['name'], + homing_acceleration=config_hardware['z_axis']['homing_acceleration'], + homing_velocity=config_hardware['z_axis']['homing_velocity'], + profile_acceleration=config_hardware['z_axis']['profile_acceleration'], + profile_velocity=config_hardware['z_axis']['profile_velocity'], + 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'],) 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 f4e637e2..f94860de 100644 --- a/field_friend/hardware/field_friend_simulation.py +++ b/field_friend/hardware/field_friend_simulation.py @@ -4,6 +4,7 @@ # change the config to the config of simulated Robot import config.config_selection as config_selector +from .axis import AxisSimulation from .chain_axis import ChainAxisSimulation from .external_mower import MowerSimulation from .field_friend import FieldFriend @@ -12,8 +13,6 @@ from .flashlight_v2 import FlashlightSimulationV2 from .safety import SafetySimulation from .tornado import TornadoSimulation -from .y_axis import YAxisSimulation -from .z_axis import ZAxisSimulation class FieldFriendSimulation(FieldFriend, rosys.hardware.RobotSimulation): @@ -43,11 +42,11 @@ def __init__(self, robot_id) -> None: raise NotImplementedError(f'Unknown FieldFriend tool: {tool}') wheels = rosys.hardware.WheelsSimulation(self.WHEEL_DISTANCE) - y_axis: YAxisSimulation | ChainAxisSimulation | None + y_axis: AxisSimulation | ChainAxisSimulation | None if config_hardware['y_axis']['version'] == 'chain_axis': y_axis = ChainAxisSimulation() - elif config_hardware['y_axis']['version'] in ['y_axis_stepper', 'y_axis_canopen']: - y_axis = YAxisSimulation( + elif config_hardware['y_axis']['version'] in ['y_axis_stepper', 'y_axis_canopen', 'axis_d1']: + y_axis = AxisSimulation( min_position=config_hardware['y_axis']['min_position'], max_position=config_hardware['y_axis']['max_position'], axis_offset=config_hardware['y_axis']['axis_offset'], @@ -57,9 +56,9 @@ def __init__(self, robot_id) -> None: else: raise NotImplementedError(f'Unknown Y-Axis version: {config_hardware["y_axis"]["version"]}') - z_axis: ZAxisSimulation | TornadoSimulation | None - if config_hardware['z_axis']['version'] in ['z_axis_stepper', 'z_axis_canopen']: - z_axis = ZAxisSimulation( + z_axis: AxisSimulation | TornadoSimulation | None + if config_hardware['z_axis']['version'] in ['z_axis_stepper', 'z_axis_canopen', 'axis_d1']: + z_axis = AxisSimulation( min_position=config_hardware['z_axis']['min_position'], max_position=config_hardware['z_axis']['max_position'], axis_offset=config_hardware['z_axis']['axis_offset'], diff --git a/field_friend/hardware/safety.py b/field_friend/hardware/safety.py index 2be8dab9..eead4380 100644 --- a/field_friend/hardware/safety.py +++ b/field_friend/hardware/safety.py @@ -3,18 +3,26 @@ import rosys +from .axis import Axis, AxisSimulation +from .axis_D1 import AxisD1 from .chain_axis import ChainAxis, ChainAxisHardware, ChainAxisSimulation from .double_wheels import DoubleWheelsHardware from .external_mower import Mower, MowerHardware, MowerSimulation from .flashlight import Flashlight, FlashlightHardware, FlashlightSimulation -from .flashlight_pwm import FlashlightPWM, FlashlightPWMHardware, FlashlightPWMSimulation -from .flashlight_pwm_v2 import FlashlightPWMHardwareV2, FlashlightPWMSimulationV2, FlashlightPWMV2 +from .flashlight_pwm import ( + FlashlightPWM, + FlashlightPWMHardware, + FlashlightPWMSimulation, +) +from .flashlight_pwm_v2 import ( + FlashlightPWMHardwareV2, + FlashlightPWMSimulationV2, + FlashlightPWMV2, +) from .flashlight_v2 import FlashlightHardwareV2, FlashlightSimulationV2, FlashlightV2 from .tornado import Tornado, TornadoHardware, TornadoSimulation -from .y_axis import YAxis, YAxisSimulation from .y_axis_canopen_hardware import YAxisCanOpenHardware from .y_axis_stepper_hardware import YAxisStepperHardware -from .z_axis import ZAxis, ZAxisSimulation from .z_axis_canopen_hardware import ZAxisCanOpenHardware from .z_axis_stepper_hardware import ZAxisStepperHardware @@ -25,8 +33,8 @@ class Safety(rosys.hardware.Module, abc.ABC): def __init__(self, *, wheels: rosys.hardware.Wheels, estop: rosys.hardware.EStop, - y_axis: Union[YAxis, ChainAxis, None] = None, - z_axis: Union[ZAxis, Tornado, None] = None, + y_axis: Union[Axis, ChainAxis, None] = None, + z_axis: Union[Axis, Tornado, None] = None, flashlight: Union[Flashlight, FlashlightV2, FlashlightPWM, FlashlightPWMV2, None] = None, mower: Union[Mower, None] = None, **kwargs) -> None: @@ -57,11 +65,16 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, # implement lizard stop method for available hardware lizard_code = f'let stop do {wheels.name}.speed(0, 0);' if y_axis is not None: - lizard_code += f' {y_axis.name}.stop();' + if isinstance(y_axis, AxisD1): + lizard_code += f'{y_axis.name}_motor.stop();' + else: + lizard_code += f' {y_axis.name}.stop();' if z_axis is not None: if isinstance(z_axis, TornadoHardware): lizard_code += f'{z_axis.name}_z.stop();' lizard_code += f'{z_axis.name}_motor_turn.speed(0);' + elif isinstance(z_axis, AxisD1): + lizard_code += f'{z_axis.name}_motor.stop();' else: lizard_code += f' {z_axis.name}.stop();' if isinstance(flashlight, FlashlightHardware): @@ -75,9 +88,9 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, for name in estop.pins: lizard_code += f'when estop_{name}.level == 0 then stop(); end\n' if isinstance(bumper, rosys.hardware.BumperHardware): - lizard_code += 'when ' + \ - ' or '.join(f'{bumper.name}_{pin}.level == 1' for pin in bumper.pins) + \ - f' then {wheels.name}.off(); end\n' + for name in bumper.pins: + lizard_code += f'bumper_{name}.level = 0\n' + lizard_code += f'when bumper_{name}.level == 1 then stop(); end\n' # implement stop call for "ground check" reference sensors if isinstance(y_axis, ChainAxisHardware): @@ -105,8 +118,8 @@ class SafetySimulation(Safety, rosys.hardware.ModuleSimulation): def __init__(self, *, wheels: rosys.hardware.Wheels, estop: rosys.hardware.EStop, - y_axis: Union[YAxisSimulation, ChainAxisSimulation, None] = None, - z_axis: Union[ZAxisSimulation, TornadoSimulation, None] = None, + y_axis: Union[AxisSimulation, ChainAxisSimulation, None] = None, + z_axis: Union[AxisSimulation, TornadoSimulation, None] = None, flashlight: Union[FlashlightSimulation, FlashlightSimulationV2, FlashlightPWMSimulation, FlashlightPWMSimulationV2, None], mower: Union[MowerSimulation, None] = None) -> None: super().__init__(wheels=wheels, estop=estop, y_axis=y_axis, z_axis=z_axis, flashlight=flashlight, mower=mower) diff --git a/field_friend/hardware/teltonika_router.py b/field_friend/hardware/teltonika_router.py index a3d8a288..9d9269f7 100644 --- a/field_friend/hardware/teltonika_router.py +++ b/field_friend/hardware/teltonika_router.py @@ -25,6 +25,8 @@ def __init__(self) -> None: self.auth_token: str = '' self.token_time: float = 0.0 self.connection_check_running = False + self.mobile_upload_permission = False + self.MOBILE_UPLOAD_PERMISSION_CHANGED = rosys.event.Event() if ADMIN_PASSWORD: self.log.info('Connecting to Teltonika router...') rosys.on_repeat(self.get_current_connection, 1.0) diff --git a/field_friend/hardware/y_axis.py b/field_friend/hardware/y_axis.py deleted file mode 100644 index c0394a70..00000000 --- a/field_friend/hardware/y_axis.py +++ /dev/null @@ -1,130 +0,0 @@ -import abc -from typing import Optional - -import rosys - - -class YAxis(rosys.hardware.Module, abc.ABC): - """The y axis module is a simple example for a representation of real or simulated robot hardware.""" - - def __init__(self, max_speed, reference_speed, min_position, max_position, axis_offset, steps_per_m, reversed_direction, **kwargs) -> None: - super().__init__(**kwargs) - - self.max_speed: int = max_speed - self.reference_speed: int = reference_speed - self.min_position: float = min_position - self.max_position: float = max_position - self.axis_offset: float = axis_offset - self.steps_per_m: float = steps_per_m - self.reversed_direction: bool = reversed_direction - - self.steps: int = 0 - self.alarm: bool = False - self.idle: bool = False - - self.is_referenced: bool = False - - self.end_l: bool = False - self.end_r: bool = False - - rosys.on_shutdown(self.stop) - - @abc.abstractmethod - async def stop(self) -> None: - pass - - @abc.abstractmethod - async def move_to(self, position: float, speed: int | None = None) -> None: - if not speed: - speed = self.max_speed - if not self.is_referenced: - raise RuntimeError('yaxis is not referenced, reference first') - if speed > self.max_speed: - raise RuntimeError(f'yaxis speed is too high, max speed is {self.max_speed}') - if not self.min_position <= position <= self.max_position: - raise RuntimeError( - f'target position {position} ist out of yaxis range {self.min_position} to {self.max_position}') - self.log.info(f'moving yaxis to {position} with speed {speed}') - - @abc.abstractmethod - 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. - - 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) - - @property - def position(self) -> float: - return self.compute_position(self.steps) - - -class YAxisSimulation(YAxis, rosys.hardware.ModuleSimulation): - '''The y axis simulation module is a simple example for a representation of simulated robot hardware. - ''' - - def __init__(self, - max_speed: int = 80_000, - reference_speed: int = 40, - min_position: float = -0.12, - max_position: float = 0.12, - axis_offset: float = 0.123, - steps_per_m: float = 666.67 * 1000, - reversed_direction: bool = False, - ) -> None: - self.speed: int = 0 - self.target_steps: Optional[int] = None - super().__init__( - max_speed=max_speed, - reference_speed=reference_speed, - min_position=min_position, - max_position=max_position, - axis_offset=axis_offset, - steps_per_m=steps_per_m, - reversed_direction=reversed_direction, - ) - - async def stop(self) -> None: - await super().stop() - self.speed = 0 - self.target_steps = None - - async def move_to(self, position: float, speed: int | None = None) -> None: - if speed is None: - speed = self.max_speed - try: - await super().move_to(position, speed) - except RuntimeError as e: - rosys.notify(e, type='negative') - self.log.error(f'could not move yaxis to {position} because of {e}') - return - self.target_steps = self.compute_steps(position) - self.speed = speed if self.target_steps > self.steps else speed * -1 - while self.target_steps is not None: - await rosys.sleep(0.2) - - async def try_reference(self) -> bool: - if not await super().try_reference(): - return False - self.steps = 0 - self.is_referenced = True - return True - - async def reset_fault(self) -> None: - self.alarm = False - - async def step(self, dt: float) -> None: - await super().step(dt) - self.steps += int(dt * self.speed) - self.idle = self.speed == 0 - if self.target_steps is not None: - if (self.speed > 0) == (self.steps > self.target_steps): - self.steps = self.target_steps - self.target_steps = None - self.speed = 0 diff --git a/field_friend/hardware/y_axis_canopen_hardware.py b/field_friend/hardware/y_axis_canopen_hardware.py index 09d9ef45..4a835628 100644 --- a/field_friend/hardware/y_axis_canopen_hardware.py +++ b/field_friend/hardware/y_axis_canopen_hardware.py @@ -3,10 +3,10 @@ import rosys from rosys.helpers import remove_indentation -from .y_axis import YAxis +from .axis import Axis -class YAxisCanOpenHardware(YAxis, rosys.hardware.ModuleHardware): +class YAxisCanOpenHardware(Axis, rosys.hardware.ModuleHardware): """The y axis hardware module is a simple example for a representation of real robot hardware.""" def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, diff --git a/field_friend/hardware/y_axis_stepper_hardware.py b/field_friend/hardware/y_axis_stepper_hardware.py index 33c212e8..788a75fb 100644 --- a/field_friend/hardware/y_axis_stepper_hardware.py +++ b/field_friend/hardware/y_axis_stepper_hardware.py @@ -3,10 +3,10 @@ import rosys from rosys.helpers import remove_indentation -from .y_axis import YAxis +from .axis import Axis -class YAxisStepperHardware(YAxis, rosys.hardware.ModuleHardware): +class YAxisStepperHardware(Axis, rosys.hardware.ModuleHardware): """The y axis hardware module is a simple example for a representation of real robot hardware.""" def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, diff --git a/field_friend/hardware/z_axis_canopen_hardware.py b/field_friend/hardware/z_axis_canopen_hardware.py index 8e1870fa..c740579d 100644 --- a/field_friend/hardware/z_axis_canopen_hardware.py +++ b/field_friend/hardware/z_axis_canopen_hardware.py @@ -3,10 +3,10 @@ import rosys from rosys.helpers import remove_indentation -from .z_axis import ZAxis +from .axis import Axis -class ZAxisCanOpenHardware(ZAxis, rosys.hardware.ModuleHardware): +class ZAxisCanOpenHardware(Axis, rosys.hardware.ModuleHardware): """The z axis hardware module is a simple example for a representation of real robot hardware.""" def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, diff --git a/field_friend/hardware/z_axis_stepper_hardware.py b/field_friend/hardware/z_axis_stepper_hardware.py index 492416a9..81c819ee 100644 --- a/field_friend/hardware/z_axis_stepper_hardware.py +++ b/field_friend/hardware/z_axis_stepper_hardware.py @@ -3,10 +3,10 @@ import rosys from rosys.helpers import remove_indentation -from .z_axis import ZAxis +from .axis import Axis -class ZAxisStepperHardware(ZAxis, rosys.hardware.ModuleHardware): +class ZAxisStepperHardware(Axis, rosys.hardware.ModuleHardware): """The z axis module is a simple example for a representation of real or simulated robot hardware.""" def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, diff --git a/field_friend/interface/components/camera_card.py b/field_friend/interface/components/camera_card.py index 44ad3e22..2ecb2067 100644 --- a/field_friend/interface/components/camera_card.py +++ b/field_friend/interface/components/camera_card.py @@ -10,7 +10,8 @@ from field_friend.automations.implements.weeding_implement import WeedingImplement -from ...hardware import FlashlightPWM, FlashlightPWMV2, Tornado, ZAxis +from ...automations import PlantLocator, Puncher +from ...hardware import Axis, FlashlightPWM, FlashlightPWMV2, Tornado from .calibration_dialog import calibration_dialog if TYPE_CHECKING: @@ -68,7 +69,7 @@ async def toggle_flashlight(): with ui.row(): ui.checkbox('Punching').bind_value(self, 'punching_enabled').tooltip( 'Enable punching mode').bind_enabled_from(self.automator, 'is_running', backward=lambda x: not x) - if isinstance(self.field_friend.z_axis, ZAxis): + if isinstance(self.field_friend.z_axis, Axis): self.depth = ui.number('depth', value=0.02, format='%.2f', step=0.01, min=self.field_friend.z_axis.max_position, max=-self.field_friend.z_axis.min_position).classes('w-16').bind_visibility_from(self, 'punching_enabled') elif isinstance(self.field_friend.z_axis, Tornado): @@ -148,7 +149,7 @@ def on_mouse_move(self, e: MouseEventArguments): self.debug_position.set_text(f'last punch: {point2d} -> {point3d}') if self.puncher is not None and self.punching_enabled: self.log.info(f'punching {point3d}') - if isinstance(self.field_friend.z_axis, ZAxis): + if isinstance(self.field_friend.z_axis, Axis): self.automator.start(self.puncher.drive_and_punch(point3d.x, point3d.y, self.depth.value)) elif isinstance(self.field_friend.z_axis, Tornado): self.automator.start(self.puncher.drive_and_punch( @@ -218,8 +219,8 @@ def build_svg_for_implement(self) -> str: def build_svg_for_plant_provider(self) -> str: if self.camera is None or self.camera.calibration is None: return '' - position = rosys.geometry.Point(x=self.camera.calibration.extrinsics.translation[0], - y=self.camera.calibration.extrinsics.translation[1]) + position = rosys.geometry.Point3d(x=self.camera.calibration.extrinsics.translation[0], + y=self.camera.calibration.extrinsics.translation[1]) 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) diff --git a/field_friend/interface/components/development.py b/field_friend/interface/components/development.py index 2cf4610f..a2587adb 100644 --- a/field_friend/interface/components/development.py +++ b/field_friend/interface/components/development.py @@ -5,6 +5,7 @@ from .hardware_control import hardware_control from .io_overview import io_overview +from .settings import settings from .status_dev import status_dev_page if TYPE_CHECKING: @@ -16,6 +17,7 @@ def development(system: 'System') -> None: with ui.card().style('background-color: #2E5396; width: 100%;'): with ui.column().style("width: 100%;"): ui.label("Development Tools").style('font-size: 1.5rem; color: white;') + settings(system) with ui.row().style("width: 100%"): with ui.card().style('background-color: #3E63A6; color: white;'): if isinstance(system.field_friend, rosys.hardware.RobotHardware): diff --git a/field_friend/interface/components/field_friend_object.py b/field_friend/interface/components/field_friend_object.py index 33bee3c9..340f90a0 100644 --- a/field_friend/interface/components/field_friend_object.py +++ b/field_friend/interface/components/field_friend_object.py @@ -4,7 +4,7 @@ from rosys.geometry import Prism from rosys.vision import CameraProjector, CameraProvider, camera_objects -from ...hardware import ChainAxis, FieldFriend, Tornado, YAxis, ZAxis +from ...hardware import ChainAxis, FieldFriend, Tornado, Axis class field_friend_object(robot_object): @@ -19,7 +19,7 @@ def __init__(self, odometer: Odometer, camera_provider: CameraProvider, self.with_stl('assets/field_friend.stl', x=-0.365, y=-0.3, z=0.06, scale=0.001, color='#6E93D6', opacity=0.7) camera_objects(camera_provider, CameraProjector(camera_provider, interval=0.1), interval=0.1) with self: - if isinstance(self.robot.y_axis, YAxis): + if isinstance(self.robot.y_axis, Axis): with Group() as self.tool: Box(0.015, 0.015, 0.35).material('#4488ff').move(z=0.4) Cylinder(0.03, 0, 0.05).material('#4488ff').move(z=0.2).rotate(1.571, 0, 0) @@ -34,13 +34,13 @@ def __init__(self, odometer: Odometer, camera_provider: CameraProvider, def update(self) -> None: super().update() - if isinstance(self.robot.y_axis, YAxis) and isinstance(self.robot.z_axis, ZAxis): + if isinstance(self.robot.y_axis, Axis) and isinstance(self.robot.z_axis, Axis): self.tool.move(x=self.robot.WORK_X, y=self.robot.y_axis.position, z=self.robot.z_axis.position) if self.robot.y_axis.position > self.robot.y_axis.max_position or self.robot.y_axis.position < self.robot.y_axis.min_position: self.tool.material('red') else: self.tool.material('#4488ff') - elif isinstance(self.robot.y_axis, YAxis) and isinstance(self.robot.z_axis, Tornado): + elif isinstance(self.robot.y_axis, Axis) and isinstance(self.robot.z_axis, Tornado): self.tool.move(x=self.robot.WORK_X, y=self.robot.y_axis.position, z=self.robot.z_axis.position_z) if self.robot.y_axis.position > self.robot.y_axis.max_position or self.robot.y_axis.position < self.robot.y_axis.min_position: self.tool.material('red') diff --git a/field_friend/interface/components/field_planner.py b/field_friend/interface/components/field_planner.py index e4fef760..822974ef 100644 --- a/field_friend/interface/components/field_planner.py +++ b/field_friend/interface/components/field_planner.py @@ -1,16 +1,20 @@ import logging from typing import TYPE_CHECKING, Literal, Optional, TypedDict -from nicegui import events, ui +from nicegui import app, events, ui from ...automations import Field, FieldObstacle, Row from .field_creator import FieldCreator +from .geodata_importer import geodata_importer from .leaflet_map import leaflet_map if TYPE_CHECKING: from field_friend.system import System +TabType = Literal["Plants", "Obstacles", "Outline", "Rows"] + + class ActiveObject(TypedDict): object_type: Literal["Obstacles", "Rows", "Outline"] object: Row | FieldObstacle @@ -25,14 +29,21 @@ def __init__(self, system: 'System', leaflet: leaflet_map) -> None: self.gnss = system.gnss self.cultivatable_crops = system.crop_category_names self.leaflet_map = leaflet - self.active_field: Field | None = None + self.tab: TabType = "Plants" self.active_object: ActiveObject | None = None - self.tab: Literal["Plants", "Obstacles", "Outline", "Rows"] = "Plants" + self.active_field: Field | None = None + self.restore_actives() + + with ui.dialog() as self.clear_field_dialog, ui.card(): + ui.label('Are you sure you want to delete all fields?') + with ui.row(): + ui.button('Cancel', on_click=self.clear_field_dialog.close) + ui.button('Yes, delete', on_click=self.clear_fields, color='warning') with ui.row().classes("w-full").style("height: 100%; max-height:100%; width: 100%;"): with ui.card().style("width: 40%; max-width: 40%; max-height: 100%; height: 100%;"): with ui.row(): - ui.button("Upload Field", on_click=lambda field_provider=self.field_provider: geodata_picker(field_provider)) \ + ui.button("Upload Field", on_click=lambda field_provider=self.field_provider: geodata_importer(field_provider)) \ .tooltip("Upload a file with field boundaries. Supported file formats: KML, XML and Shape").classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;") ui.button("Add field", on_click=self.field_provider.create_field).tooltip("Add a new field") \ .classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;") @@ -41,7 +52,7 @@ def __init__(self, system: 'System', leaflet: leaflet_map) -> None: with ui.row().style("width: 100%;"): self.show_field_table() with ui.row(): - ui.button("Clear fields", on_click=self.field_provider.clear_fields).props("outline color=warning") \ + 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") \ .tooltip("Set current position as geo reference and restart the system").classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;") @@ -99,6 +110,12 @@ def show_field_table(self) -> None: @ui.refreshable def show_field_settings(self) -> None: + with ui.dialog() as self.delete_active_field_dialog, ui.card(): + ui.label('Are you sure you want to delete this field?') + with ui.row(): + ui.button('Cancel', on_click=self.delete_active_field_dialog.close) + ui.button('Yes, delete', on_click=lambda field=self.active_field: self.delete_field( + field), color='warning') with ui.card().classes('col-grow').style("max-height: 100%; height: 100%;"): if self.active_field is None: with ui.column().style("display: block; margin: auto;"): @@ -112,7 +129,7 @@ def show_field_settings(self) -> None: .on("blur", self.field_provider.request_backup) \ .bind_value(self.active_field, "name") \ .classes("w-32") - ui.button(on_click=lambda field=self.active_field: self.field_provider.remove_field(field)) \ + ui.button(on_click=self.delete_active_field_dialog.open) \ .props("icon=delete color=warning fab-mini flat") \ .classes("ml-auto").style("display: block; margin-top:auto; margin-bottom: auto;") \ .tooltip("Delete field") @@ -154,10 +171,10 @@ def show_field_settings(self) -> None: for obstacle in self.active_field.obstacles: radio_el[obstacle.id] = obstacle.name if (self.active_object is not None and self.active_object["object"] is not None and self.tab == "Obstacles"): - obstacle_radio = ui.radio(radio_el, on_change=lambda event: self._set_active_object( + ui.radio(radio_el, on_change=lambda event: self._set_active_object( event.value, self.tab), value=self.active_object["object"].id) else: - obstacle_radio = ui.radio( + ui.radio( radio_el, on_change=lambda event: self._set_active_object(event.value, self.tab)) with ui.row().classes("items-center mt-3").style("width: 100%"): ui.button(icon="add", on_click=lambda field=self.active_field: self.field_provider @@ -168,10 +185,10 @@ def show_field_settings(self) -> None: for row in self.active_field.rows: radio_el[row.id] = row.name if (self.active_object is not None and self.active_object["object"] is not None and self.tab == "Rows"): - row_radio = ui.radio(radio_el, on_change=lambda event: - self._set_active_object(event.value, self.tab), value=self.active_object["object"].id) + ui.radio(radio_el, on_change=lambda event: + self._set_active_object(event.value, self.tab), value=self.active_object["object"].id) else: - row_radio = ui.radio( + ui.radio( radio_el, on_change=lambda event: self._set_active_object(event.value, self.tab)) with ui.row().classes("items-center mt-3").style("width: 100%"): ui.button(icon="add", on_click=lambda field=self.active_field: self.field_provider.create_row( @@ -188,16 +205,21 @@ def show_object_settings(self) -> None: ui.label("select an object").style("display: block; margin: auto; color: #6E93D6;") else: if self.tab == "Obstacles": + with ui.dialog() as self.delete_obstacle_dialog, ui.card(): + ui.label('Are you sure you want to delete this obstacle?') + with ui.row(): + ui.button('Cancel', on_click=self.delete_obstacle_dialog.close) + ui.button('Yes, delete', on_click=lambda field=self.active_field, + obstacle=self.active_object['object']: self.delete_obstacle(field, obstacle), color='warning') with ui.row().style("width: 100%;"): ui.icon("dangerous").props("size=sm color=primary") \ .style("display:block; margin-top:auto; margin-bottom: auto;") ui.input("Obstacle name", value=f'{self.active_object["object"].name}',) \ .on("blur", self.field_provider.invalidate).bind_value(self.active_object["object"], "name").classes("w-32") - ui.button(on_click=lambda field=self.active_field, obstacle=self.active_object["object"]: self.field_provider.remove_obstacle(field, obstacle)) \ + ui.button(on_click=self.delete_obstacle_dialog.open) \ .props("icon=delete color=warning fab-mini flat").classes("ml-auto").style("display:block; margin-top:auto; margin-bottom: auto;").tooltip("Delete obstacle") with ui.column().style("display: block; overflow: auto; width: 100%"): assert self.active_field is not None - assert self.active_field.reference is not None for geo_point in self.active_object["object"].points: with ui.row().style("width: 100%;"): ui.button(on_click=lambda point=geo_point: self.leaflet_map.m.set_center(point.tuple)) \ @@ -211,10 +233,16 @@ def show_object_settings(self) -> None: with ui.row().classes("items-center mt-2"): ui.icon("place").props("size=sm color=grey").classes("ml-8") ui.button("", on_click=lambda field=self.active_field, obstacle=self.active_object["object"]: self.field_provider - .add_obstacle_point(field, obstacle),).props("icon=add color=primary fab-mini flat") + .add_obstacle_point(field, obstacle)).props("icon=add color=primary fab-mini flat") ui.button("", on_click=lambda obstacle=self.active_object["object"]: self.field_provider.remove_obstacle_point(obstacle)) \ .props("icon=remove color=warning fab-mini flat") elif self.tab == "Rows": + with ui.dialog() as self.delete_row_dialog, ui.card(): + ui.label('Are you sure you want to delete this row?') + with ui.row(): + ui.button('Cancel', on_click=self.delete_row_dialog.close) + ui.button('Yes, delete', on_click=lambda field=self.active_field, + row=self.active_object['object']: self.delete_row(field, row), color='warning') with ui.row().style("width: 100%"): ui.icon("spa").props("size=sm color=primary") \ .style("height: 100%; margin-top:auto; margin-bottom: auto;") @@ -225,9 +253,10 @@ def show_object_settings(self) -> None: .props("icon=expand_more color=primary fab-mini flat").classes("ml-auto").style("display:block; margin-top:auto; margin-bottom: auto; margin-left: 0; margin-right: 0;") ui.input(value=self.active_object["object"].name).on("blur", self.field_provider.invalidate) \ .bind_value(self.active_object["object"], "name").classes("w-32") - ui.button(on_click=lambda row=self.active_object["object"]: self.field_provider.remove_row(self.active_field, row)) \ + ui.button(on_click=self.delete_row_dialog.open) \ .props("icon=delete color=warning fab-mini flat").classes("ml-auto").style("display:block; margin-top:auto; margin-bottom: auto;").tooltip("Delete Row") with ui.column().style("display: block; overflow: auto; width: 100%"): + assert self.active_field is not None for geo_point in self.active_object["object"].points: with ui.row().style("width: 100%;"): ui.button(on_click=lambda point=geo_point: self.leaflet_map.m.set_center(point.tuple)) \ @@ -240,7 +269,7 @@ def show_object_settings(self) -> None: .add_row_point(field, row, point)).props("icon=edit_location_alt color=primary fab-mini flat").tooltip("Relocate point").classes("ml-0") with ui.row().classes("items-center mt-2").style("display: block; margin: auto;"): ui.icon("place").props("size=sm color=grey").classes("ml-2") - ui.button("", on_click=lambda field=self.active_field, row=self.active_object["object"]: self.field_provider.add_row_point(field, row),) \ + ui.button("", on_click=lambda field=self.active_field, row=self.active_object["object"]: self.field_provider.add_row_point(field, row)) \ .props("icon=add color=primary fab-mini flat").tooltip("Add point") ui.button("", on_click=lambda row=self.active_object["object"]: self.field_provider.remove_row_point(row),) \ .props("icon=remove color=warning fab-mini flat").tooltip("Remove point") @@ -253,9 +282,11 @@ def refresh_ui(self) -> None: self.active_field = None self.active_object = None else: - if (self.active_object is not None and self.active_object["object"] is not None): - if (self.active_object["object"] not in self.active_field.obstacles and self.active_object["object"] not in self.active_field.rows): + if self.active_object and self.active_object.get("object") is not None: + if not (self.active_object.get("object") in self.active_field.obstacles or self.active_object.get("object") in self.active_field.rows): self.active_object = None + else: + self.tab = self.active_object["object_type"] else: self.active_object = None else: @@ -267,20 +298,65 @@ def refresh_ui(self) -> None: def _set_active_field(self, field_id: str) -> None: self.active_field = self.field_provider.get_field(field_id) + if self.active_field is not None: + app.storage.user['active_field'] = self.active_field.id + else: + app.storage.user['active_field'] = None self.show_field_settings.refresh() + self.show_object_settings.refresh() - def _set_active_object(self, object_id: Optional[str] = None, object_type: Optional[Literal["Obstacles", "Rows", "Outline"]] = None) -> None: + def _set_active_object(self, object_id: Optional[str] = None, object_type: TabType | None = None) -> None: if (self.active_field is not None and object_id is not None and object_type is not None): if object_type == "Obstacles": - for obstacle in self.active_field.obstacles: - if object_id == obstacle.id: - self.active_object = {"object_type": object_type, "object": obstacle} + self.active_object = next( + ({"object_type": object_type, "object": obj} + for obj in self.active_field.obstacles if obj.id == object_id), + None + ) + app.storage.user['active_object'] = { + "object_type": "Obstacles", "object": self.active_object["object"].id} elif object_type == "Rows": - for row in self.active_field.rows: - if object_id == row.id: - self.active_object = {"object_type": object_type, "object": row} + self.active_object = next( + ({"object_type": object_type, "object": obj} + for obj in self.active_field.rows if obj.id == object_id), + None + ) + app.storage.user['active_object'] = { + "object_type": "Rows", "object": self.active_object["object"].id} else: self.active_object = None + app.storage.user['active_object'] = None else: self.active_object = None + app.storage.user['active_object'] = None self.show_object_settings.refresh() + + def delete_field(self, field: Field) -> None: + self.delete_active_field_dialog.close() + self.field_provider.remove_field(field) + + def clear_fields(self) -> None: + self.clear_field_dialog.close() + self.field_provider.clear_fields() + + def delete_obstacle(self, field: Field, obstacle: FieldObstacle) -> None: + self.delete_obstacle_dialog.close() + self.field_provider.remove_obstacle(field, obstacle) + + def delete_row(self, field: Field, row: Row) -> None: + self.delete_row_dialog.close() + self.field_provider.remove_row(field, row) + + def restore_actives(self) -> None: + stored_field = app.storage.user.get('active_field', None) + if stored_field is not None: + self.active_field = self.field_provider.get_field(stored_field) + stored_object = app.storage.user.get('active_object', None) + if self.active_field is not None and stored_object is not None: + if stored_object["object_type"] == "Obstacles": + self.active_object = next(({"object_type": stored_object["object_type"], "object": obj} + for obj in self.active_field.obstacles if obj.id == stored_object["object"]), None) + elif stored_object["object_type"] == "Rows": + self.active_object = next(({"object_type": stored_object["object_type"], "object": obj} + for obj in self.active_field.rows if obj.id == stored_object["object"]), None) + self.refresh_ui() diff --git a/field_friend/interface/components/geodata_picker.py b/field_friend/interface/components/geodata_importer.py similarity index 90% rename from field_friend/interface/components/geodata_picker.py rename to field_friend/interface/components/geodata_importer.py index d79d37b0..e95651e7 100644 --- a/field_friend/interface/components/geodata_picker.py +++ b/field_friend/interface/components/geodata_importer.py @@ -1,7 +1,4 @@ import json -import platform -import tempfile -import uuid import xml.etree.ElementTree as ET from pathlib import Path from typing import Optional @@ -13,6 +10,7 @@ from shapely.ops import transform from ...automations import Field, FieldProvider +from ...localization import GeoPoint # Enable fiona driver fiona.drvsupport.supported_drivers['kml'] = 'rw' # enable KML support which is disabled by default @@ -20,7 +18,7 @@ fiona.drvsupport.supported_drivers['LIBKML'] = 'rw' -class geodata_picker(ui.dialog): +class geodata_importer(ui.dialog): def __init__(self, field_provider: FieldProvider) -> None: super().__init__() self.field_provider = field_provider @@ -44,7 +42,7 @@ def extract_coordinates_kml(self, event: events.UploadEventArguments) -> list: x_coordinate, y_coordinate = gdf['geometry'].iloc[0].xy extracted_points = list(zip(x_coordinate, y_coordinate)) for point in extracted_points: - coordinates.append([point[1], point[0]]) + coordinates.append(GeoPoint(lat=point[1], long=point[0])) return coordinates def extract_coordinates_xml(self, event: events.UploadEventArguments) -> list: @@ -55,7 +53,7 @@ def extract_coordinates_xml(self, event: events.UploadEventArguments) -> list: for point in geo_data.findall('.//PNT'): lat = float(point.attrib['C']) lon = float(point.attrib['D']) - coordinates.append([lat, lon]) + coordinates.append(GeoPoint(lat=lat, long=lon)) return coordinates def extract_coordinates_shp(self, event: events.UploadEventArguments) -> Optional[list]: @@ -66,7 +64,9 @@ def extract_coordinates_shp(self, event: events.UploadEventArguments) -> Optiona print(gdf) gdf['geometry'] = gdf['geometry'].apply(lambda geom: transform(self.swap_coordinates, geom)) feature = json.loads(gdf.to_json()) - coordinates = feature["features"][0]["geometry"]["coordinates"][0] + shp_coordinates = feature["features"][0]["geometry"]["coordinates"][0] + for point in shp_coordinates: + coordinates.append(GeoPoint(lat=point[0], long=point[1])) return coordinates except: rosys.notify("The .zip file does not contain a shape file.", type='warning') diff --git a/field_friend/interface/components/hardware_control.py b/field_friend/interface/components/hardware_control.py index 2a1e04b2..33f796bd 100644 --- a/field_friend/interface/components/hardware_control.py +++ b/field_friend/interface/components/hardware_control.py @@ -4,8 +4,8 @@ from ...automations import Puncher from ...hardware import (ChainAxis, FieldFriend, FieldFriendHardware, Flashlight, FlashlightPWM, FlashlightPWMV2, - FlashlightV2, Mower, MowerHardware, MowerSimulation, Tornado, YAxis, YAxisCanOpenHardware, - ZAxis, ZAxisCanOpenHardware) + FlashlightV2, Mower, MowerHardware, MowerSimulation, Tornado, Axis, YAxisCanOpenHardware, + ZAxisCanOpenHardware) from .confirm_dialog import ConfirmDialog as confirm_dialog from .status_bulb import StatusBulb as status_bulb @@ -64,7 +64,7 @@ async def toggle_flashlight(e: ValueChangeEventArguments) -> None: field_friend.flashlight, 'duty_cycle') if field_friend.y_axis is not None: - if isinstance(field_friend.y_axis, YAxis): + if isinstance(field_friend.y_axis, Axis): with ui.column(): ui.markdown('**Y-Axis**') ui.button('Reference', on_click=lambda: automator.start(field_friend.y_axis.try_reference())) @@ -89,7 +89,7 @@ async def toggle_flashlight(e: ValueChangeEventArguments) -> None: ui.button('chop to right', on_click=lambda: automator.start(field_friend.y_axis.move_dw_to_r_ref())) if field_friend.z_axis is not None: - if isinstance(field_friend.z_axis, ZAxis): + if isinstance(field_friend.z_axis, Axis): with ui.column(): ui.markdown('**Z-Axis**') ui.button('Reference', on_click=lambda: automator.start(field_friend.z_axis.try_reference())) @@ -131,13 +131,13 @@ async def toggle_flashlight(e: ValueChangeEventArguments) -> None: 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.max_position, depth=depth.value))) - elif isinstance(field_friend.y_axis, YAxis) and isinstance(field_friend.z_axis, Tornado): + elif isinstance(field_friend.y_axis, Axis) and isinstance(field_friend.z_axis, Tornado): ui.button(on_click=lambda: automator.start( puncher.punch(field_friend.y_axis.min_position, angle=angle.value))) ui.button(on_click=lambda: automator.start(puncher.punch(0, angle=angle.value))) ui.button(on_click=lambda: automator.start( puncher.punch(field_friend.y_axis.max_position, angle=angle.value))) - elif isinstance(field_friend.y_axis, YAxis) and isinstance(field_friend.z_axis, ZAxis): + elif isinstance(field_friend.y_axis, Axis) and isinstance(field_friend.z_axis, Axis): ui.button(on_click=lambda: automator.start( puncher.punch(field_friend.y_axis.max_position, depth=depth.value))) ui.button(on_click=lambda: automator.start(puncher.punch(0, depth=depth.value))) diff --git a/field_friend/interface/components/leaflet_map.py b/field_friend/interface/components/leaflet_map.py index 7e106603..0fcb8a38 100644 --- a/field_friend/interface/components/leaflet_map.py +++ b/field_friend/interface/components/leaflet_map.py @@ -1,14 +1,11 @@ import logging -import uuid -from typing import TYPE_CHECKING, Any, Literal, TypedDict +from typing import TYPE_CHECKING, Literal, TypedDict -import rosys -import rosys.geometry from nicegui import app, events, ui -from nicegui.elements.leaflet_layers import GenericLayer, TileLayer +from nicegui.elements.leaflet_layers import GenericLayer, Marker, TileLayer -from ...automations import Field, FieldObstacle, FieldProvider, Row +from ...automations import Field, FieldObstacle, Row from ...localization.geo_point import GeoPoint from .key_controls import KeyControls @@ -55,7 +52,7 @@ def __init__(self, system: 'System', draw_tools: bool) -> None: self.current_basemap: TileLayer | None = None self.toggle_basemap() self.field_layers: list[GenericLayer] = [] - self.robot_marker = None + self.robot_marker: Marker | None = None self.drawn_marker = None self.obstacle_layers: list = [] self.row_layers: list = [] @@ -67,8 +64,8 @@ def __init__(self, system: 'System', draw_tools: bool) -> None: def handle_draw(e: events.GenericEventArguments): if e.args['layerType'] == 'marker': - latlon = (e.args['layer']['_latlng']['lat'], - e.args['layer']['_latlng']['lng']) + latlon: list[float] = [e.args['layer']['_latlng']['lat'], + e.args['layer']['_latlng']['lng']] self.drawn_marker = self.m.marker(latlng=latlon) if system.is_real: with ui.dialog() as real_marker_dialog, ui.card(): @@ -76,13 +73,17 @@ def handle_draw(e: events.GenericEventArguments): ui.button('add point to selected object (row, obstacle)', on_click=lambda: self.add_point_active_object(latlon, real_marker_dialog)) ui.button('Close', on_click=lambda: self.abort_point_drawing(real_marker_dialog)) + real_marker_dialog.on('hide', self.on_dialog_close) real_marker_dialog.open() else: with ui.dialog() as simulated_marker_dialog, ui.card(): ui.label('You are currently working in a simulation. What does the placed point represent?') ui.button('as point for the current object', on_click=lambda: self.add_point_active_object(latlon, simulated_marker_dialog)) + ui.button('update roboter position', on_click=lambda: self.update_robot_position( + GeoPoint.from_list(latlon), simulated_marker_dialog)) ui.button('Close', on_click=lambda: self.abort_point_drawing(simulated_marker_dialog)) + simulated_marker_dialog.on('hide', self.on_dialog_close) simulated_marker_dialog.open() if e.args['layerType'] == 'polygon': coordinates = e.args['layer']['_latlngs'] @@ -111,16 +112,14 @@ def buttons(self) -> None: .tooltip('center map on field boundaries').classes('ml-0') def abort_point_drawing(self, dialog) -> None: - if self.drawn_marker is not None: - self.m.remove_layer(self.drawn_marker) - self.drawn_marker = None + self.on_dialog_close() dialog.close() def add_point_active_object(self, latlon, dialog) -> None: dialog.close() - self.m.remove_layer(self.drawn_marker) - if self.active_object is not None and self.active_object["object"] is not None: - self.active_object["object"].points.append(GeoPoint.from_list(latlon)) + self.on_dialog_close() + if self.active_object and self.active_object.get("object") is not None: + self.active_object.get("object").points.append(GeoPoint.from_list(latlon)) self.field_provider.invalidate() self.update_layers() else: @@ -135,19 +134,19 @@ def update_layers(self) -> None: color = '#6E93D6' if field.id == self.active_field else '#999' self.field_layers.append(self.m.generic_layer(name="polygon", args=[field.points_as_tuples, {'color': color}])) - field = self.field_provider.get_field(self.active_field) - if field is None: - return + current_field: Field | None = self.field_provider.get_field(self.active_field) for layer in self.obstacle_layers: self.m.remove_layer(layer) self.obstacle_layers = [] for layer in self.row_layers: self.m.remove_layer(layer) self.row_layers = [] - for obstacle in field.obstacles: + if current_field is None: + return + for obstacle in current_field.obstacles: self.obstacle_layers.append(self.m.generic_layer(name="polygon", args=[obstacle.points_as_tuples, {'color': '#C10015'}])) - for row in field.rows: + for row in current_field.rows: self.row_layers.append(self.m.generic_layer(name="polyline", args=[row.points_as_tuples, {'color': '#F2C037'}])) @@ -160,9 +159,12 @@ def active_field(self, field_id: str | None) -> None: self._active_field = field_id self.update_layers() - def update_robot_position(self, position: GeoPoint) -> None: - if self.robot_marker is None: - self.robot_marker = self.m.marker(latlng=position.tuple) + def update_robot_position(self, position: GeoPoint, dialog=None) -> None: + if dialog: + self.on_dialog_close() + dialog.close() + self.gnss.relocate(position) + self.robot_marker = self.robot_marker or self.m.marker(latlng=position.tuple) icon = 'L.icon({iconUrl: "assets/robot_position_side.png", iconSize: [50,50], iconAnchor:[20,20]})' self.robot_marker.run_method(':setIcon', icon) self.robot_marker.move(*position.tuple) @@ -215,3 +217,8 @@ def toggle_basemap(self) -> None: ) if self.current_basemap.options['maxZoom'] - 1 < self.m.zoom: self.m.set_zoom(self.current_basemap.options['maxZoom'] - 1) + + def on_dialog_close(self) -> None: + if self.drawn_marker is not None: + self.m.remove_layer(self.drawn_marker) + self.drawn_marker = None diff --git a/field_friend/interface/components/plant_object.py b/field_friend/interface/components/plant_object.py index c5a8b827..3db2265a 100644 --- a/field_friend/interface/components/plant_object.py +++ b/field_friend/interface/components/plant_object.py @@ -18,7 +18,7 @@ def __init__(self, plant_provider: PlantProvider, weed_category_names: list[str] self.plant_provider.PLANTS_CHANGED.register_ui(self.update) def update(self) -> None: - origin = rosys.geometry.Point(x=0, y=0) + origin = rosys.geometry.Point3d(x=0, y=0, z=0) in_world = {p.id: p for p in self.plant_provider.get_relevant_weeds(origin, max_distance=1000) + self.plant_provider.get_relevant_crops(origin, max_distance=1000)} diff --git a/field_friend/interface/components/settings.py b/field_friend/interface/components/settings.py new file mode 100644 index 00000000..570748ec --- /dev/null +++ b/field_friend/interface/components/settings.py @@ -0,0 +1,18 @@ +from typing import TYPE_CHECKING + +from nicegui import ui + +if TYPE_CHECKING: + from field_friend.system import System + + +def settings(system: 'System') -> None: + with ui.card().style('background-color: #3E63A6; color: white;'): + ui.markdown('**Settings**').style('color: #6E93D6;').classes('w-full text-center') + ui.separator() + with ui.column().classes('items-stretch'): + if system.is_real: + ui.markdown('**Learning Loop**') + with ui.row().classes('items-center'): + ui.switch('allow image upload via mobile network', value=False, on_change=system.teltonika_router.MOBILE_UPLOAD_PERMISSION_CHANGED.emit).tooltip( + 'enable detected image upload to learning loop via mobile network. When off, images will only be uploaded with Wifi connection or cable').bind_value(system.teltonika_router, 'mobile_upload_permission') diff --git a/field_friend/interface/components/status_dev.py b/field_friend/interface/components/status_dev.py index 9551d577..b8c27b79 100644 --- a/field_friend/interface/components/status_dev.py +++ b/field_friend/interface/components/status_dev.py @@ -6,8 +6,8 @@ from nicegui import ui from ... import localization -from ...hardware import (ChainAxis, FieldFriend, FieldFriendHardware, FlashlightPWMHardware, FlashlightPWMHardwareV2, - Tornado, YAxis, ZAxis) +from ...hardware import (Axis, AxisD1, ChainAxis, FieldFriend, FieldFriendHardware, FlashlightPWMHardware, + FlashlightPWMHardwareV2, Tornado) if TYPE_CHECKING: from field_friend.system import System @@ -27,7 +27,7 @@ def status_dev_page(robot: FieldFriend, system: 'System'): ui.label('Software ESTOP is active!').classes('text-red mt-1') with ui.row().bind_visibility_from(robot.estop, 'active', value=False): - if isinstance(robot.z_axis, ZAxis): + if isinstance(robot.z_axis, Axis): with ui.row().bind_visibility_from(robot.z_axis, 'end_t'): ui.icon('report').props('size=md').classes('text-red') ui.label('Z-axis in end top position, error!').classes('text-red mt-1') @@ -39,7 +39,7 @@ def status_dev_page(robot: FieldFriend, system: 'System'): with ui.row().bind_visibility_from(robot.z_axis, 'alarm'): ui.icon('report').props('size=md').classes('text-yellow') ui.label('Z-axis in alarm, warning!').classes('text-orange mt-1') - if isinstance(robot.y_axis, ChainAxis) or isinstance(robot.y_axis, YAxis): + if isinstance(robot.y_axis, ChainAxis) or isinstance(robot.y_axis, Axis): with ui.row().bind_visibility_from(robot.y_axis, 'alarm'): ui.icon('report').props('size=md').classes('text-yellow') ui.label('Y-axis in alarm, warning!').classes('text-orange mt-1') @@ -213,7 +213,7 @@ def update_status() -> None: f'{robot.y_axis.steps:.0f}', f'{robot.y_axis.position:.2f}m' if robot.y_axis.is_referenced else '' ] - elif isinstance(robot.y_axis, YAxis): + elif isinstance(robot.y_axis, Axis): y_axis_flags = [ 'not referenced' if not robot.y_axis.is_referenced else '', 'alarm' if robot.y_axis.alarm else '', @@ -225,7 +225,7 @@ def update_status() -> None: ] else: y_axis_flags = ['no y-axis'] - if isinstance(robot.z_axis, ZAxis): + if isinstance(robot.z_axis, Axis): z_axis_flags = [ '' if robot.z_axis.is_referenced else 'not referenced', 'alarm' if robot.z_axis.alarm else '', diff --git a/field_friend/interface/components/status_drawer.py b/field_friend/interface/components/status_drawer.py index 7df0039d..b55d71cc 100644 --- a/field_friend/interface/components/status_drawer.py +++ b/field_friend/interface/components/status_drawer.py @@ -4,7 +4,14 @@ from nicegui import ui from ... import localization -from ...hardware import ChainAxis, FieldFriend, FlashlightPWMHardware, FlashlightPWMHardwareV2, Tornado, YAxis, ZAxis +from ...hardware import ( + Axis, + ChainAxis, + FieldFriend, + FlashlightPWMHardware, + FlashlightPWMHardwareV2, + Tornado, +) from ...localization import Gnss if TYPE_CHECKING: @@ -42,7 +49,7 @@ def status_drawer(system: 'System', robot: FieldFriend, gnss: Gnss, odometer: ro ui.label('Software ESTOP is active!').classes('text-red mt-1') with ui.row().bind_visibility_from(robot.estop, 'active', value=False): - if isinstance(robot.z_axis, ZAxis): + if isinstance(robot.z_axis, Axis): with ui.row().bind_visibility_from(robot.z_axis, 'end_t'): ui.icon('report').props('size=md').classes('text-red') ui.label('Z-axis in top end position!').classes('text-red mt-1') @@ -54,7 +61,7 @@ def status_drawer(system: 'System', robot: FieldFriend, gnss: Gnss, odometer: ro with ui.row().bind_visibility_from(robot.z_axis, 'alarm'): ui.icon('report').props('size=md').classes('text-yellow') ui.label('Z-axis in alarm, warning!').classes('text-orange mt-1') - if isinstance(robot.y_axis, ChainAxis) or isinstance(robot.y_axis, YAxis): + if isinstance(robot.y_axis, ChainAxis) or isinstance(robot.y_axis, Axis): with ui.row().bind_visibility_from(robot.y_axis, 'alarm'): ui.icon('report').props('size=md').classes('text-yellow') ui.label('Y-axis in alarm, warning!').classes('text-orange mt-1') @@ -151,7 +158,7 @@ def update_status() -> None: f'{robot.y_axis.steps:.0f}', f'{robot.y_axis.position:.2f}m' if robot.y_axis.is_referenced else '' ] - elif isinstance(robot.y_axis, YAxis): + elif isinstance(robot.y_axis, Axis): y_axis_flags = [ 'not referenced' if not robot.y_axis.is_referenced else '', 'alarm' if robot.y_axis.alarm else '', @@ -163,7 +170,7 @@ def update_status() -> None: ] else: y_axis_flags = ['no y-axis'] - if isinstance(robot.z_axis, ZAxis): + if isinstance(robot.z_axis, Axis): z_axis_flags = [ '' if robot.z_axis.is_referenced else 'not referenced', 'alarm' if robot.z_axis.alarm else '', diff --git a/field_friend/localization/gnss_simulation.py b/field_friend/localization/gnss_simulation.py index b2589f32..f2adeb43 100644 --- a/field_friend/localization/gnss_simulation.py +++ b/field_friend/localization/gnss_simulation.py @@ -1,6 +1,7 @@ from typing import Optional import rosys +from rosys.geometry import Pose from .. import localization from .geo_point import GeoPoint @@ -27,12 +28,16 @@ async def _create_new_record(self) -> Optional[GNSSRecord]: else: new_position = localization.reference.shifted(pose.point) record = GNSSRecord(timestamp=pose.time, location=new_position) - record.mode = "simulation" # TODO check for possible values and replace "simulation" record.gps_qual = self.gps_quality record.mode = self.mode await rosys.sleep(0.1) # NOTE simulation does not be so fast and only eats a lot of cpu time return record + def relocate(self, position: GeoPoint) -> None: + localization.reference = position + self.wheels.pose = Pose(x=position.lat, y=position.long, yaw=0.0, time=rosys.time()) + self.last_pose_update = rosys.time() + def disconnect(self): """Simulate serial disconnection. diff --git a/field_friend/system.py b/field_friend/system.py index a9deeef8..0875d133 100644 --- a/field_friend/system.py +++ b/field_friend/system.py @@ -123,6 +123,10 @@ def watch_robot() -> None: self.automator = rosys.automation.Automator(self.steerer, on_interrupt=self.field_friend.stop) self.automation_watcher = AutomationWatcher(self) self.monitoring = Recorder(self) + self.timelapse_recorder = rosys.analysis.TimelapseRecorder() + self.timelapse_recorder.frame_info_builder = lambda _: f'{self.version}, {self.current_navigation.name}, tags: {", ".join(self.plant_locator.tags)}' + rosys.NEW_NOTIFICATION.register(self.timelapse_recorder.notify) + rosys.on_startup(self.timelapse_recorder.compress_video) # NOTE: cleanup JPEGs from before last shutdown self.field_navigation = RowsOnFieldNavigation(self, self.monitoring) self.straight_line_navigation = StraightLineNavigation(self, self.monitoring) self.follow_crops_navigation = FollowCropsNavigation(self, self.monitoring) diff --git a/main.py b/main.py index 8b1c8b85..f290b37f 100755 --- a/main.py +++ b/main.py @@ -3,13 +3,14 @@ from dotenv import load_dotenv from nicegui import app, ui -from rosys.analysis import logging_page +from rosys.analysis import logging_page, videos_page import field_friend.log_configuration as log_configuration from field_friend import interface from field_friend.interface.components import header_bar, status_drawer, system_bar from field_friend.system import System + logger = log_configuration.configure() app.add_static_files('/assets', 'assets') @@ -46,6 +47,7 @@ def status(): return {'status': 'ok'} logging_page(['field_friend', 'rosys']) # /logging + videos_page() # /videos app.on_startup(startup) diff --git a/requirements.txt b/requirements.txt index 1a529334..13d49442 100644 --- a/requirements.txt +++ b/requirements.txt @@ -6,5 +6,5 @@ geopy shapely fiona geopandas -rosys == v0.15.1 +rosys == v0.16.1 uvicorn == 0.28.1 \ No newline at end of file diff --git a/tests/test_navigation.py b/tests/test_navigation.py index a6b3e23d..198ae826 100644 --- a/tests/test_navigation.py +++ b/tests/test_navigation.py @@ -117,7 +117,7 @@ async def test_follow_crops_no_direction(system: System, detector: rosys.vision. assert system.automator.is_running await forward(until=lambda: not system.automator.is_running, timeout=300) assert not system.automator.is_running, 'automation should stop if no crops are detected anymore' - assert system.odometer.prediction.distance(rosys.geometry.Point(x=0, y=0)) == pytest.approx(2.0, abs=0.1) + assert system.odometer.prediction.distance(rosys.geometry.Point3d(x=0, y=0, z=0)) == pytest.approx(2.0, abs=0.1) assert system.odometer.prediction.point.x == pytest.approx(2.0, abs=0.1) assert system.odometer.prediction.point.y == pytest.approx(0, abs=0.01) assert system.odometer.prediction.yaw_deg == pytest.approx(0, abs=1.0) @@ -139,7 +139,7 @@ async def test_follow_crops_empty(system: System, detector: rosys.vision.Detecto async def test_follow_crops_straight(system: System, detector: rosys.vision.DetectorSimulation): for i in range(10): x = i/10 - p = rosys.geometry.Point3d(x=x, y=0, z=0) + p = rosys.geometry.Point3d(x=x) detector.simulated_objects.append(rosys.vision.SimulatedObject(category_name='maize', position=p)) system.current_navigation = system.follow_crops_navigation assert isinstance(system.current_navigation.implement, Recorder) diff --git a/tests/test_plant_provider.py b/tests/test_plant_provider.py index c3832640..024579ba 100644 --- a/tests/test_plant_provider.py +++ b/tests/test_plant_provider.py @@ -7,12 +7,12 @@ def test_extracting_relevant_crops(): plants = PlantProvider() for i in range(20): plants.add_crop(create_crop(i/10.0, 0)) - crops = plants.get_relevant_crops(rosys.geometry.Point(x=1.0, y=0), max_distance=0.45) + crops = plants.get_relevant_crops(rosys.geometry.Point3d(x=1.0, y=0, z=0), max_distance=0.45) assert len(crops) == 9 # TODO do not clear list; better to use weighted average in confidence property plants.crops[10].confidences.clear() plants.crops[10].confidences.append(0.4) - crops = plants.get_relevant_crops(rosys.geometry.Point(x=1.0, y=0), max_distance=0.45) + crops = plants.get_relevant_crops(rosys.geometry.Point3d(x=1.0, y=0, z=0), max_distance=0.45) assert len(crops) == 8, 'crops with a confidence of less than PlantProvider.MINIMUM_COMBINED_CROP_CONFIDENCE should be ignored' @@ -20,7 +20,7 @@ def create_crop(x: float, y: float) -> Plant: """Creates a maize plant with three observed positions at the given coordinates.""" plant = Plant(type='maize', detection_time=rosys.time()) for _ in range(3): - plant.positions.append(rosys.geometry.Point(x=x, y=y)) + plant.positions.append(rosys.geometry.Point3d(x=x, y=y, z=0)) plant.confidences.append(0.9) return plant @@ -32,7 +32,7 @@ def test_crop_prediction(): def add_crop(x): plant = Plant(type='maize', detection_time=rosys.time()) - plant.positions.append(rosys.geometry.Point(x=x, y=0)) + plant.positions.append(rosys.geometry.Point3d(x=x, y=0, z=0)) plant.confidences.append(confidence) plant_provider.add_crop(plant) diff --git a/tests/test_recorder.py b/tests/test_recorder.py index 16914178..49ee984c 100644 --- a/tests/test_recorder.py +++ b/tests/test_recorder.py @@ -11,4 +11,4 @@ async def test_locating_of_plants(system: System, detector: rosys.vision.Detecto await forward(20) assert len(system.plant_provider.crops) == 1 assert system.plant_provider.crops[0].type == 'sugar_beet' - assert_point(system.plant_provider.crops[0].position, rosys.geometry.Point(x=0.212, y=0.03)) + assert_point(system.plant_provider.crops[0].position, rosys.geometry.Point3d(x=0.212, y=0.03, z=0))