Merge branch 'navigationd-service' into nav-desires

This reverts commit b69da9e5ea.

.github/workflows/tests.yaml

@@ -21,11 +21,12 @@ env:
   PYTHONWARNINGS: error
   BASE_IMAGE: sunnypilot-base
   AZURE_TOKEN: ${{ secrets.AZURE_COMMADATACI_OPENPILOTCI_TOKEN }}
+  MAPBOX_TOKEN_CI: ${{ secrets.MAPBOX_TOKEN_CI }}
 
   DOCKER_LOGIN: docker login ghcr.io -u ${{ github.actor }} -p ${{ secrets.GITHUB_TOKEN }}
   BUILD: release/ci/docker_build_sp.sh base
 
-  RUN: docker run --shm-size 2G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e CI=1 -e PYTHONWARNINGS=error -e FILEREADER_CACHE=1 -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v $GITHUB_WORKSPACE/.ci_cache/scons_cache:/tmp/scons_cache -v $GITHUB_WORKSPACE/.ci_cache/comma_download_cache:/tmp/comma_download_cache -v $GITHUB_WORKSPACE/.ci_cache/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/bash -c
+  RUN: docker run --shm-size 2G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e CI=1 -e PYTHONWARNINGS=error -e FILEREADER_CACHE=1 -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -e MAPBOX_TOKEN_CI=$MAPBOX_TOKEN_CI -v $GITHUB_WORKSPACE/.ci_cache/scons_cache:/tmp/scons_cache -v $GITHUB_WORKSPACE/.ci_cache/comma_download_cache:/tmp/comma_download_cache -v $GITHUB_WORKSPACE/.ci_cache/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/bash -c
 
   PYTEST: pytest --continue-on-collection-errors --durations=0 -n logical
 

cereal/custom.capnp

@@ -369,6 +369,7 @@ struct CarControlSP @0xa5cd762cd951a455 {
   leadOne @2 :LeadData;
   leadTwo @3 :LeadData;
   intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
+  speed @5 :Float32;
 
   struct Param {
     key @0 :Text;
@@ -454,7 +455,20 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
   }
 }
 
-struct CustomReserved10 @0xcb9fd56c7057593a {
+struct Navigationd @0xcb9fd56c7057593a {
+  upcomingTurn @0 :Text;
+  currentSpeedLimit @1 :UInt16;
+  bannerInstructions @2 :Text;
+  distanceFromRoute @3 :Float32;
+  allManeuvers @4 :List(Maneuver);
+  valid @5 :Bool;
+
+  struct Maneuver {
+    distance @0 :Float32;
+    type @1 :Text;
+    modifier @2 :Text;
+    instruction @3 :Text;
+  }
 }
 
 struct CustomReserved11 @0xc2243c65e0340384 {

cereal/log.capnp

@@ -2635,7 +2635,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

@@ -189,6 +189,14 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"ModelManager_LastSyncTime", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}},
     {"ModelManager_ModelsCache", {PERSISTENT | BACKUP, JSON}},
 
+    // Navigation params
+    {"AllowNavigation", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"MapboxToken", {PERSISTENT | BACKUP, STRING}},
+    {"MapboxSettings", {CLEAR_ON_MANAGER_START, JSON}},
+    {"MapboxRoute", {PERSISTENT, STRING}},
+    {"MapboxRecompute", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"NavDesiresAllowed", {PERSISTENT | BACKUP, BOOL, "0"}},
+
     // Neural Network Lateral Control
     {"NeuralNetworkLateralControl", {PERSISTENT | BACKUP, BOOL, "0"}},
 

selfdrive/controls/lib/desire_helper.py

@@ -3,6 +3,7 @@ from openpilot.common.constants import CV
 from openpilot.common.realtime import DT_MDL
 from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode
 from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController
+from openpilot.sunnypilot.navd.navigation_desires.navigation_desires import NavigationDesires
 
 LaneChangeState = log.LaneChangeState
 LaneChangeDirection = log.LaneChangeDirection
@@ -51,6 +52,7 @@ class DesireHelper:
     self.alc = AutoLaneChangeController(self)
     self.lane_turn_controller = LaneTurnController(self)
     self.lane_turn_direction = TurnDirection.none
+    self.navigation_desires = NavigationDesires()
 
   @staticmethod
   def get_lane_change_direction(CS):
@@ -143,3 +145,7 @@ class DesireHelper:
         self.desire = log.Desire.none
 
     self.alc.update_state()
+
+    nav_desire = self.navigation_desires.update(carstate, lateral_active)
+    if nav_desire != log.Desire.none and (self.desire == log.Desire.none or self.desire in (log.Desire.turnLeft, log.Desire.turnRight)):
+      self.desire = nav_desire

sunnypilot/navd/README.md

@@ -0,0 +1,6 @@
+# Navigation
+
+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/constants.py

