feat: navigationd onroad events

cereal/custom.capnp

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

sunnypilot/navd/event_builder.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.
+"""
+from cereal import custom
+
+
+def build_navigation_events(sm) -> list:
+  nav_msg = sm['navigationd']
+  if not nav_msg.valid:
+    return []
+
+  banner_message = _build_banner_message(nav_msg)
+  if not banner_message:
+    return []
+
+  if nav_msg.upcomingTurn != 'none':
+    banner_message = _get_turning_message(nav_msg.upcomingTurn)
+    alert_size = 'small'
+  else:
+    alert_size = 'mid'
+
+  return [{
+    'name': custom.OnroadEventSP.EventName.navigationBanner,
+    'type': 'warning',
+    'message': banner_message,
+    'size': alert_size,
+  }]
+
+
+def _build_banner_message(nav_msg):
+  m = nav_msg.allManeuvers[0]
+  if m.distance >= 1000:
+    return None
+
+  dist_str = f"{int(m.distance)}m"
+  next_m = nav_msg.allManeuvers[1] if len(nav_msg.allManeuvers) > 1 else m
+  banner = nav_msg.bannerInstructions
+
+  if next_m.modifier == 'sharp right' and 'Turn right' in banner:
+    banner = banner.replace('Turn right', 'Take a sharp right')
+  elif next_m.modifier == 'sharp left' and 'Turn left' in banner:
+    banner = banner.replace('Turn left', 'Take a sharp left')
+  elif next_m.modifier == 'slight right' and 'Turn right' in banner:
+    banner = banner.replace('Turn right', 'Take a slight right')
+  elif next_m.modifier == 'slight left' and 'Turn left' in banner:
+    banner = banner.replace('Turn left', 'Take a slight left')
+  elif next_m.modifier == 'uturn' and 'Turn' in banner:
+    banner = banner.replace('Turn', 'Make a U-turn', 1)
+
+  return f"In {dist_str}, {banner}"
+
+
+def _get_turning_message(upcoming_turn):
+  turn_messages = {
+    'left': 'Turning Left',
+    'right': 'Turning Right',
+    '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()}")

sunnypilot/navd/helpers.py

@@ -163,6 +163,12 @@ def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) -
   if field_valid(p, 'modifier'):
     instruction['maneuverModifier'] = p['modifier']
 
+  # Combine with secondary and/or sub for full text
+  if field_valid(current_banner, 'secondary') and p.get('type') == 'arrive':
+    instruction['maneuverPrimaryText'] += f" at {current_banner['secondary']['text']}"
+  if field_valid(current_banner, 'sub') and current_banner['sub'].get('type') == 'turn':
+    instruction['maneuverPrimaryText'] += f" onto {current_banner['sub']['text']}"
+
   # Secondary
   if field_valid(current_banner, 'secondary'):
     instruction['maneuverSecondaryText'] = current_banner['secondary']['text']

sunnypilot/navd/tests/test_event_builder.py

@@ -0,0 +1,84 @@
+from cereal import custom
+
+from openpilot.sunnypilot.navd.event_builder import build_navigation_events
+
+
+class MockSM:
+  def __init__(self, nav_msg):
+    self.nav_msg = nav_msg
+
+  def __getitem__(self, key):
+    if key == 'navigationd':
+      return self.nav_msg
+    return None
+
+
+class TestEventBuilder:
+  def test_build_navigation_events(self):
+    # These direction messages were taken courtesy of the autonomy repo <3
+    nav_msg = custom.Navigationd.new_message()
+    nav_msg.valid = True
+    nav_msg.bannerInstructions = 'Turn right onto West Esplanade Drive'
+    nav_msg.upcomingTurn = 'none'
+    nav_msg.allManeuvers = [
+      custom.Navigationd.Maneuver.new_message(distance=192.848, type='depart', modifier='none'),
+      custom.Navigationd.Maneuver.new_message(distance=192.64264487162754, type='turn', modifier='right')
+    ]
+
+    events = build_navigation_events(MockSM(nav_msg))
+    expected = [{
+      'name': custom.OnroadEventSP.EventName.navigationBanner,
+      'type': 'warning',
+      'message': 'In 192m, Turn right onto West Esplanade Drive',
+      'size': 'mid'
+    }]
+    assert events == expected
+
+  def test_no_event_when_distance_too_far(self):
+    nav_msg = custom.Navigationd.new_message()
+    nav_msg.valid = True
+    nav_msg.bannerInstructions = 'Turn right'
+    nav_msg.upcomingTurn = 'none'
+    nav_msg.allManeuvers = [
+      custom.Navigationd.Maneuver.new_message(distance=1000.0, type='turn', modifier='right')
+    ]
+
+    events = build_navigation_events(MockSM(nav_msg))
+    assert events == []
+
+  def test_upcoming_turn_override(self):
+    nav_msg = custom.Navigationd.new_message()
+    nav_msg.valid = True
+    nav_msg.bannerInstructions = 'Turn right'
+    nav_msg.upcomingTurn = 'left'
+    nav_msg.allManeuvers = [
+      custom.Navigationd.Maneuver.new_message(distance=50.0, type='turn', modifier='left')
+    ]
+
+    events = build_navigation_events(MockSM(nav_msg))
+    expected = [{
+      'name': custom.OnroadEventSP.EventName.navigationBanner,
+      'type': 'warning',
+      'message': 'Turning Left',
+      'size': 'small'
+    }]
+    assert events == expected
+
+  def test_sharp_turn_enhancement(self):
+    nav_msg = custom.Navigationd.new_message()
+    nav_msg.valid = True
+    nav_msg.bannerInstructions = 'Turn right onto Main St'
+    nav_msg.upcomingTurn = 'none'
+    nav_msg.allManeuvers = [
+      custom.Navigationd.Maneuver.new_message(distance=300.0, type='turn', modifier='none'),
+      custom.Navigationd.Maneuver.new_message(distance=300.0, type='turn', modifier='sharp right')
+    ]
+
+    events = build_navigation_events(MockSM(nav_msg))
+    expected = [{
+      'name': custom.OnroadEventSP.EventName.navigationBanner,
+      'type': 'warning',
+      'message': 'In 300m, Take a sharp right onto Main St',
+      'size': 'mid'
+    }]
+    assert events == expected

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 build_navigation_events
 
 
 AlertSize = log.SelfdriveState.AlertSize
@@ -55,6 +56,15 @@ 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 = build_navigation_events(sm)
+  if not events:
+    return Alert("", "", AlertStatus.normal, AlertSize.none, Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.)
+
+  size = AlertSize.mid if events[0]['size'] == 'mid' else AlertSize.small
+  return Alert(events[0]['message'], "", AlertStatus.normal, size, Priority.LOW, VisualAlert.none, AudibleAlert.none, 2.)
+
+
 class EventsSP(EventsBase):
   def __init__(self):
     super().__init__()
@@ -226,4 +236,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,
+  },
 }