check for big yaw diffences

zauberzeug · Sep 18, 2024 · 7823f10 · 7823f10
1 parent e62f377
commit 7823f10
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/field_friend/automations/navigation/follow_crops_navigation.py b/field_friend/automations/navigation/follow_crops_navigation.py
@@ -57,7 +57,9 @@ async def _drive(self, distance: float) -> None:
             # flip if it is pointing backwards
             if np.abs(yaw - self.odometer.prediction.yaw) > math.pi / 2:
                 yaw = yaw + math.pi
-
+            if np.abs(yaw - self.odometer.prediction.yaw) > 0.2:
+                self.log.error(f'Yaw difference is too high: {np.abs(yaw - self.odometer.prediction.yaw)}')
+                return
             fitted_line = rosys.geometry.Line(a=m, b=-1, c=c)
             closest_point = fitted_line.foot_point(self.odometer.prediction.point)
             target = rosys.geometry.Pose(x=closest_point.x, y=closest_point.y, yaw=yaw)