Continue on a row

field_friend/automations/field.py

@@ -140,22 +140,7 @@ def _generate_rows(self) -> list[Row]:
 
     def _generate_outline(self) -> list[GeoPoint]:
         assert len(self.rows) > 0
-        outline_unbuffered: list[Point] = [
-            self.first_row_end.cartesian(),
-            self.first_row_start.cartesian()
-        ]
-        if len(self.rows) > 1:
-            for p in self.rows[-1].points:
-                outline_unbuffered.append(p.cartesian())
-            outline_shape = Polygon([p.tuple for p in outline_unbuffered])
-        else:
-            outline_shape = LineString([p.tuple for p in outline_unbuffered])
-        bufferd_polygon = outline_shape.buffer(
-            self.outline_buffer_width, cap_style='square', join_style='mitre', mitre_limit=math.inf)
-        bufferd_polygon_coords = bufferd_polygon.exterior.coords
-        outline: list[GeoPoint] = [localization.reference.shifted(
-            Point(x=p[0], y=p[1])) for p in bufferd_polygon_coords]
-        return outline
+        return self.get_buffered_area(self.rows, self.outline_buffer_width)
 
     def to_dict(self) -> dict:
         return {
@@ -186,3 +171,21 @@ def from_dict(cls, data: dict[str, Any]) -> Self:
             RowSupportPoint, sp) for sp in data['row_support_points']] if 'row_support_points' in data else []
         field_data = cls(**cls.args_from_dict(data))
         return field_data
+
+    def get_buffered_area(self, rows: list[Row], buffer_width: float) -> list[GeoPoint]:
+        outline_unbuffered: list[Point] = [
+            self.first_row_end.cartesian(),
+            self.first_row_start.cartesian()
+        ]
+        if len(self.rows) > 1:
+            for p in self.rows[-1].points:
+                outline_unbuffered.append(p.cartesian())
+            outline_shape = Polygon([p.tuple for p in outline_unbuffered])
+        else:
+            outline_shape = LineString([p.tuple for p in outline_unbuffered])
+        bufferd_polygon = outline_shape.buffer(
+            self.outline_buffer_width, cap_style='square', join_style='mitre', mitre_limit=math.inf)
+        bufferd_polygon_coords = bufferd_polygon.exterior.coords
+        outline: list[GeoPoint] = [localization.reference.shifted(
+            Point(x=p[0], y=p[1])) for p in bufferd_polygon_coords]
+        return outline

field_friend/automations/navigation/field_navigation.py

@@ -20,12 +20,15 @@ class State(Enum):
     FOLLOWING_ROW = auto()
     ROW_COMPLETED = auto()
     FIELD_COMPLETED = auto()
+    ERROR = auto()
 
 
 class FieldNavigation(StraightLineNavigation):
     DRIVE_STEP = 0.2
     TURN_STEP = np.deg2rad(25.0)
     MAX_GNSS_WAITING_TIME = 15.0
+    MAX_DISTANCE_DEVIATION = 0.05
+    MAX_ANGLE_DEVIATION = np.deg2rad(10.0)
 
     def __init__(self, system: 'System', implement: Implement) -> None:
         super().__init__(system, implement)
@@ -49,6 +52,7 @@ def __init__(self, system: 'System', implement: Implement) -> None:
         self._turn_step = self.TURN_STEP
         self._max_gnss_waiting_time = self.MAX_GNSS_WAITING_TIME
         self.rows_to_work_on: list[Row] = []
+        self.robot_on_field = False
 
     @property
     def current_row(self) -> Row:
@@ -83,7 +87,7 @@ async def prepare(self) -> bool:
         return True
 
     def _should_finish(self) -> bool:
-        return self._state == State.FIELD_COMPLETED
+        return self._state in (State.FIELD_COMPLETED, State.ERROR)
 
     async def finish(self) -> None:
         await super().finish()
@@ -103,17 +107,44 @@ def set_start_and_end_points(self):
         assert self.field is not None
         self.start_point = None
         self.end_point = None
