From 918dfbf1e06d4fe19ef99d15350ac941bff5bae5 Mon Sep 17 00:00:00 2001
From: LukasBaecker <65940705+LukasBaecker@users.noreply.github.com>
Date: Tue, 30 Jul 2024 17:12:50 +0200
Subject: [PATCH] Driving timeout (#128)

* logging and quick-fix for U5

* added logs for monitoring, changed timeout value

* deleting log

* writing test for yaw_calculation greater than 180deg

* deleting fixme comment

* Correction of incorrect test renaming

* faulty tagging

* deleting unnecessary logging

* cleanup

* revert timeout to 2 seconds and use as parameter

* raise driving timeout again but keep the log as the error message

* wip: find turning bug

* Revert "wip: find turning bug"

This reverts commit 45dbff62e5118da6ccb1b6c8780864a29225bf23.

* fix gnss pose update guard

---------

Co-authored-by: Pascal Schade <pascal@zauberzeug.com>
Co-authored-by: Rodja Trappe <rodja@zauberzeug.com>
---
 field_friend/automations/automation_watcher.py |  2 +-
 .../automations/navigation/navigation.py       |  7 ++++---
 .../navigation/straight_line_navigation.py     |  4 +---
 tests/test_navigation.py                       | 18 ++++++++++++++++++
 4 files changed, 24 insertions(+), 7 deletions(-)

diff --git a/field_friend/automations/automation_watcher.py b/field_friend/automations/automation_watcher.py
index 9fcd1663..65af6607 100644
--- a/field_friend/automations/automation_watcher.py
+++ b/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()
diff --git a/field_friend/automations/navigation/navigation.py b/field_friend/automations/navigation/navigation.py
index a8cfc4de..f00d34de 100644
--- a/field_friend/automations/navigation/navigation.py
+++ b/field_friend/automations/navigation/navigation.py
@@ -86,8 +86,8 @@ 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):
@@ -95,7 +95,8 @@ async def _drive_to_yaw(self, distance: float, yaw: float):
         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()
diff --git a/field_friend/automations/navigation/straight_line_navigation.py b/field_friend/automations/navigation/straight_line_navigation.py
index ec79aded..20939e29 100644
--- a/field_friend/automations/navigation/straight_line_navigation.py
+++ b/field_friend/automations/navigation/straight_line_navigation.py
@@ -1,4 +1,3 @@
-import asyncio
 from random import randint
 from typing import TYPE_CHECKING, Any
 
@@ -6,8 +5,7 @@
 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:
diff --git a/tests/test_navigation.py b/tests/test_navigation.py
index 8df5dd63..85409b69 100644
--- a/tests/test_navigation.py
+++ b/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