gap

#	common/params_keys.h
#	sunnypilot/navd/constants.py

cereal/custom.capnp

@@ -340,6 +340,7 @@ struct OnroadEventSP @0xda96579883444c35 {
     speedLimitChanged @21;
     speedLimitPending @22;
     e2eChime @23;
+    navigationBanner @24;
   }
 }
 

common/params_keys.h

@@ -194,6 +194,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"MapboxRoute", {PERSISTENT, STRING}},
     {"MapboxRecompute", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"NavDesiresAllowed", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"NavEvents", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     // Neural Network Lateral Control
     {"NeuralNetworkLateralControl", {PERSISTENT | BACKUP, BOOL, "0"}},

selfdrive/controls/plannerd.py

@@ -27,7 +27,7 @@ def main():
   longitudinal_planner = LongitudinalPlanner(CP, CP_SP)
   pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
   sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
-                            'liveMapDataSP', 'carStateSP', gps_location_service],
+                            'liveMapDataSP', 'navigationd', 'carStateSP', gps_location_service],
                            poll='carState')
 
   while True:

selfdrive/selfdrived/selfdrived.py

@@ -88,7 +88,7 @@ class SelfdriveD(CruiseHelper):
     # TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches
     self.car_state_sock = messaging.sub_sock('carState', timeout=20)
 
-    ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ['modelDataV2SP']
+    ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ['modelDataV2SP'] + ['navigationd']
     if SIMULATION:
       ignore += ['driverCameraState', 'managerState']
     if REPLAY:
@@ -98,7 +98,7 @@ class SelfdriveD(CruiseHelper):
                                    'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
                                    'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback',
-                                   'modelDataV2SP', 'longitudinalPlanSP'] + \
+                                   'modelDataV2SP', 'longitudinalPlanSP', 'navigationd'] + \
                                    self.camera_packets + self.sensor_packets + self.gps_packets,
                                   ignore_alive=ignore, ignore_avg_freq=ignore,
                                   ignore_valid=ignore, frequency=int(1/DT_CTRL))

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -71,6 +71,7 @@ class Plant:
     model = messaging.new_message('modelV2')
     car_state_sp = messaging.new_message('carStateSP')
     live_map_data_sp = messaging.new_message('liveMapDataSP')
+    navigationd = messaging.new_message('navigationd')
     gps_data = messaging.new_message('gpsLocation')
     a_lead = (v_lead - self.v_lead_prev)/self.ts
     self.v_lead_prev = v_lead
@@ -141,6 +142,7 @@ class Plant:
           'modelV2': model.modelV2,
           'carStateSP': car_state_sp.carStateSP,
           'liveMapDataSP': live_map_data_sp.liveMapDataSP,
+          'navigationd': navigationd.navigationd,
           'gpsLocation': gps_data.gpsLocation}
     self.planner.update(sm)
     self.acceleration = self.planner.output_a_target

sunnypilot/navd/event_builder.py