-        relative_point_0 = self.odometer.prediction.distance(self.current_row.points[0].cartesian())
-        relative_point_1 = self.odometer.prediction.distance(self.current_row.points[-1].cartesian())
-        # self.log.info(f'Relative point 0: {relative_point_0} Relative point 1: {relative_point_1}')
-        if relative_point_0 < relative_point_1:
-            self.start_point = self.current_row.points[0].cartesian()
-            self.end_point = self.current_row.points[-1].cartesian()
-        elif relative_point_1 < relative_point_0:
-            self.start_point = self.current_row.points[-1].cartesian()
-            self.end_point = self.current_row.points[0].cartesian()
+        start_point = self.current_row.points[0].cartesian()
+        end_point = self.current_row.points[-1].cartesian()
+        distance_to_start = self.odometer.prediction.distance(start_point)
+        distance_to_end = self.odometer.prediction.distance(end_point)
+        relative_start = self.odometer.prediction.relative_point(start_point)
+        relative_end = self.odometer.prediction.relative_point(end_point)
+        self.log.debug('Start: %s Relative: %s Distance: %s', start_point, relative_start, distance_to_start)
+        self.log.debug('End: %s Relative: %s Distance: %s', end_point, relative_end, distance_to_end)
+
+        swap_points: bool
+        self.robot_on_field = relative_start.x * relative_end.x <= 0
+        if self.robot_on_field:
+            self.log.debug('Robot on field')
+            foot_point = self.current_row.line_segment().line.foot_point(self.odometer.prediction.point)
+            distance_to_row = foot_point.distance(self.odometer.prediction.point)
+            if distance_to_row > self.MAX_DISTANCE_DEVIATION:
+                rosys.notify('Between two rows', 'negative')
+                return
+            abs_angle_to_start = abs(self.odometer.prediction.relative_direction(start_point))
+            abs_angle_to_end = abs(self.odometer.prediction.relative_direction(end_point))
+            if abs_angle_to_start > self.MAX_ANGLE_DEVIATION and abs_angle_to_end > self.MAX_ANGLE_DEVIATION:
+                rosys.notify('Robot heading deviates too much from row direction', 'negative')
+                return
+            swap_points = abs_angle_to_start < abs_angle_to_end
+        else:
+            self.log.debug('Robot outside of field')
+            swap_points = distance_to_start > distance_to_end
+
+        if swap_points:
+            self.log.debug('Swapping start and end points')
+            start_point, end_point = end_point, start_point
+            distance_to_start, distance_to_end = distance_to_end, distance_to_start
+            relative_start, relative_end = relative_end, relative_start
+
+        self.start_point = start_point
+        self.end_point = end_point
+        self.log.debug('Start point: %s End point: %s', self.start_point, self.end_point)
         self.update_target()
-        # self.log.info(f'Start point: {self.start_point} End point: {self.end_point}')
 
     def update_target(self) -> None:
         self.origin = self.odometer.prediction.point
@@ -135,24 +166,28 @@ async def _drive(self, distance: float) -> None:
             self._state = await self._run_row_completed()
 
     async def _run_approaching_row_start(self) -> State:
+        self.robot_on_field = False
         self.set_start_and_end_points()
+        if self.start_point is None or self.end_point is None:
+            return State.ERROR
+        if isinstance(self.detector, rosys.vision.DetectorSimulation) and not rosys.is_test:
+            self.create_simulation()
+        else:
+            self.plant_provider.clear()
+
         await self.gnss.ROBOT_POSE_LOCATED.emitted(self._max_gnss_waiting_time)
-        # turn towards row start
-        assert self.start_point is not None
-        target_yaw = self.odometer.prediction.direction(self.start_point)
-        await self.turn_in_steps(target_yaw)
-        # drive to row start
-        await self.drive_in_steps(Pose(x=self.start_point.x, y=self.start_point.y, yaw=target_yaw))
-        await self.gnss.ROBOT_POSE_LOCATED.emitted(self._max_gnss_waiting_time)
+        if not self.robot_on_field:
+            # turn towards row start
+            assert self.start_point is not None
+            target_yaw = self.odometer.prediction.direction(self.start_point)
+            await self.turn_in_steps(target_yaw)
+            # drive to row start
+            await self.drive_in_steps(Pose(x=self.start_point.x, y=self.start_point.y, yaw=target_yaw))
+            await self.gnss.ROBOT_POSE_LOCATED.emitted(self._max_gnss_waiting_time)
         # turn to row
         assert self.end_point is not None
         row_yaw = self.start_point.direction(self.end_point)
         await self.turn_in_steps(row_yaw)
-
-        if isinstance(self.detector, rosys.vision.DetectorSimulation) and not rosys.is_test:
-            self.create_simulation()
-        else:
-            self.plant_provider.clear()
         return State.FOLLOWING_ROW
 
     async def drive_in_steps(self, target: Pose) -> None: