End navigation automatically after stopping at the destination for 15 seconds.

openpilot/starpilot/navigation/runtime.py

@@ -6,11 +6,14 @@ import math
 import time
 import uuid
 
-from openpilot.starpilot.navigation.owner import NavigationOwner, ValidationError, ROUTE_AVOIDANCES
+from openpilot.starpilot.navigation.owner import ConflictError, NavigationOwner, ValidationError, ROUTE_AVOIDANCES
 from openpilot.starpilot.navigation.route_engine import MapboxRouteEngine
 
 from openpilot.starpilot.gps.source import GPS_MAX_AGE_NS, GPS_SOURCES, bearing, select_location
 
+ARRIVED_NS = 5_000_000_000      # stopped at the destination this long shows "You have arrived"
+END_ROUTE_NS = 15_000_000_000   # ...and this long ends navigation, like tapping End navigation
+
 
 def location(sm, now_ns: int):
   fix = select_location(sm, now_ns)
@@ -39,6 +42,7 @@ class RouteRuntime:
     self.retry_after = 0
     self.off_route_since = 0
     self.arrived_since = 0
+    self.ended_revision = None
     self.session = uuid.uuid4().hex
 
   def update(self, now_ns: int, position, drive_id: int) -> dict:
@@ -122,12 +126,24 @@ class RouteRuntime:
       self.routes = []
       result['status'] = 'routing'
       return result
-    arrived = self.arrived_since and now_ns - self.arrived_since >= 5_000_000_000
+    arrived = self.arrived_since and now_ns - self.arrived_since >= ARRIVED_NS
+    if self.arrived_since and now_ns - self.arrived_since >= END_ROUTE_NS:
+      self._end_route(settings['revision'])
     result.update(status='arrived' if arrived else 'guiding', instruction=progress.instruction,
                   nextManeuver=progress.next_maneuver, route=self.route.preview(),
                   controlValid=bool(drive_id > 0 and not progress.off_route and not progress.arrived and stamp >= drive_id))
     return result
 
+  def _end_route(self, revision: str) -> None:
+    # Once per destination; a concurrent change (new destination, Galaxy edit) wins over arrival.
+    if self.ended_revision == revision:
+      return
+    self.ended_revision = revision
+    try:
+      self.owner.clear(revision, True)
+    except (ConflictError, ValidationError, OSError, PermissionError):
+      pass
+
 
 def publish_status(pm, value: dict) -> None:
   from openpilot.cereal import messaging

openpilot/starpilot/navigation/tests/test_route_runtime.py

@@ -63,6 +63,39 @@ def test_generation_discard_and_current_drive_control(tmp_path, route):
   assert result['status'] == 'routing' and not result['controlValid']
 
 
+def test_arrival_ends_navigation_after_a_short_stop(tmp_path, route):
+  owner, executor = configured(tmp_path), Executor()
+  runtime = RouteRuntime(owner, engine=SimpleNamespace(fetch=None), executor=executor)
+  def at(t, speed=0.):
+    return runtime.update(t * 1_000_000_000, (t * 1_000_000_000 - 1_000_000, (.01, 0.), speed, 90.), 1_000_000_000)
+  at(10, 5.)
+  executor.jobs[0].set_result(route)
+  assert at(11, 5.)['status'] == 'guiding'
+  assert at(12)['status'] == 'guiding'  # stopping starts the arrival clock
+  assert at(17)['status'] == 'arrived'
+  assert owner.read()['destination'] is not None
+  assert at(20, 5.)['status'] == 'guiding', 'driving off resets arrival'
+  at(21)
+  assert at(35)['status'] == 'arrived' and owner.read()['destination'] is not None
+  assert at(36)['status'] == 'arrived'
+  assert owner.read()['destination'] is None, 'navigation ends 15 s after stopping at the destination'
+  assert at(37)['status'] == 'noDestination'
+
+
+def test_arrival_does_not_override_a_newer_destination(tmp_path, route):
+  owner, executor = configured(tmp_path), Executor()
+  runtime = RouteRuntime(owner, engine=SimpleNamespace(fetch=None), executor=executor)
+  def at(t):
+    return runtime.update(t * 1_000_000_000, (t * 1_000_000_000 - 1_000_000, (.01, 0.), 0., 90.), 1_000_000_000)
+  at(10)
+  executor.jobs[0].set_result(route)
+  at(11)
+  stale = owner.read()['revision']
+  owner.select({'name': 'Elsewhere', 'longitude': .02, 'latitude': 0.}, stale, True)
+  runtime._end_route(stale)
+  assert owner.read()['destination']['name'] == 'Elsewhere'
+
+
 def test_route_failure_backoff_and_no_parallel_requests(tmp_path):
   executor = Executor()
   runtime = RouteRuntime(configured(tmp_path), engine=SimpleNamespace(fetch=None), executor=executor)