@@ -0,0 +1,83 @@
+"""
+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 cereal import custom, messaging
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+
+from sunnypilot.navd.constants import NAV_CV
+
+
+class EventBuilder:
+  def __init__(self):
+    self._counter: int = -1
+    self._enabled: bool = False
+    self._params = Params()
+
+  @staticmethod
+  def _build_banner_message(metric: bool, nav_msg):
+    m = nav_msg.allManeuvers[1] if len(nav_msg.allManeuvers) > 1 else nav_msg.allManeuvers[0]
+    banner = m.instruction
+
+    if metric:
+      dist = f'{m.distance / NAV_CV.METERS_TO_KILO:.1f} km,'
+      if m.distance < NAV_CV.SHORT_DISTANCE_METERS:
+        dist = f'{int(m.distance)}m,'
+    else:
+      dist = f'{m.distance / NAV_CV.METERS_TO_MILE:.1f} mi,'
+      if m.distance < NAV_CV.QUARTER_MILE:
+        dist = f'{round((m.distance * NAV_CV.METERS_TO_FEET) / 50) * 50}ft,'
+
+    if m.type == 'arrive' or m.type == 'depart' or 'Your destination' in banner:
+      base_msg = banner
+    elif banner.startswith(('Continue', 'Drive', 'Head')):
+      base_msg = f'For {dist} {banner}'
+    elif 'Turn' in banner or 'Take' in banner or 'Make' in banner:
+      base_msg = f'In {dist} {banner}'
+    else:
+      base_msg = f'For {dist} Continue on {banner}'
+
+    return base_msg
+
+  @staticmethod
+  def _get_turning_message(upcoming_turn):
+    turn_messages = {
+      'left': 'Turning Left, Make sure to nudge the wheel',
+      'right': 'Turning Right, Make sure to nudge the wheel',
+      'slightLeft': 'Keeping Left',
+      'slightRight': 'Keeping Right',
+      'sharpLeft': 'Sharp Left Turn',
+      'sharpRight': 'Sharp Right Turn',
+      'straight': 'Continuing Straight',
+      'uturn': 'U-Turn Ahead',
+    }
+    return turn_messages.get(upcoming_turn, f"Upcoming {upcoming_turn.replace('_', ' ').title()}")
+
+  @staticmethod
+  def build_navigation_events(sm: messaging.SubMaster, metric=True) -> list:
+    nav_msg = sm['navigationd']
+    if not nav_msg.valid:
+      return []
+
+    banner_message = EventBuilder._build_banner_message(metric, nav_msg)
+
+    if nav_msg.upcomingTurn != 'none':
+      banner_message = EventBuilder._get_turning_message(nav_msg.upcomingTurn)
+
+    return [{
+      'name': custom.OnroadEventSP.EventName.navigationBanner,
+      'message': banner_message,
+    }]
+
+  def update(self, sm: messaging.SubMaster) -> list:
+    self._counter += 1
+    if self._counter % int(3.0 / DT_MDL) == 0:
+      self._enabled = self._params.get("NavEvents", return_default=True)
+
+    if self._enabled:
+      return self.build_navigation_events(sm)
+    else:
+      return []

sunnypilot/navd/tests/test_event_builder.py

@@ -0,0 +1,92 @@
+"""
+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 cereal import custom
+from openpilot.common.params import Params
+
+from openpilot.sunnypilot.navd.event_builder import EventBuilder
+
+
+class MockSM(dict):
+  def __init__(self, nav_msg):
+    super().__init__()
+    self['navigationd'] = nav_msg
+
+
+class TestEventBuilder:
+  def setup_method(self):
+    self.params = Params()
+    self.event_builder = EventBuilder()
+
+  def create_nav_msg(self, upcoming_turn='none', valid=True):
+    nav_msg = custom.Navigationd.new_message()
+    nav_msg.valid = valid
+    nav_msg.upcomingTurn = upcoming_turn
+    nav_msg.allManeuvers = [
+      custom.Navigationd.Maneuver.new_message(distance=192.84873284, type='turn', modifier='left', instruction='West Esplanade Drive'),
+      custom.Navigationd.Maneuver.new_message(distance=192.84809314, type='turn', modifier='right', instruction='West Esplanade Drive'),
+    ]
+    return nav_msg
+
+  def test_validity(self):
+    nav_msg = self.create_nav_msg(valid=False)
+    events = EventBuilder.build_navigation_events(MockSM(nav_msg))
+    assert events == []
+
+  def test_enabled(self):
+    self.params.put("NavEvents", True)
+    nav_msg = self.create_nav_msg()
+    events = self.event_builder.update(MockSM(nav_msg))
+    expected = [{
+      'name': custom.OnroadEventSP.EventName.navigationBanner,
+      'message': 'For 192m, Continue on West Esplanade Drive'
+    }]
+    assert events == expected
+
+    self.params.put("NavEvents", False)
+    self.event_builder._counter = 59
+    events = self.event_builder.update(MockSM(nav_msg))
+    assert events == []
+
+
+  def test_build_navigation_events(self):
+    nav_msg = self.create_nav_msg()
+    events = EventBuilder.build_navigation_events(MockSM(nav_msg), False)
+    expected = [{
+      'name': custom.OnroadEventSP.EventName.navigationBanner,
+      'message': 'For 650ft, Continue on West Esplanade Drive',
+    }]
+    assert events == expected
+
+  def test_distance_condition_imperial(self):
+    nav_msg = self.create_nav_msg()
+    nav_msg.allManeuvers[1] = custom.Navigationd.Maneuver.new_message(distance=160.0, type='continue', modifier='straight', instruction='1234 Apple Way')
+    events = EventBuilder.build_navigation_events(MockSM(nav_msg), False)
+    expected = [{
+      'name': custom.OnroadEventSP.EventName.navigationBanner,
+      'message': 'For 500ft, Continue on 1234 Apple Way',
+    }]
+    assert events == expected
+
+  def test_upcoming_turn_override(self):
+    nav_msg = self.create_nav_msg(upcoming_turn='left')
+    events = EventBuilder.build_navigation_events(MockSM(nav_msg))
+    expected = [{
+      'name': custom.OnroadEventSP.EventName.navigationBanner,
+      'message': 'Turning Left, Make sure to nudge the wheel',
+    }]
+    assert events == expected
+
+  def test_straight(self):
+    nav_msg = self.create_nav_msg()
+    nav_msg.allManeuvers[1] = custom.Navigationd.Maneuver.new_message(distance=80.0, type='continue', modifier='straight', instruction='1234 Apple Way')
+
+    events = EventBuilder.build_navigation_events(MockSM(nav_msg))
+    expected = [{
+      'name': custom.OnroadEventSP.EventName.navigationBanner,
+      'message': 'For 80m, Continue on 1234 Apple Way'
+    }]
+    assert events == expected

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -16,6 +16,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 from openpilot.sunnypilot.models.helpers import get_active_bundle
+from openpilot.sunnypilot.navd.event_builder import EventBuilder
 
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
@@ -32,6 +33,7 @@ class LongitudinalPlannerSP:
     self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
     self.source = LongitudinalPlanSource.cruise
     self.e2e_alerts_helper = E2EAlertsHelper()