@@ -0,0 +1,16 @@
+"""
+Copyright (c) 2021-, James Vecellio, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+
+class NAV_CV:
+  """ These distances are expected in meters format and convert to desired format """
+  SHORT_DISTANCE_METERS = 200.0
+  QUARTER_MILE = 402.336
+  POINT_ONE_MILE = 160.9344
+  METERS_TO_KILO = 1000  # divide n by this
+  METERS_TO_MILE = 1609.344  # divide n by this
+  METERS_TO_FEET = 3.280839895  # multiply n by this

sunnypilot/navd/helpers.py

@@ -72,6 +72,15 @@ class Coordinate:
     return x * EARTH_MEAN_RADIUS
 
 
+def bearing_between_two_points(point_one: Coordinate, point_two: Coordinate) -> float:
+  dlon = math.radians(point_two.longitude - point_one.longitude)
+  bearing_radians = math.atan2(math.sin(dlon)* math.cos(point_two.latitude), math.cos(point_one.latitude) * math.sin(point_two.latitude) -
+                               math.sin(point_one.latitude) * math.cos(point_two.latitude) * math.cos(dlon))
+  bearing_degrees = math.degrees(bearing_radians)
+  bearing_normalized = (bearing_degrees + 360) % 360
+  return bearing_normalized
+
+
 def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate):
   if a.distance_to(b) < 0.01:
     return a.distance_to(p)
@@ -126,6 +135,8 @@ def string_to_direction(direction: str) -> str:
     if d in direction:
       if 'slight' in direction and d in MODIFIABLE_DIRECTIONS:
         return 'slight' + d.capitalize()
+      elif 'sharp' in direction and d in MODIFIABLE_DIRECTIONS:
+        return 'sharp' + d.capitalize()
       return d
   return 'none'
 

sunnypilot/navd/navigation_desires/navigation_desires.py

@@ -0,0 +1,42 @@
+"""
+Copyright (c) 2021-, James Vecellio, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import cereal.messaging as messaging
+from cereal import car, log
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+
+
+class NavigationDesires:
+  def __init__(self):
+    self.sm = messaging.SubMaster(['navigationd'])
+    self.desire = log.Desire.none
+    self._turn_speed_limit = 20 * CV.MPH_TO_MS
+    self._params = Params()
+    self.param_counter = -1
+    self.nav_allowed: bool = False
+
+  def update_params(self):
+    self.param_counter += 1
+    if self.param_counter % 60 == 0:  # every 3 seconds at 20hz
+      self.nav_allowed = self._params.get("NavDesiresAllowed", return_default=True)
+
+  def update(self, CS: car.CarState, lateral_active: bool) -> log.Desire:
+    self.update_params()
+    self.sm.update(0)
+    nav_msg = self.sm['navigationd']
+    self.desire = log.Desire.none
+    if self.nav_allowed and nav_msg.valid and lateral_active:
+      upcoming = nav_msg.upcomingTurn
+      if upcoming == 'slightLeft' and not CS.rightBlinker and not CS.leftBlindspot and CS.steeringPressed and CS.steeringTorque > 0:
+        self.desire = log.Desire.keepLeft
+      elif upcoming == 'slightRight' and not CS.leftBlinker and not CS.rightBlindspot and CS.steeringPressed and CS.steeringTorque < 0:
+        self.desire = log.Desire.keepRight
+      elif upcoming == 'left' and not CS.rightBlinker and not CS.leftBlindspot and CS.vEgo < self._turn_speed_limit:
+        self.desire = log.Desire.turnLeft
+      elif upcoming == 'right' and not CS.leftBlinker and not CS.rightBlindspot and CS.vEgo < self._turn_speed_limit:
+        self.desire = log.Desire.turnRight
+    return self.desire

sunnypilot/navd/navigation_desires/tests/test_navigation_desires.py

