diff --git a/cereal/custom.capnp b/cereal/custom.capnp index abb9099fc7..a831d630b5 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -340,6 +340,7 @@ struct OnroadEventSP @0xda96579883444c35 { speedLimitChanged @21; speedLimitPending @22; e2eChime @23; + navigationBanner @24; } } diff --git a/sunnypilot/navd/event_builder.py b/sunnypilot/navd/event_builder.py new file mode 100644 index 0000000000..81556c08a5 --- /dev/null +++ b/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()}") diff --git a/sunnypilot/navd/helpers.py b/sunnypilot/navd/helpers.py index 8788653380..7bfa3c3a54 100644 --- a/sunnypilot/navd/helpers.py +++ b/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'] diff --git a/sunnypilot/navd/tests/test_event_builder.py b/sunnypilot/navd/tests/test_event_builder.py new file mode 100644 index 0000000000..bd7b872fa8 --- /dev/null +++ b/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 diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index 4edc0bd470..6e790a2a57 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/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, + }, }