+    self.event_builder = EventBuilder()
 
     self.output_v_target = 0.
     self.output_a_target = 0.
@@ -77,10 +79,16 @@ class LongitudinalPlannerSP:
     self.output_v_target, self.output_a_target = targets[self.source]
     return self.output_v_target, self.output_a_target
 
+  def update_navigation_events(self, sm: messaging.SubMaster) -> None:
+    nav_events = self.event_builder.update(sm)
+    for event in nav_events:
+      self.events_sp.add(event['name'])
+
   def update(self, sm: messaging.SubMaster) -> None:
     self.events_sp.clear()
     self.dec.update(sm)
     self.e2e_alerts_helper.update(sm, self.events_sp)
+    self.update_navigation_events(sm)
 
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
     plan_sp_send = messaging.new_message('longitudinalPlanSP')

sunnypilot/selfdrive/selfdrived/events.py

@@ -4,6 +4,7 @@ from openpilot.common.constants import CV
 from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \
   NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD
+from openpilot.sunnypilot.navd.event_builder import EventBuilder
 
 
 AlertSize = log.SelfdriveState.AlertSize
@@ -55,6 +56,14 @@ def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messag
     Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleLow, .1)
 
 
+def navigation_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  events = EventBuilder.build_navigation_events(sm, metric)
+  if not events:
+    return Alert("", "", AlertStatus.normal, AlertSize.none, Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.)
+
+  return Alert(events[0]['message'], "", AlertStatus.normal, AlertSize.small, Priority.LOW, VisualAlert.none, AudibleAlert.none, 2.)
+
+
 class EventsSP(EventsBase):
   def __init__(self):
     super().__init__()
@@ -226,4 +235,8 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
       AlertStatus.normal, AlertSize.none,
       Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
   },
+
+  EventNameSP.navigationBanner: {
+    ET.WARNING: navigation_alert,
+  },
 }