@@ -0,0 +1,96 @@
+"""
+Copyright (c) 2021-, James Vecellio, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import pytest
+import types
+
+from cereal import log
+from openpilot.common.params import Params
+
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from openpilot.sunnypilot.navd.navigation_desires.navigation_desires import NavigationDesires
+
+
+def make_car(vEgo=0, leftBlinker=False, rightBlinker=False, leftBlindspot=False, rightBlindspot=False, steeringPressed=False, steeringTorque=0):
+  return types.SimpleNamespace(
+    vEgo=vEgo, leftBlinker=leftBlinker, rightBlinker=rightBlinker,
+    leftBlindspot=leftBlindspot, rightBlindspot=rightBlindspot,
+    steeringPressed=steeringPressed, steeringTorque=steeringTorque
+  )
+
+NAVIGATION_PARAMS: list[tuple] = [
+  ('slightLeft', make_car(steeringPressed=True, steeringTorque=1), log.Desire.keepLeft),
+  ('slightRight', make_car(steeringPressed=True, steeringTorque=-1), log.Desire.keepRight),
+  ('slightLeft', make_car(vEgo=9, leftBlindspot=True), log.Desire.none),
+  ('slightRight', make_car(vEgo=9, rightBlindspot=True), log.Desire.none),
+  ('left', make_car(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False), log.Desire.turnLeft),
+  ('left', make_car(vEgo=5, leftBlinker=False, rightBlinker=True), log.Desire.none),
+  ('right', make_car(vEgo=6, rightBlinker=True, leftBlindspot=False), log.Desire.turnRight),
+  ('right', make_car(vEgo=6, rightBlinker=True, rightBlindspot=True), log.Desire.none),
+  ('left', make_car(vEgo=9, leftBlinker=True), log.Desire.none),
+]
+
+INTEGRATION_PARAMS: list[tuple] = [(carstate, upcoming, log.Desire.none, expected) for upcoming, carstate, expected in NAVIGATION_PARAMS] + [
+  (make_car(vEgo=6, leftBlinker=True, steeringPressed=True, steeringTorque=1), 'slightLeft', log.Desire.turnLeft, log.Desire.keepLeft),
+  (make_car(vEgo=5, rightBlinker=True, steeringPressed=True, steeringTorque=-1), 'slightRight', log.Desire.turnRight, log.Desire.keepRight),
+  (make_car(vEgo=9, leftBlinker=True), 'slightLeft', log.Desire.laneChangeLeft, log.Desire.laneChangeLeft),
+  (make_car(vEgo=9, rightBlinker=True), 'slightRight', log.Desire.laneChangeRight, log.Desire.laneChangeRight),
+  (make_car(vEgo=9), 'none', log.Desire.none, log.Desire.none),
+]
+
+def make_nav_msg(valid=False, upcoming='none'):
+  return types.SimpleNamespace(valid=valid, upcomingTurn=upcoming)
+
+def params_setter(allowed: bool):
+  params = Params()
+  params.put("NavDesiresAllowed", allowed)
+
+@pytest.fixture
+def mock_submaster(mocker):
+  mock_sm = mocker.patch('cereal.messaging.SubMaster')
+  mock_sm_instance = mocker.Mock()
+  mock_sm.return_value = mock_sm_instance
+  mock_sm_instance.__getitem__ = mocker.Mock(return_value=make_nav_msg(valid=False))
+  params_setter(True)
+  return mock_sm_instance
+
+@pytest.mark.parametrize("upcoming, carstate, expected", NAVIGATION_PARAMS)
+def test_navigation_desires_update(mock_submaster, mocker, upcoming, carstate, expected):
+  nav_desires = NavigationDesires()
+  nav_desires.sm.__getitem__ = mocker.Mock(return_value=make_nav_msg(valid=True, upcoming=upcoming))
+  nav_desires.update(carstate, True)
+  assert nav_desires.desire == expected
+
+@pytest.mark.parametrize("msg_valid,lateral_active", [(False, True), (True, False)])
+def test_invalid_or_inactive(mock_submaster, mocker, msg_valid, lateral_active):
+  nav_desires = NavigationDesires()
+  nav_desires.sm.__getitem__ = mocker.Mock(return_value=make_nav_msg(valid=msg_valid, upcoming='slightLeft'))
+  nav_desires.update(make_car(), lateral_active)
+  assert nav_desires.desire == log.Desire.none
+
+def test_update(mock_submaster, mocker):
+  mock_submaster.__getitem__ = mocker.Mock(return_value=make_nav_msg(valid=True, upcoming='left'))
+  nav_desires = NavigationDesires()
+
+  assert nav_desires.update(make_car(leftBlinker=True, steeringPressed=True, steeringTorque=1), True) == log.Desire.turnLeft
+
+  params_setter(False)
+  nav_desires.param_counter = 59
+  nav_desires.update(make_car(leftBlinker=True), True)
+  assert nav_desires.desire == log.Desire.none
+
+@pytest.mark.parametrize("carstate, upcoming, current_desire, expected", INTEGRATION_PARAMS)
+def test_desire_helper(mock_submaster, mocker, carstate, upcoming, current_desire, expected):
+  mock_submaster.__getitem__ = mocker.Mock(return_value=make_nav_msg(valid=True, upcoming=upcoming))
+  dh = DesireHelper()
+  dh.desire = current_desire
+
+  if current_desire in (log.Desire.laneChangeLeft, log.Desire.laneChangeRight):
+    dh.lane_change_state = log.LaneChangeState.laneChangeStarting
+    dh.lane_change_direction = log.LaneChangeDirection.left if current_desire == log.Desire.laneChangeLeft else log.LaneChangeDirection.right
+
+  dh.update(carstate, True, 1.0)
+  assert dh.desire == expected

sunnypilot/navd/navigation_helpers/mapbox_integration.py

@@ -0,0 +1,113 @@
+"""
+Copyright (c) 2021-, James Vecellio, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import requests
+from urllib.parse import quote
+
+from openpilot.common.params import Params
+
+
+class MapboxIntegration:
+  def __init__(self):
+    self.params = Params()
+
+  def get_public_token(self) -> str:
+    token: str = self.params.get('MapboxToken', return_default=True)
+    return token
+
+  def set_destination(self, postvars, current_lon, current_lat, bearing=None) -> tuple[dict, bool]:
+    if 'latitude' in postvars and 'longitude' in postvars:
+      self.nav_confirmed(postvars, current_lon, current_lat, bearing)
+      return postvars, True
+
+    addr = postvars['place_name']
+    if not addr:
+      return postvars, False
+
+    token = self.get_public_token()
+    url = f'https://api.mapbox.com/geocoding/v5/mapbox.places/{quote(addr)}.json?access_token={token}&limit=1&proximity={current_lon},{current_lat}'
+    try:
+      response = requests.get(url, timeout=5)
+      if response.status_code == 200:
+        features = response.json()['features']
+        if features:
+          longitude, latitude = features[0]['geometry']['coordinates']
+          postvars.update({'latitude': latitude, 'longitude': longitude, 'name': addr})
+          self.nav_confirmed(postvars, current_lon, current_lat, bearing)
+          return postvars, True
+    except requests.RequestException:
+      pass  # Broad exception to handle network errors like no internet without crashing navd process.
+    return postvars, False
+
+  def nav_confirmed(self, postvars, start_lon, start_lat, bearing=None) -> None:
+    if not postvars:
+      return
+
+    latitude = float(postvars['latitude'])
+    longitude = float(postvars['longitude'])
+
+    data: dict = {'navData': {'current': {'latitude': latitude, 'longitude': longitude}, 'route': {}}}
+
+    token = self.get_public_token()
+    route_data = self.generate_route(start_lon, start_lat, longitude, latitude, token, bearing)
+    if route_data:
+      data['navData']['route'] = route_data
+    self.params.put('MapboxSettings', data)
+
+  @staticmethod
+  def generate_route(start_lon, start_lat, end_lon, end_lat, token, bearing=None) -> dict | None:
+    if not token:
+      return None
+
+    params = {
+      'access_token': token,
+      'geometries': 'geojson',
+      'steps': 'true',
+      'overview': 'full',
+      'annotations': 'maxspeed',
+      'alternatives': 'false',
+      'banner_instructions': 'true',
+    }
+    if bearing is not None:
+      params['bearings'] = f'{int((bearing + 360) % 360):.0f},90;'
+
+    try:
+      response = requests.get(f'https://api.mapbox.com/directions/v5/mapbox/driving/{start_lon},{start_lat};{end_lon},{end_lat}', params=params, timeout=5)
+      data = response.json() if response.status_code == 200 else {}
+    except requests.RequestException:
+      return None
+
+    routes = data['routes'] if data else None
+    legs = routes[0]['legs'] if routes else None
+
+    if data.get('code') != 'Ok' or not routes or not legs:
+      return None
+
+    route = routes[0]
+    leg = legs[0]
+
+    steps = [
+      {
+        'maneuver': step['maneuver']['type'],
+        'instruction': step['maneuver']['instruction'],
+        'distance': step['distance'],
+        'duration': step['duration'],
+        'location': {'longitude': step['maneuver']['location'][0], 'latitude': step['maneuver']['location'][1]},
+        'modifier': step['maneuver'].get('modifier', 'none'),
+        'bannerInstructions': step['bannerInstructions'],
+      }
+      for step in leg['steps']
+    ]
+
+    maxspeed = [{'speed': item['speed'], 'unit': item['unit']} for item in leg['annotation']['maxspeed'] if 'speed' in item]
+
+    return {
+      'steps': steps,
+      'totalDistance': route['distance'],
+      'totalDuration': route['duration'],
+      'geometry': [{'longitude': coord[0], 'latitude': coord[1]} for coord in route['geometry']['coordinates']],
+      'maxspeed': maxspeed,
+    }

