From 1bf2717626fc92ec937b8d5259df9e77c1ba01c8 Mon Sep 17 00:00:00 2001 From: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Date: Wed, 12 Aug 2026 22:19:42 -0400 Subject: [PATCH] Quick Nav --- selfdrive/ui/tests/test_home_info_card.py | 5 +- selfdrive/ui/widgets/home_info_card.py | 2 +- starpilot/navigation/destination_store.py | 58 +++++++++-- .../navigation/test_destination_store.py | 49 ++++++++++ system/manager/manager.py | 9 +- system/manager/test/test_manager.py | 98 +++++++++++++++++++ 6 files changed, 212 insertions(+), 9 deletions(-) diff --git a/selfdrive/ui/tests/test_home_info_card.py b/selfdrive/ui/tests/test_home_info_card.py index 767a1d12c..cb94c8ae2 100644 --- a/selfdrive/ui/tests/test_home_info_card.py +++ b/selfdrive/ui/tests/test_home_info_card.py @@ -7,6 +7,7 @@ from openpilot.starpilot.navigation.destination_store import ( FAVORITE_DESTINATIONS_KEY, NAVIGATION_DESTINATION_KEY, RECENT_DESTINATIONS_KEY, + START_ON_NEXT_DRIVE_KEY, same_destination, ) @@ -101,7 +102,9 @@ def test_page_switch_is_in_memory_and_destination_rows_select_once(): row = card._destination_rects[0] row_center = rl.Vector2(row.x + row.width / 2, row.y + row.height / 2) card._handle_mouse_release(row_center) - assert json.loads(params.values[NAVIGATION_DESTINATION_KEY])["name"] == "Home" + stored_destination = json.loads(params.values[NAVIGATION_DESTINATION_KEY]) + assert stored_destination["name"] == "Home" + assert stored_destination[START_ON_NEXT_DRIVE_KEY] is True assert params.values[RECENT_DESTINATIONS_KEY][0]["place_name"] == "Home" first_write_count = len(params.writes) assert card.active_destination["name"] == "Home" diff --git a/selfdrive/ui/widgets/home_info_card.py b/selfdrive/ui/widgets/home_info_card.py index 67498fce8..bdf958b21 100644 --- a/selfdrive/ui/widgets/home_info_card.py +++ b/selfdrive/ui/widgets/home_info_card.py @@ -129,7 +129,7 @@ class HomeInfoCard(Widget): return favorite = self._favorites[index] - destination = self._store.set_destination(favorite, skip_if_same=True) + destination = self._store.set_destination(favorite, skip_if_same=True, start_on_next_drive=True) if destination is not None: self._active_destination = destination diff --git a/starpilot/navigation/destination_store.py b/starpilot/navigation/destination_store.py index 0a3b9dc1b..9879b00ce 100644 --- a/starpilot/navigation/destination_store.py +++ b/starpilot/navigation/destination_store.py @@ -10,6 +10,8 @@ RECENT_DESTINATIONS_KEY = "ApiCache_NavDestinations" FAVORITE_DESTINATIONS_KEY = "FavoriteDestinations" NAV_INSTRUCTION_STATE_KEY = "NavInstructionState" NAV_INSTRUCTION_COLLAPSED_KEY = "NavInstructionCollapsed" +# Internal marker: preserve an offroad quick-start selection until the next onroad edge. +START_ON_NEXT_DRIVE_KEY = "start_on_next_drive" RECENT_DESTINATIONS_LIMIT = 10 @@ -81,6 +83,29 @@ def parse_destination_json(raw_value: str | bytes | dict[str, Any] | None) -> di return normalize_destination_payload(payload) +def _parse_next_drive_destination(raw_value: Any) -> dict[str, Any] | None: + payload = _json_value(raw_value, None) + if not isinstance(payload, dict) or payload.get(START_ON_NEXT_DRIVE_KEY) is not True: + return None + return normalize_destination_payload(payload) + + +def is_next_drive_destination(raw_value: Any) -> bool: + return _parse_next_drive_destination(raw_value) is not None + + +def activate_next_drive_destination(params: Any) -> dict[str, Any] | None: + """Promote an offroad quick-start destination to a normal active destination.""" + raw_value = _param_get(params, NAVIGATION_DESTINATION_KEY, "") + destination = _parse_next_drive_destination(raw_value) + payload = _json_value(raw_value, None) + if destination is not None and isinstance(payload, dict): + payload = dict(payload) + payload.pop(START_ON_NEXT_DRIVE_KEY, None) + params.put(NAVIGATION_DESTINATION_KEY, json.dumps(payload)) + return destination + + def _favorite_destination_id(payload: dict[str, Any]) -> str: """Keep the exact favorite ID formula used by Galaxy's existing backend.""" raw = f"{payload.get('longitude')},{payload.get('latitude')}|{payload.get('routeId') or ''}|{payload.get('name') or ''}" @@ -167,21 +192,31 @@ def routing_configured(params: Any) -> bool: return bool(_text(_param_get(params, "MapboxSecretKey", "")).strip()) -def set_navigation_destination(params: Any, payload: Any, *, skip_if_same: bool = False) -> dict[str, Any] | None: +def set_navigation_destination( + params: Any, + payload: Any, + *, + skip_if_same: bool = False, + start_on_next_drive: bool = False, +) -> dict[str, Any] | None: destination = normalize_destination_payload(payload) if destination is None: return None + current_raw = _param_get(params, NAVIGATION_DESTINATION_KEY, "") if skip_if_same: - current = parse_destination_json(_param_get(params, NAVIGATION_DESTINATION_KEY, "")) - if same_destination(current, destination): + current = parse_destination_json(current_raw) + if same_destination(current, destination) and is_next_drive_destination(current_raw) == start_on_next_drive: return destination raw_recent_destinations = _param_get(params, RECENT_DESTINATIONS_KEY, "[]") if isinstance(raw_recent_destinations, (list, dict)): raw_recent_destinations = json.dumps(raw_recent_destinations) recent_destinations = update_recent_destinations(raw_recent_destinations, destination) - params.put(NAVIGATION_DESTINATION_KEY, json.dumps(destination)) + stored_destination = dict(destination) + if start_on_next_drive: + stored_destination[START_ON_NEXT_DRIVE_KEY] = True + params.put(NAVIGATION_DESTINATION_KEY, json.dumps(stored_destination)) params.put(RECENT_DESTINATIONS_KEY, recent_destinations) return destination @@ -350,8 +385,19 @@ class NavigationDestinationStore: self.params.put(FAVORITE_DESTINATIONS_KEY, favorites) return favorites - def set_destination(self, payload: Any, *, skip_if_same: bool = False) -> dict[str, Any] | None: - return set_navigation_destination(self.params, payload, skip_if_same=skip_if_same) + def set_destination( + self, + payload: Any, + *, + skip_if_same: bool = False, + start_on_next_drive: bool = False, + ) -> dict[str, Any] | None: + return set_navigation_destination( + self.params, + payload, + skip_if_same=skip_if_same, + start_on_next_drive=start_on_next_drive, + ) def clear_navigation(self) -> bool: collapsed_supported = True diff --git a/starpilot/navigation/test_destination_store.py b/starpilot/navigation/test_destination_store.py index d358e9f89..af94951ce 100644 --- a/starpilot/navigation/test_destination_store.py +++ b/starpilot/navigation/test_destination_store.py @@ -4,8 +4,11 @@ from openpilot.starpilot.navigation.destination_store import ( FAVORITE_DESTINATIONS_KEY, NAVIGATION_DESTINATION_KEY, RECENT_DESTINATIONS_KEY, + START_ON_NEXT_DRIVE_KEY, NavigationDestinationStore, + activate_next_drive_destination, favorite_destination_id, + is_next_drive_destination, load_favorite_destinations, normalize_destination_payload, normalize_favorite_destination, @@ -155,6 +158,27 @@ def test_destination_write_updates_active_destination_and_recents(): assert params.values[RECENT_DESTINATIONS_KEY][0]["place_name"] == "Home" +def test_next_drive_destination_is_marked_then_canonicalized_on_activation(): + params = FakeParams({RECENT_DESTINATIONS_KEY: "[]"}) + + destination = set_navigation_destination( + params, + {"name": "Home", "latitude": 1, "longitude": 2}, + start_on_next_drive=True, + ) + + stored = json.loads(params.values[NAVIGATION_DESTINATION_KEY]) + assert stored[START_ON_NEXT_DRIVE_KEY] is True + assert is_next_drive_destination(stored) + + stored["routeId"] = "main" + params.values[NAVIGATION_DESTINATION_KEY] = json.dumps(stored) + assert activate_next_drive_destination(params) == destination + promoted = json.loads(params.values[NAVIGATION_DESTINATION_KEY]) + assert promoted == {**destination, "routeId": "main"} + assert not is_next_drive_destination(params.values[NAVIGATION_DESTINATION_KEY]) + + def test_same_destination_write_is_idempotent_and_does_not_touch_recents(): params = FakeParams({ NAVIGATION_DESTINATION_KEY: json.dumps({"name": "Home", "latitude": 1, "longitude": 2}), @@ -176,6 +200,31 @@ def test_same_destination_write_is_idempotent_and_does_not_touch_recents(): assert [key for key, _value in settings_params.writes] == [NAVIGATION_DESTINATION_KEY, RECENT_DESTINATIONS_KEY] +def test_same_destination_is_rewritten_when_next_drive_intent_changes(): + params = FakeParams({ + NAVIGATION_DESTINATION_KEY: json.dumps({"name": "Home", "latitude": 1, "longitude": 2}), + RECENT_DESTINATIONS_KEY: [], + }) + + set_navigation_destination( + params, + {"name": "Home", "latitude": 1, "longitude": 2}, + skip_if_same=True, + start_on_next_drive=True, + ) + + assert is_next_drive_destination(params.values[NAVIGATION_DESTINATION_KEY]) + first_write_count = len(params.writes) + + set_navigation_destination( + params, + {"name": "Home", "latitude": 1, "longitude": 2}, + skip_if_same=True, + start_on_next_drive=True, + ) + assert len(params.writes) == first_write_count + + def test_navigation_destination_store_keeps_settings_favorite_migration_and_mutations(): params = FakeParams({ FAVORITE_DESTINATIONS_KEY: json.dumps([{"name": "Home", "latitude": 1, "longitude": 2}]), diff --git a/system/manager/manager.py b/system/manager/manager.py index c9859ae17..0a8bb889e 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -45,6 +45,7 @@ from openpilot.starpilot.common.starpilot_variables import ( LEGACY_STARPILOT_STATS_KEY_RENAMES, get_starpilot_toggles, ) +from openpilot.starpilot.navigation.destination_store import activate_next_drive_destination, is_next_drive_destination _MANAGER_IMPORT_DONE = time.monotonic() _manager_import_timing_line = ( @@ -135,7 +136,7 @@ def get_nav_offroad_clear_timeout_seconds(params) -> int: def update_nav_offroad_clear_state(params, started: bool, tracked_destination, tracked_started_at, now: float): nav_destination = params.get("NavDestination") - if started or not params.get_bool("ClearNavOnOffroad") or not nav_destination: + if started or not params.get_bool("ClearNavOnOffroad") or not nav_destination or is_next_drive_destination(nav_destination): return None, None if nav_destination != tracked_destination or tracked_started_at is None: @@ -143,6 +144,9 @@ def update_nav_offroad_clear_state(params, started: bool, tracked_destination, t tracked_started_at = now if now - tracked_started_at >= get_nav_offroad_clear_timeout_seconds(params): + current_destination = params.get("NavDestination") + if current_destination != nav_destination or is_next_drive_destination(current_destination): + return None, None params.remove("NavDestination") return None, None @@ -1152,6 +1156,9 @@ def manager_thread() -> None: # StarPilot variables params_memory.clear_all(ParamKeyFlag.CLEAR_ON_OFFROAD_TRANSITION) + if started and not started_prev: + activate_next_drive_destination(params) + offroad_nav_destination, offroad_nav_started_at = update_nav_offroad_clear_state( params, started, offroad_nav_destination, offroad_nav_started_at, time.monotonic() ) diff --git a/system/manager/test/test_manager.py b/system/manager/test/test_manager.py index ac4647600..54894dc1a 100644 --- a/system/manager/test/test_manager.py +++ b/system/manager/test/test_manager.py @@ -12,6 +12,7 @@ import openpilot.system.manager.manager as manager from openpilot.system.manager.process import ensure_running from openpilot.system.manager.process_config import BigDeviceUIProcess, managed_processes, procs from openpilot.system.hardware import HARDWARE +from openpilot.starpilot.navigation.destination_store import START_ON_NEXT_DRIVE_KEY os.environ['FAKEUPLOAD'] = "1" @@ -71,6 +72,103 @@ class FileBackedFakeParams: def put_float(self, key, value): self.put(key, float(value)) + def remove(self, key): + Path(self.get_param_path(key)).unlink(missing_ok=True) + + +def test_offroad_navigation_cleanup_preserves_next_drive_destination(tmp_path): + destination = { + "name": "Home", + "place_name": "Home", + "latitude": 1.0, + "longitude": 2.0, + START_ON_NEXT_DRIVE_KEY: True, + } + params = FileBackedFakeParams(tmp_path / "params", { + "ClearNavOnOffroad": True, + "ClearNavOnOffroadTimeoutMinutes": 0, + "NavDestination": destination, + }) + + state = manager.update_nav_offroad_clear_state(params, False, None, None, 10.0) + + assert state == (None, None) + assert json.loads(params.get("NavDestination")) == destination + + +def test_next_drive_selection_disarms_delayed_cleanup_for_same_destination(tmp_path): + active_destination = { + "name": "Home", + "place_name": "Home", + "latitude": 1.0, + "longitude": 2.0, + } + params = FileBackedFakeParams(tmp_path / "params", { + "ClearNavOnOffroad": True, + "ClearNavOnOffroadTimeoutMinutes": 15, + "NavDestination": active_destination, + }) + + tracked = manager.update_nav_offroad_clear_state(params, False, None, None, 10.0) + assert tracked == (params.get("NavDestination"), 10.0) + + params.put("NavDestination", {**active_destination, START_ON_NEXT_DRIVE_KEY: True}) + state = manager.update_nav_offroad_clear_state(params, False, *tracked, 20.0) + + assert state == (None, None) + assert json.loads(params.get("NavDestination"))[START_ON_NEXT_DRIVE_KEY] is True + + +def test_unmarked_navigation_destination_still_clears_immediately_offroad(tmp_path): + params = FileBackedFakeParams(tmp_path / "params", { + "ClearNavOnOffroad": True, + "ClearNavOnOffroadTimeoutMinutes": 0, + "NavDestination": {"name": "Home", "latitude": 1.0, "longitude": 2.0}, + }) + + state = manager.update_nav_offroad_clear_state(params, False, None, None, 10.0) + + assert state == (None, None) + assert params.get("NavDestination") is None + + +def test_offroad_cleanup_does_not_remove_destination_replaced_after_snapshot(tmp_path): + old_destination = json.dumps({"name": "Old", "latitude": 1.0, "longitude": 2.0}) + next_drive_destination = { + "name": "Home", + "place_name": "Home", + "latitude": 3.0, + "longitude": 4.0, + START_ON_NEXT_DRIVE_KEY: True, + } + + class SnapshotRaceParams(FileBackedFakeParams): + def __init__(self, root, values): + self._first_nav_read = old_destination + self.removed = [] + super().__init__(root, values) + + def get(self, key): + if key == "NavDestination" and self._first_nav_read is not None: + value, self._first_nav_read = self._first_nav_read, None + return value + return super().get(key) + + def remove(self, key): + self.removed.append(key) + super().remove(key) + + params = SnapshotRaceParams(tmp_path / "params", { + "ClearNavOnOffroad": True, + "ClearNavOnOffroadTimeoutMinutes": 0, + "NavDestination": next_drive_destination, + }) + + state = manager.update_nav_offroad_clear_state(params, False, None, None, 10.0) + + assert state == (None, None) + assert "NavDestination" not in params.removed + class FakeManagedProcess: def __init__(self):