diff --git a/starpilot/navigation/navigationd.py b/starpilot/navigation/navigationd.py index 5ed6db3fa8..7cf0c89fd2 100644 --- a/starpilot/navigation/navigationd.py +++ b/starpilot/navigation/navigationd.py @@ -218,15 +218,17 @@ class Navigationd: if self._timer_expired(self._arrival_started_at, ARRIVAL_CLEAR_SECONDS, now): self._clear_route(remove_destination=True) - def _publish_nav_instruction(self, route: NavigationRoute | None, progress: RouteProgress | None, location_valid: bool) -> None: + def _publish_nav_instruction( + self, + route: NavigationRoute | None, + progress: RouteProgress | None, + location_valid: bool, + payload: dict[str, object] | None = None, + ) -> None: msg = messaging.new_message("navInstruction") msg.valid = bool(route is not None and progress is not None and location_valid) - if msg.valid and route is not None and progress is not None and self._last_position is not None: - payload = route.build_instruction_payload( - progress, - use_vienna_sign=self.params.get_bool("UseVienna"), - ) + if msg.valid and route is not None and progress is not None and self._last_position is not None and payload is not None: nav_instruction = msg.navInstruction nav_instruction.maneuverPrimaryText = payload["maneuverPrimaryText"] nav_instruction.maneuverSecondaryText = payload["maneuverSecondaryText"] @@ -244,14 +246,22 @@ class Navigationd: self.pm.send("navInstruction", msg) - def _publish_nav_state(self, route: NavigationRoute | None, progress: RouteProgress | None, location_valid: bool) -> None: + def _publish_nav_state( + self, + route: NavigationRoute | None, + progress: RouteProgress | None, + location_valid: bool, + payload: dict[str, object] | None = None, + ) -> None: if route is None or progress is None or not location_valid: if self._last_nav_state is not None: self.params_memory.remove("NavInstructionState") self._last_nav_state = None return - payload = route.build_instruction_payload(progress, use_vienna_sign=self.params.get_bool("UseVienna")) + if payload is None: + return + all_maneuvers = payload.get("allManeuvers") or [] next_maneuver = all_maneuvers[1] if len(all_maneuvers) > 1 and isinstance(all_maneuvers[1], dict) else {} active_lane_direction = "" @@ -342,8 +352,15 @@ class Navigationd: if route is None: progress = None - self._publish_nav_instruction(route, progress, location_valid) - self._publish_nav_state(route, progress, location_valid) + payload = None + if route is not None and progress is not None and location_valid: + payload = route.build_instruction_payload( + progress, + use_vienna_sign=self.params.get_bool("UseVienna"), + ) + + self._publish_nav_instruction(route, progress, location_valid, payload) + self._publish_nav_state(route, progress, location_valid, payload) self._publish_nav_route_if_needed() self.rk.keep_time() diff --git a/starpilot/navigation/test_navigationd.py b/starpilot/navigation/test_navigationd.py new file mode 100644 index 0000000000..21e55b5493 --- /dev/null +++ b/starpilot/navigation/test_navigationd.py @@ -0,0 +1,72 @@ +import pytest + +from openpilot.starpilot.navigation.navigationd import Navigationd + + +class CountingParams: + def __init__(self): + self.use_vienna_reads = 0 + + def get(self, key: str, *, encoding: str): + assert key == "NavDestination" + return None + + def get_bool(self, key: str) -> bool: + assert key == "UseVienna" + self.use_vienna_reads += 1 + return True + + +class CountingRoute: + def __init__(self): + self.payload_builds = 0 + + def build_instruction_payload(self, progress, *, use_vienna_sign: bool): + assert progress == "progress" + assert use_vienna_sign + self.payload_builds += 1 + return {"payload": self.payload_builds} + + +class StopLoop(Exception): + pass + + +class Recorder: + def __init__(self, return_value=None): + self.calls = [] + self.return_value = return_value + + def __call__(self, *args): + self.calls.append(args) + return self.return_value + + +class Ratekeeper: + def keep_time(self): + raise StopLoop + + +def test_run_builds_one_payload_for_both_navigation_publishers(): + navigationd = Navigationd.__new__(Navigationd) + route = CountingRoute() + params = CountingParams() + navigationd.params = params + navigationd.rk = Ratekeeper() + navigationd._update_location = Recorder((True, 0.0)) + navigationd._maybe_update_route = Recorder((route, None, 0)) + navigationd._build_progress = Recorder(("progress", None)) + navigationd._maybe_recompute = Recorder() + navigationd._snapshot_route = Recorder((route, None, 0)) + navigationd._publish_nav_instruction = Recorder() + navigationd._publish_nav_state = Recorder() + navigationd._publish_nav_route_if_needed = Recorder() + + with pytest.raises(StopLoop): + navigationd.run() + + assert route.payload_builds == 1 + assert params.use_vienna_reads == 1 + instruction_payload = navigationd._publish_nav_instruction.calls[0][3] + state_payload = navigationd._publish_nav_state.calls[0][3] + assert instruction_payload is state_payload