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

@@ -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 {

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;

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

common/params_keys.h

@@ -190,6 +190,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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"}},

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).

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()

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

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),
 ]