Driving timeout

field_friend/automations/automation_watcher.py

@@ -130,7 +130,7 @@ async def ensure_robot_pose_updates_when_not_in_automation(self) -> None:
             self.log.warning('GNSS is paused, this should not happen outside of an automation')
             self.gnss.is_paused = False
             return
-        if self.last_robot_pose.distance(self.odometer.prediction) == 0:  # if robot stands still
+        if self.last_robot_pose == self.odometer.prediction:
             await self.gnss.update_robot_pose()
         else:
             self.gnss.observed_poses.clear()

field_friend/automations/navigation/navigation.py

@@ -86,16 +86,17 @@ async def finish(self) -> None:
     async def _drive(self, distance: float) -> None:
         """Drives the vehicle a short distance forward"""
 
-    async def _drive_to_yaw(self, distance: float, yaw: float):
-        deadline = rosys.time() + 2
+    async def _drive_to_yaw(self, distance: float, yaw: float, timeout: float = 2.0) -> None:
+        deadline = rosys.time() + timeout
         start_position = self.odometer.prediction.point
         yaw = angle(self.odometer.prediction.yaw, yaw)  # take current yaw into account and only steer the difference
         with self.driver.parameters.set(linear_speed_limit=self.linear_speed_limit, angular_speed_limit=self.angular_speed_limit):
             await self.driver.wheels.drive(*self.driver._throttle(1, yaw))  # pylint: disable=protected-access
         try:
             while self.odometer.prediction.point.distance(start_position) < distance:
                 if rosys.time() >= deadline:
-                    raise TimeoutError('Driving Timeout')
+                    raise TimeoutError(
+                        f'Driving Timeout at startpoint: {start_position} with yaw: {yaw} and target point: {self.odometer.prediction.point}')
                 await rosys.sleep(0.01)
         finally:
             await self.driver.wheels.stop()

field_friend/automations/navigation/straight_line_navigation.py

@@ -1,13 +1,11 @@
-import asyncio
 from random import randint
 from typing import TYPE_CHECKING, Any
 
 import numpy as np
 import rosys
 from nicegui import ui
 
-from field_friend.automations.implements.implement import Implement
-
+from ...automations.implements.implement import Implement
 from .navigation import Navigation
 
 if TYPE_CHECKING:

tests/test_navigation.py

@@ -24,6 +24,24 @@ async def test_straight_line(system: System):
     assert system.odometer.prediction.point.x == pytest.approx(system.straight_line_navigation.length, abs=0.1)
 
 
+async def test_straight_line_with_high_angles(system: System):
+    assert isinstance(system.field_friend.wheels, rosys.hardware.WheelsSimulation)
+    predicted_yaw = 190
+    start_yaw = system.odometer.prediction.yaw
+    target_yaw = start_yaw + np.deg2rad(predicted_yaw)
+    await system.driver.wheels.drive(*system.driver._throttle(0, 0.1))  # pylint: disable=protected-access
+    await forward(until=lambda: abs(rosys.helpers.angle(system.odometer.prediction.yaw, target_yaw)) < np.deg2rad(0.01))
+    await system.driver.wheels.stop()
+    assert isinstance(system.current_navigation, StraightLineNavigation)
+    system.current_navigation.length = 1.0
+    system.automator.start()
+    await forward(until=lambda: system.automator.is_running)
+    await forward(until=lambda: system.automator.is_stopped)
+    assert system.odometer.prediction.point.x == pytest.approx(-0.985, abs=0.1)
+    assert system.odometer.prediction.point.y == pytest.approx(-0.174, abs=0.1)
+    assert system.odometer.prediction.yaw_deg == pytest.approx(predicted_yaw, abs=5)
+
+
 async def test_straight_line_with_failing_gnss(system: System, gnss: GnssSimulation, detector: rosys.vision.DetectorSimulation):
     async def empty():
         return None