sunnypilot/navd/navigation_helpers/nav_instructions.py

@@ -0,0 +1,152 @@
+"""
+Copyright (c) 2021-, James Vecellio, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from numpy import interp
+
+from openpilot.common.params import Params
+
+from openpilot.sunnypilot.navd.helpers import Coordinate, bearing_between_two_points, distance_along_geometry, string_to_direction
+
+
+class NavigationInstructions:
+  def __init__(self):
+    self.coord = Coordinate(0, 0)
+    self.params = Params()
+
+    self._cached_route = None
+    self._route_loaded = False
+    self._no_route = False
+
+    self.closest_idx: float = 0
+
+  def get_route_progress(self, current_lat, current_lon) -> dict | None:
+    route = self.get_current_route()
+    if not route or not route['geometry'] or not route['steps']:
+      return None
+
+    self.coord.latitude = current_lat
+    self.coord.longitude = current_lon
+
+    # Find the closest point on the route relative to self
+    self.closest_idx, min_distance = min(((idx, self.coord.distance_to(coord)) for idx, coord in enumerate(route['geometry'])), key=lambda x: x[1])
+    closest_cumulative = distance_along_geometry(route['geometry'], self.coord)
+
+    # Find the current step index, which is the HIGHEST idx where the step location cumulative less/equal closest cumulative
+    current_step_idx = max((idx for idx, step in enumerate(route['steps']) if step['cumulative_distance'] <= closest_cumulative), default=-1)
+    current_step = route['steps'][current_step_idx if current_step_idx >= 0 else 0]
+
+    # The next turn is the next step relative to our cumulative index
+    next_turn_idx = current_step_idx + 1
+    next_turn = route['steps'][next_turn_idx] if 0 <= next_turn_idx < len(route['steps']) else None
+
+    current_maxspeed = current_step['maxspeed']
+
+    distance_to_end_of_step = max(0, current_step['distance'] - (closest_cumulative - current_step['cumulative_distance']))
+
+    all_maneuvers: list = []
+    max_maneuvers = 3
+    for idx in range(current_step_idx, min(current_step_idx + max_maneuvers, len(route['steps']))):
+      step = route['steps'][idx]
+      if idx == current_step_idx:
+        distance = distance_to_end_of_step
+      else:
+        distance = step['cumulative_distance'] - closest_cumulative
+      all_maneuvers.append({'distance': distance, 'type': step['maneuver'], 'modifier': step['modifier'], 'instruction': step['instruction']})
+
+    return {
+      'distance_from_route': min_distance,
+      'current_step': current_step,
+      'next_turn': next_turn,
+      'current_maxspeed': current_maxspeed,
+      'all_maneuvers': all_maneuvers,
+      'current_step_idx': current_step_idx,
+      'distance_to_end_of_step': distance_to_end_of_step,
+    }
+
+  def get_current_route(self):
+    if self._route_loaded and self._cached_route is not None:
+      return self._cached_route
+    if self._no_route:
+      return None
+
+    param_value = self.params.get('MapboxSettings')
+    route = param_value['navData']['route'] if param_value else None
+    if not route:
+      self._no_route = True
+      return None
+
+    geometry = [Coordinate(coord['latitude'], coord['longitude']) for coord in route['geometry']]
+    cumulative_distances = [0.0]
+    cumulative_distances.extend(cumulative_distances[-1] + geometry[step - 1].distance_to(geometry[step]) for step in range(1, len(geometry)))
+    maxspeed = [(speed['speed'], speed['unit']) for speed in route['maxspeed']]
+    steps = []
+    for step in route['steps']:
+      location = Coordinate(step['location']['latitude'], step['location']['longitude'])
+      closest_idx = min(range(len(geometry)), key=lambda i: location.distance_to(geometry[i]))
+      steps.append({
+        'bannerInstructions': step['bannerInstructions'],
+        'distance': step['distance'],
+        'duration': step['duration'],
+        'maneuver': step['maneuver'],
+        'location': location,
+        'cumulative_distance': cumulative_distances[closest_idx],
+        'maxspeed': maxspeed[min(closest_idx, len(maxspeed) - 1)] if len(maxspeed) > 0 else (0, 'kmh'),
+        'modifier': string_to_direction(step['modifier']),
+        'instruction': step['instruction'],
+      })
+    self._cached_route = {
+      'bearings': [bearing_between_two_points(geometry[i], geometry[i+2]) for i in range(len(geometry)-2)],
+      'steps': steps,
+      'total_distance': route['totalDistance'],
+      'total_duration': route['totalDuration'],
+      'geometry': geometry,
+      'cumulative_distances': cumulative_distances,
+      'maxspeed': maxspeed,
+    }
+    self._route_loaded = True
+    return self._cached_route
+
+  def clear_route_cache(self):
+    self._cached_route = None
+    self._route_loaded = False
+    self._no_route = False
+
+  def route_bearing_misalign(self, route, bearing, v_ego) -> bool:
+    route_bearing_misalign:bool = False
+
+    if v_ego < 5.0:
+      route_bearing_misalign = False
+    elif 0 < self.closest_idx < len(route['geometry']) -1:
+      route_bearing = route['bearings'][self.closest_idx -1]
+      current_bearing_normalized = (bearing + 360) % 360
+      bearing_difference = abs(current_bearing_normalized - route_bearing)
+
+      if min(bearing_difference, 360 - bearing_difference) > 95:
+        route_bearing_misalign = True  # flag for recompute/cancellation
+    return route_bearing_misalign
+
+  def get_upcoming_turn_from_progress(self, progress, current_lat, current_lon, v_ego: float) -> str:
+    if progress and progress['next_turn']:
+      speed_breakpoints: list = [0, 5, 10, 15, 20, 25, 30, 35, 40]
+      distance_breakpoints: list = [20, 25, 30, 45, 60, 75, 90, 105, 120]
+      distance_interp = interp(v_ego, speed_breakpoints, distance_breakpoints)
+
+      self.coord.latitude = current_lat
+      self.coord.longitude = current_lon
+      distance = self.coord.distance_to(progress['next_turn']['location'])
+
+      if distance <= distance_interp:
+        modifier = progress['next_turn']['modifier']
+        return str(modifier)
+    return 'none'
+
+  @staticmethod
+  def arrived_at_destination(progress, v_ego) -> bool:
+    if v_ego < 1.0:
+      maneuvers = progress['all_maneuvers'][0]
+      if maneuvers['type'] == 'arrive' or maneuvers['instruction'].startswith('Your destination'):
+        return True
+    return False

sunnypilot/navd/navigation_helpers/tests/test_mapbox.py

@@ -0,0 +1,98 @@
+"""
+Copyright (c) 2021-, James Vecellio, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import os
+
+from openpilot.common.constants import CV
+
+from openpilot.sunnypilot.navd.navigation_helpers.mapbox_integration import MapboxIntegration
+from openpilot.sunnypilot.navd.navigation_helpers.nav_instructions import NavigationInstructions
+
+
+class TestMapbox:
+  @classmethod
+  def setup_class(cls):
+    cls.mapbox = MapboxIntegration()
+    cls.nav = NavigationInstructions()
+
+    token = os.environ.get('MAPBOX_TOKEN_CI')
+    if token:
+      cls.mapbox.params.put('MapboxToken', token)
+
+    # route setup
+    cls.current_lon, cls.current_lat = -119.17557, 34.23305
+    cls.mapbox.params.put('MapboxRoute', '740 E Ventura Blvd. Camarillo, CA')
+    cls.postvars = {"place_name": cls.mapbox.params.get('MapboxRoute')}
+    cls.postvars, cls.valid_addr = cls.mapbox.set_destination(cls.postvars, cls.current_lon, cls.current_lat)
+    cls.route = cls.nav.get_current_route()
+    cls.progress = cls.nav.get_route_progress(cls.current_lat, cls.current_lon)
+
+  def test_set_destination(self):
+    assert self.valid_addr
+    settings = self.mapbox.params.get('MapboxSettings')
+    assert settings is not None
+    dest_lat = settings['navData']['current']['latitude']
+    dest_lon = settings['navData']['current']['longitude']
+    assert dest_lat == self.postvars["latitude"] and dest_lon == self.postvars["longitude"]
+
+  def test_get_route(self):
+    assert self.route is not None
+    assert 'steps' in self.route
+    assert 'geometry' in self.route
+    assert 'maxspeed' in self.route
+    assert 'total_distance' in self.route
+    assert 'total_duration' in self.route
+    assert len(self.route['steps']) > 0
+    assert len(self.route['geometry']) > 0
+    assert len(self.route['maxspeed']) > 0
+
+    if self.route and 'steps' in self.route:
+      for step in self.route['steps']:
+        assert 'modifier' in step
+
+  def test_upcoming_turn_detection(self):
+    upcoming = self.nav.get_upcoming_turn_from_progress(self.progress, self.current_lat, self.current_lon, v_ego=40.0)
+    assert isinstance(upcoming, str)
+    assert upcoming == 'none'
+
+    if self.route['steps']:
+      turn_lat = self.route['steps'][1]['location'].latitude
+      turn_lon = self.route['steps'][1]['location'].longitude
+      close_lat = turn_lat - 0.000175  # slightly before the turn
+      if self.progress and self.progress.get('next_turn'):
+        expected_turn = self.progress['next_turn']['modifier']
+        upcoming_close = self.nav.get_upcoming_turn_from_progress(self.progress, close_lat, turn_lon, v_ego=0.0)
+        if expected_turn:
+          assert upcoming_close == expected_turn == 'right', "Should be a right turn upcoming"
+
+  def test_route_progress_tracking(self):
+    assert self.progress is not None
+    assert 'distance_from_route' in self.progress
+    assert 'next_turn' in self.progress
+    assert 'current_maxspeed' in self.progress
+    assert 'all_maneuvers' in self.progress
+    assert 'distance_to_end_of_step' in self.progress
+    assert self.progress['distance_from_route'] >= 0
+    assert isinstance(self.progress['all_maneuvers'], list)
+
+  def test_speed_limit_handling(self):
+    speed_limit_metric = self.progress['current_maxspeed'][0]
+    speed_limit_imperial = (round(speed_limit_metric * CV.KPH_TO_MPH))
+    assert isinstance(speed_limit_metric, int)
+    assert isinstance(speed_limit_imperial, int)
+
+  def test_arrival_detection(self):
+    is_arrived = self.nav.arrived_at_destination(self.progress, 2.0)
+    assert isinstance(is_arrived, bool)
+    assert not is_arrived
+
+  def test_bearing_misalign(self):
+    lat = self.route['steps'][1]['location'].latitude
+    lon = self.route['steps'][1]['location'].longitude
+    self.nav.get_route_progress(lat, lon)
+    route_bearing_misaligned = self.nav.route_bearing_misalign(self.route, 45, 5.0)
+    # based on math: closest index: 7, normalized bearing: 45 route bearing: 180.5486953778888, expected differential: 135.54869538
+    assert route_bearing_misaligned

