Merge commit '2a15b61c1863ba4a02407d0ba52a73fe5f1de1cf' into fix_impl…

…ement_svg

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("/", ".")

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,
+}
+}

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,
+    },
+}

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',
+}

config/f15_config_f15/robotbrain.py

@@ -0,0 +1,4 @@
+configuration = {'robot_brain': {
+    'flash_params': ['orin', 'v05', 'nand'],
+    'enable_esp_on_startup':False,
+}}

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',
     },

config/testbrain_config_rb19/params.py

@@ -6,4 +6,5 @@
     'work_x': 0.0125,
     'drill_radius': 0.025,
     'tool': 'none',
+    'antenna_offset': 0.205,
 }

config/testbrain_config_rb19/robotbrain.py

@@ -1,4 +1,4 @@
 configuration = {'robot_brain': {
-    'flash_params': ['']
+    'flash_params': ['nand']
 }}
 # test

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

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')

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

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