From 3bbb33f6bd985239600ded2564f87ecb16adcaa6 Mon Sep 17 00:00:00 2001 From: James Vecellio Date: Sat, 18 Oct 2025 18:15:22 -0700 Subject: [PATCH] feat: navigationd I changed the reroute counter to 9 updates, which is every 3 seconds. compared to 3, which is one second. --- cereal/custom.capnp | 21 +++- cereal/log.capnp | 2 +- cereal/services.py | 1 + common/params_keys.h | 1 + sunnypilot/navd/README.md | 1 + sunnypilot/navd/navigationd.py | 140 ++++++++++++++++++++++ sunnypilot/navd/tests/__init__.py | 0 sunnypilot/navd/tests/test_navigationd.py | 58 +++++++++ system/manager/process_config.py | 3 + 9 files changed, 225 insertions(+), 2 deletions(-) create mode 100755 sunnypilot/navd/navigationd.py create mode 100644 sunnypilot/navd/tests/__init__.py create mode 100644 sunnypilot/navd/tests/test_navigationd.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 5c0a004fa6..794ce58dc2 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -454,7 +454,26 @@ struct ModelDataV2SP @0xa1680744031fdb2d { } } -struct CustomReserved10 @0xcb9fd56c7057593a { +struct Navigationd @0xcb9fd56c7057593a { + timestamp @0 :UInt64; + upcomingTurn @1 :Text; + currentSpeedLimit @2 :UInt64; + bannerInstructions @3 :Text; + distanceToNextTurn @4 :Float64; + routeProgressPercent @5 :Float64; + distanceFromRoute @6 :Float64; + routePositionCumulative @7 :Float64; + distanceToEndOfStep @8 :Float64; + totalDistanceRemaining @9 :Float64; + totalTimeRemaining @10 :Float64; + allManeuvers @11 :List(Maneuver); + valid @12 :Bool; + + struct Maneuver { + distance @0 :Float64; + type @1 :Text; + modifier @2 :Text; + } } struct CustomReserved11 @0xc2243c65e0340384 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 8925bca8ae..e07ee31f8f 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2632,7 +2632,7 @@ struct Event { carStateSP @114 :Custom.CarStateSP; liveMapDataSP @115 :Custom.LiveMapDataSP; modelDataV2SP @116 :Custom.ModelDataV2SP; - customReserved10 @136 :Custom.CustomReserved10; + navigationd @136 :Custom.Navigationd; customReserved11 @137 :Custom.CustomReserved11; customReserved12 @138 :Custom.CustomReserved12; customReserved13 @139 :Custom.CustomReserved13; diff --git a/cereal/services.py b/cereal/services.py index 67548bc79e..b6871f0e0f 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -89,6 +89,7 @@ _services: dict[str, tuple] = { "carStateSP": (True, 100., 10), "liveMapDataSP": (True, 1., 1), "modelDataV2SP": (True, 20.), + "navigationd": (True, 3.), "liveLocationKalman": (True, 20.), # debug diff --git a/common/params_keys.h b/common/params_keys.h index ec749648c3..df5e1b6440 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -190,6 +190,7 @@ inline static std::unordered_map keys = { {"MapboxToken", {PERSISTENT | BACKUP, STRING}}, {"MapboxSettings", {CLEAR_ON_MANAGER_START, JSON}}, {"MapboxRoute", {CLEAR_ON_MANAGER_START, STRING}}, + {"MapboxRecompute", {PERSISTENT | BACKUP, BOOL, "0"}}, // Neural Network Lateral Control {"NeuralNetworkLateralControl", {PERSISTENT | BACKUP, BOOL, "0"}}, diff --git a/sunnypilot/navd/README.md b/sunnypilot/navd/README.md index 9cedf83345..73fbb0eb66 100644 --- a/sunnypilot/navd/README.md +++ b/sunnypilot/navd/README.md @@ -3,3 +3,4 @@ Navigation daemon with Mapbox integration for semi-offline navigation. This module handles route planning, geocoding, and turn-by-turn instructions to support autonomous driving features. - `navigation_helpers/`: Mapbox API integration and navigation instructions processing. +- `navigationd`: Navigation service which uses mapbox integration to generate a route and keep it up to date. This service runs at three hz, using keep time to ensure the while loop only updates three times a second rather than every time sm updates, which in this case would be twenty hz (LLK). \ No newline at end of file diff --git a/sunnypilot/navd/navigationd.py b/sunnypilot/navd/navigationd.py new file mode 100755 index 0000000000..f9b3f89a19 --- /dev/null +++ b/sunnypilot/navd/navigationd.py @@ -0,0 +1,140 @@ +import math +import time + +import cereal.messaging as messaging +from cereal import custom +from openpilot.common.params import Params +from openpilot.common.realtime import Ratekeeper +from openpilot.common.swaglog import cloudlog + +from openpilot.sunnypilot.navd.helpers import Coordinate, parse_banner_instructions +from openpilot.sunnypilot.navd.navigation_helpers.mapbox_integration import MapboxIntegration +from openpilot.sunnypilot.navd.navigation_helpers.nav_instructions import NavigationInstructions + + +class Navigationd: + def __init__(self): + self.params = Params() + self.mapbox = MapboxIntegration() + self.nav_instructions = NavigationInstructions() + + self.sm = messaging.SubMaster(['liveLocationKalman']) + self.pm = messaging.PubMaster(['navigationd']) + self.rk = Ratekeeper(3) # 3 Hz + + self.route = None + self.destination: str | None = None + self.new_destination: str = '' + + self.recompute_allowed: bool = False + self.allow_recompute: bool = False + self.reroute_counter: int = 0 + + self.frame: int = -1 + self.last_position: Coordinate | None = None + self.last_bearing: float | None = None + self.is_metric: bool = False + self.valid: bool = False + + def _update_params(self): + if self.last_position is not None: + self.frame += 1 + if self.frame % 9 == 0: + self.is_metric = self.params.get('IsMetric', return_default=True) + self.new_destination = self.params.get('MapboxRoute') + self.recompute_allowed = self.params.get('MapboxRecompute', return_default=True) + + self.allow_recompute: bool = (self.new_destination != self.destination and self.new_destination != '') or ( + self.recompute_allowed and self.reroute_counter > 9 and self.route + ) + + if self.allow_recompute: + postvars = {'place_name': self.new_destination} + postvars, valid_addr = self.mapbox.set_destination(postvars, self.last_position.longitude, self.last_position.latitude, self.last_bearing) + cloudlog.debug(f'Set new destination to: {self.new_destination}, valid: {valid_addr}') + if valid_addr: + self.destination = self.new_destination + self.nav_instructions.clear_route_cache() + self.route = self.nav_instructions.get_current_route() + self.reroute_counter = 0 + + self.valid = self.route is not None + + def _update_navigation(self) -> tuple[str, dict | None, dict]: + banner_instructions: str = '' + progress: dict | None = None + nav_data: dict = {} + if self.last_position is not None: + if progress := self.nav_instructions.get_route_progress(self.last_position.latitude, self.last_position.longitude): + nav_data['upcoming_turn'] = self.nav_instructions.get_upcoming_turn_from_progress(progress, self.last_position.latitude, self.last_position.longitude) + nav_data['current_speed_limit'] = self.nav_instructions.get_current_speed_limit_from_progress(progress, self.is_metric) + + if progress['current_step']: + parsed = parse_banner_instructions(progress['current_step']['bannerInstructions'], progress['distance_to_end_of_step']) + if parsed: + banner_instructions = parsed['maneuverPrimaryText'] + + nav_data['distance_to_next_turn'] = progress['distance_to_next_turn'] + nav_data['distance_to_end_of_step'] = progress['distance_to_end_of_step'] + nav_data['route_progress_percent'] = progress['route_progress_percent'] + nav_data['distance_from_route'] = progress['distance_from_route'] + nav_data['route_position_cumulative'] = progress['route_position_cumulative'] + + # Don't recompute in last segment to prevent reroute loops + if self.route: + if progress['current_step_idx'] == len(self.route['steps']) - 1: + self.allow_recompute = False + + if self.recompute_allowed: + self.reroute_counter += 1 if nav_data['distance_from_route'] > 25 else 0 + + return banner_instructions, progress, nav_data + + def _build_navigation_message(self, banner_instructions, progress, nav_data): + msg = custom.Navigationd.new_message() + msg.timestamp = int(time.monotonic() * 1000) + msg.upcomingTurn = nav_data.get('upcoming_turn', 'none') + msg.currentSpeedLimit = nav_data.get('current_speed_limit', 0) + msg.bannerInstructions = banner_instructions + msg.distanceToNextTurn = nav_data.get('distance_to_next_turn', 0.0) + msg.distanceToEndOfStep = nav_data.get('distance_to_end_of_step', 0.0) + msg.routeProgressPercent = nav_data.get('route_progress_percent', 0.0) + msg.distanceFromRoute = nav_data.get('distance_from_route', 0.0) + msg.routePositionCumulative = nav_data.get('route_position_cumulative', 0.0) + msg.totalDistanceRemaining = progress['total_distance_remaining'] if progress else 0.0 + msg.totalTimeRemaining = progress['total_time_remaining'] if progress else 0.0 + msg.valid = self.valid + + all_maneuvers = ( + [custom.Navigationd.Maneuver.new_message(distance=m['distance'], type=m['type'], modifier=m['modifier']) for m in progress['all_maneuvers']] + if progress + else [] + ) + msg.allManeuvers = all_maneuvers + + return msg + + def run(self): + cloudlog.warning('navigationd init') + + while True: + self.sm.update() + location = self.sm['liveLocationKalman'] + localizer_valid = location.positionGeodetic.valid if location else False + + if localizer_valid: + self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2]) + self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1]) + + self._update_params() + banner_instructions, progress, nav_data = self._update_navigation() + + msg = self._build_navigation_message(banner_instructions, progress, nav_data) + + self.pm.send('navigationd', msg) + self.rk.keep_time() + + +def main(): + nav = Navigationd() + nav.run() diff --git a/sunnypilot/navd/tests/__init__.py b/sunnypilot/navd/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/navd/tests/test_navigationd.py b/sunnypilot/navd/tests/test_navigationd.py new file mode 100644 index 0000000000..2d3e1c715c --- /dev/null +++ b/sunnypilot/navd/tests/test_navigationd.py @@ -0,0 +1,58 @@ +import cereal.messaging as messaging + +from sunnypilot.navd.navigationd import Navigationd +from openpilot.sunnypilot.navd.helpers import Coordinate + + +def test_init(): + nav = Navigationd() + assert nav.sm is not None + assert nav.pm is not None + assert nav.rk is not None + assert nav.route is None + assert nav.destination is None + assert nav.new_destination == '' + assert nav.frame == -1 + assert nav.last_position is None + assert not nav.is_metric + assert not nav.valid + +def test_update_params(): + nav = Navigationd() + nav.last_position = None + nav._update_params() + assert nav.frame == -1 + nav.last_position = Coordinate(latitude=37.0, longitude=128.0) + nav._update_params() + assert nav.frame == 0 # frame only updates when last position is set + +def test_update_navigation_no_position(): + nav = Navigationd() + nav.last_position = None + banner, progress, nav_data = nav._update_navigation() + assert banner == '' + assert progress is None + assert nav_data == {} + +def test_update_navigation(): + nav = Navigationd() + nav.last_position = Coordinate(latitude=37.0, longitude=128.0) + nav.route = {'580 Winchester dr, oxnard, CA': True} + banner, progress, nav_data = nav._update_navigation() + assert isinstance(banner, str) + assert not progress # no route was actually set + assert isinstance(nav_data, dict) + +def test_build_navigation_message(): + sm = messaging.SubMaster(['navigationd']) + nav = Navigationd() + msg = nav._build_navigation_message('', None, {}) + + nav.pm.send('navigationd', msg) + sm.update() + received_msg = sm['navigationd'] + + assert received_msg.bannerInstructions == msg.bannerInstructions + assert received_msg.totalDistanceRemaining == msg.totalDistanceRemaining + assert received_msg.totalTimeRemaining == msg.totalTimeRemaining + assert received_msg.valid == msg.valid diff --git a/system/manager/process_config.py b/system/manager/process_config.py index c5f20e511b..607de3eae7 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -180,6 +180,9 @@ procs += [ NativeProcess("mapd", Paths.mapd_root(), ["bash", "-c", f"{MAPD_PATH} > /dev/null 2>&1"], mapd_ready), PythonProcess("mapd_manager", "sunnypilot.mapd.mapd_manager", always_run), + # navigationd + PythonProcess("navigationd", "sunnypilot.navd.navigationd", only_onroad), + # locationd NativeProcess("locationd_llk", "sunnypilot/selfdrive/locationd", ["./locationd"], only_onroad), ]