sunnypilot/navd/navigationd.py

@@ -0,0 +1,167 @@
+"""
+Copyright (c) 2021-, James Vecellio, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from math import degrees
+from numpy import interp
+
+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.constants import NAV_CV
+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(['carControlSP', '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.allow_navigation: bool = False
+    self.recompute_allowed: bool = False
+    self.allow_recompute: bool = False
+    self.reroute_counter: int = 0
+    self.cancel_route_counter: int = 0
+
+    self.frame: int = -1
+    self.last_position: Coordinate | None = None
+    self.last_bearing: float | None = None
+    self.valid: bool = False
+
+  def _update_params(self):
+    if self.last_position is not None:
+      self.frame += 1
+      if self.frame % 15 == 0:
+        self.allow_navigation = self.params.get('AllowNavigation', 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)
+
+        if valid_addr:
+          self.destination = self.new_destination
+          self.nav_instructions.clear_route_cache()
+          self.route = self.nav_instructions.get_current_route()
+          self.cancel_route_counter = 0
+          self.reroute_counter = 0
+
+      if self.cancel_route_counter == 30:
+        self.cancel_route_counter = 0
+        self.params.put_nonblocking("MapboxRoute", "")
+        self.nav_instructions.clear_route_cache()
+        self.route = None
+
+      self.valid = self.route is not None
+
+  def _update_navigation(self) -> tuple[str, dict | None, dict]:
+    banner_instructions: str = ''
+    nav_data: dict = {}
+    if self.allow_navigation and self.route and self.last_position is not None:
+      if progress := self.nav_instructions.get_route_progress(self.last_position.latitude, self.last_position.longitude):
+        v_ego = float(max(self.sm['carControlSP'].speed, 0.0))
+        nav_data['upcoming_turn'] = self.nav_instructions.get_upcoming_turn_from_progress(progress, self.last_position.latitude,
+                                                                                          self.last_position.longitude, v_ego)
+        speed_limit, _ = progress['current_maxspeed']
+        nav_data['current_speed_limit'] = speed_limit
+        arrived = self.nav_instructions.arrived_at_destination(progress, v_ego)
+
+        if progress['current_step']:
+          if parsed := parse_banner_instructions(progress['current_step']['bannerInstructions'], progress['distance_to_end_of_step']):
+            banner_instructions = parsed['maneuverPrimaryText']
+
+        nav_data['distance_from_route'] = progress['distance_from_route']
+        speed_breakpoints: list = [0.0, 5.0, 10.0, 20.0, 40.0]
+        distance_list: list = [100.0, 125.0, 150.0, 200.0, 250.0]
+        large_distance: bool = progress['distance_from_route'] > float(interp(v_ego, speed_breakpoints, distance_list))
+
+        route_bearing_misalign: bool = self.nav_instructions.route_bearing_misalign(self.route, self.last_bearing, v_ego)
+
+        if large_distance and not arrived:
+          self.cancel_route_counter = self.cancel_route_counter + 1 if progress['distance_from_route'] > NAV_CV.QUARTER_MILE else 0
+          if self.recompute_allowed:
+            self.reroute_counter += 1
+        elif arrived:
+          self.cancel_route_counter += 1
+          self.recompute_allowed = False
+        elif route_bearing_misalign:
+          self.cancel_route_counter += 1
+          if self.recompute_allowed:
+            self.reroute_counter += 1
+        else:
+          self.cancel_route_counter = 0
+          self.reroute_counter = 0
+
+        # Don't recompute in last segment to prevent reroute loops
+        if progress['current_step_idx'] == len(self.route['steps']) - 1:
+          self.recompute_allowed = False
+          self.allow_navigation = False
+    else:
+      banner_instructions = ''
+      progress = None
+      nav_data = {}
+
+    return banner_instructions, progress, nav_data
+
+  def _build_navigation_message(self, banner_instructions: str, progress: dict | None, nav_data: dict, valid: bool):
+    msg = messaging.new_message('navigationd')
+    msg.valid = valid
+    msg.navigationd.upcomingTurn = nav_data.get('upcoming_turn', 'none')
+    msg.navigationd.currentSpeedLimit = nav_data.get('current_speed_limit', 0)
+    msg.navigationd.bannerInstructions = banner_instructions
+    msg.navigationd.distanceFromRoute = nav_data.get('distance_from_route', 0.0)
+    msg.navigationd.valid = self.valid
+
+    all_maneuvers = (
+      [custom.Navigationd.Maneuver.new_message(distance=m['distance'], type=m['type'], modifier=m['modifier'],
+                                               instruction=m['instruction']) for m in progress['all_maneuvers']]
+      if progress
+      else []
+    )
+    msg.navigationd.allManeuvers = all_maneuvers
+    return msg
+
+  def run(self):
+    cloudlog.warning('navigationd init')
+
+    while True:
+      self.sm.update(0)
+      location = self.sm['liveLocationKalman']
+      localizer_valid = location.positionGeodetic.valid if location else False
+
+      if localizer_valid:
+        self.last_bearing = 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, valid=localizer_valid)
+
+      self.pm.send('navigationd', msg)
+      self.rk.keep_time()
+
+
+def main():
+  nav = Navigationd()
+  nav.run()

sunnypilot/navd/tests/test_navigationd.py

@@ -0,0 +1,67 @@
+"""
+Copyright (c) 2021-, James Vecellio, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import platform
+import pytest
+
+import cereal.messaging as messaging
+
+from openpilot.sunnypilot.navd.navigationd import Navigationd
+from openpilot.sunnypilot.navd.helpers import Coordinate
+
+
+class TestNavigationd:
+  is_darwin = platform.system() == "Darwin"
+
+  @pytest.fixture(autouse=True)
+  def setup_method(self, mocker):
+    if self.is_darwin:
+      mocker.patch('cereal.messaging.SubMaster')
+      mocker.patch('cereal.messaging.PubMaster')
+
+  def test_update_params(self):
+    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(self):
+    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(self):
+    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(self):
+    if self.is_darwin:
+      nav = Navigationd()
+      msg = nav._build_navigation_message('', None, {}, True)
+      assert msg.navigationd.bannerInstructions == ''
+      assert msg.navigationd.valid is False
+    else:
+      sm = messaging.SubMaster(['navigationd'])
+      nav = Navigationd()
+      msg = nav._build_navigation_message('', None, {}, True)
+
+      nav.pm.send('navigationd', msg)
+      sm.update()
+      received_msg = sm['navigationd']
+
+      assert received_msg.bannerInstructions == msg.navigationd.bannerInstructions
+      assert received_msg.valid == msg.navigationd.valid

sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -72,6 +72,8 @@ class ControlsExt:
 
     CC_SP.intelligentCruiseButtonManagement = sm['selfdriveStateSP'].intelligentCruiseButtonManagement
 
+    CC_SP.speed = sm['carState'].vEgo
+
     return CC_SP
 
   @staticmethod

sunnypilot/selfdrive/controls/lib/tests/test_auto_lane_change.py

@@ -7,11 +7,17 @@ See the LICENSE.md file in the root directory for more details.
 
 from parameterized import parameterized
 
+import cereal.messaging
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState, LaneChangeDirection
 from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode, \
   AUTO_LANE_CHANGE_TIMER, ONE_SECOND_DELAY
 
+class MockSubMaster:
+  def __init__(self, services):
+    pass
+
+
 AUTO_LANE_CHANGE_TIMER_COMBOS = [
   (AutoLaneChangeMode.NUDGELESS, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGELESS]),
   (AutoLaneChangeMode.HALF_SECOND, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.HALF_SECOND]),
@@ -23,6 +29,7 @@ AUTO_LANE_CHANGE_TIMER_COMBOS = [
 
 class TestAutoLaneChangeController:
   def setup_method(self):
+    cereal.messaging.SubMaster = MockSubMaster
     self.DH = DesireHelper()
     self.alc = AutoLaneChangeController(self.DH)
 

sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py

@@ -1,14 +1,29 @@
 import pytest
+
+import cereal.messaging
 from cereal import log, custom
 from openpilot.common.params import Params
 
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
-from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
 
 TurnDirection = custom.ModelDataV2SP.TurnDirection
 
 
+class MockSubMaster:
+  def __init__(self, services): pass
+
+  def update(self, timeout): pass
+
+  def __getitem__(self, key):
+    return type('nav_msg', (), {'valid': False})()
+
+
+@pytest.fixture(autouse=True)
+def mock_submaster():
+  cereal.messaging.SubMaster = MockSubMaster
+
+
 @pytest.mark.parametrize("left_blinker,right_blinker,v_ego,blindspot_left,blindspot_right,expected", [
   (True, False, 5, False, False, TurnDirection.turnLeft),
   (False, True, 6, False, False, TurnDirection.turnRight),
@@ -107,7 +122,6 @@ def set_lane_turn_params():
 ])
 def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
   dh = DesireHelper()
-  dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
   for _ in range(10):
     dh.update(carstate, lateral_active, lane_change_prob)
-  assert dh.desire == expected_desire  # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers
+  assert dh.desire == expected_desire

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