diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 674f109fc..23e26a8cc 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -149,6 +149,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { vTarget @4 :Float32; aTarget @5 :Float32; events @6 :List(OnroadEventSP.Event); + e2eAlerts @7 :E2eAlerts; struct DynamicExperimentalControl { state @0 :DynamicExperimentalControlState; @@ -246,6 +247,10 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { sccMap @2; speedLimitAssist @3; } + + struct E2eAlerts { + greenLightAlert @0 :Bool; + } } struct OnroadEventSP @0xda96579883444c35 { @@ -291,6 +296,7 @@ struct OnroadEventSP @0xda96579883444c35 { speedLimitActive @20; speedLimitChanged @21; speedLimitPending @22; + e2eChime @23; } } diff --git a/common/params_keys.h b/common/params_keys.h index 1056a3903..b8cda8369 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -148,6 +148,7 @@ inline static std::unordered_map keys = { {"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}}, {"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}}, {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}}, + {"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}}, {"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}}, {"IntelligentCruiseButtonManagement", {PERSISTENT | BACKUP , BOOL}}, {"InteractivityTimeout", {PERSISTENT | BACKUP, INT, "0"}}, diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc index 10ead5377..07df99f72 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc @@ -49,6 +49,16 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) { "", false, }, + { + "GreenLightAlert", + tr("Green Traffic Light Alert (Beta)"), + QString("%1
" + "

%2


") + .arg(tr("A chime and on-screen alert will play when the traffic light you are waiting for turns green and you have no vehicle in front of you.")) + .arg(tr("Note: This chime is only designed as a notification. It is the driver's responsibility to observe their environment and make decisions accordingly.")), + "", + false, + }, }; // Add regular toggles first diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index df1bc3c00..d05822bb8 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc @@ -14,6 +14,11 @@ HudRendererSP::HudRendererSP() { plus_arrow_up_img = loadPixmap("../../sunnypilot/selfdrive/assets/img_plus_arrow_up", {105, 105}); minus_arrow_down_img = loadPixmap("../../sunnypilot/selfdrive/assets/img_minus_arrow_down", {105, 105}); + + int green_light_small_max = green_light_alert_small * 2 - 40; + int green_light_large_max = green_light_alert_large * 2 - 40; + green_light_alert_small_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/green_light.png", {green_light_small_max, green_light_small_max}); + green_light_alert_large_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/green_light.png", {green_light_large_max, green_light_large_max}); } void HudRendererSP::updateState(const UIState &s) { @@ -105,11 +110,15 @@ void HudRendererSP::updateState(const UIState &s) { smartCruiseControlVisionActive = lp_sp.getSmartCruiseControl().getVision().getActive(); smartCruiseControlMapEnabled = lp_sp.getSmartCruiseControl().getMap().getEnabled(); smartCruiseControlMapActive = lp_sp.getSmartCruiseControl().getMap().getActive(); + + greenLightAlert = lp_sp.getE2eAlerts().getGreenLightAlert(); } void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { HudRenderer::draw(p, surface_rect); + e2eAlertDisplayTimer = std::max(0, e2eAlertDisplayTimer - 1); + p.save(); if (is_cruise_available) { @@ -194,6 +203,18 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { // Road Name drawRoadName(p, surface_rect); + + // Green Light Alert + if (greenLightAlert) { + e2eAlertDisplayTimer = 3 * UI_FREQ; + } + + if (e2eAlertDisplayTimer > 0) { + e2eAlertFrame++; + drawE2eAlert(p, surface_rect); + } else { + e2eAlertFrame = 0; + } } p.restore(); @@ -666,3 +687,37 @@ void HudRendererSP::drawSetSpeedSP(QPainter &p, const QRect &surface_rect) { p.setPen(set_speed_color); p.drawText(set_speed_rect.adjusted(0, 77, 0, 0), Qt::AlignTop | Qt::AlignHCenter, setSpeedStr); } + +void HudRendererSP::drawE2eAlert(QPainter &p, const QRect &surface_rect) { + int size = devUiInfo > 0 ? green_light_alert_small : green_light_alert_large; + int x = surface_rect.center().x() + surface_rect.width() / 4; + int y = surface_rect.center().y() + 40; + x += devUiInfo > 0 ? 0 : 50; + y += devUiInfo > 0 ? 0 : 80; + QRect alertRect(x - size, y - size, size * 2, size * 2); + + QString alert_text = tr("GREEN\nLIGHT"); + + // Alert Circle + QPoint center = alertRect.center(); + QColor frameColor = pulseElement(e2eAlertFrame) ? QColor(255, 255, 255, 75) : QColor(0, 255, 0, 75); + p.setPen(QPen(frameColor, 15)); + p.setBrush(QColor(0, 0, 0, 190)); + p.drawEllipse(center, size, size); + + // Alert Text + QColor txtColor = pulseElement(e2eAlertFrame) ? QColor(255, 255, 255, 255) : QColor(0, 255, 0, 255); + p.setFont(InterFont(48, QFont::Bold)); + p.setPen(txtColor); + QFontMetrics fm(p.font()); + QRect textRect = fm.boundingRect(alertRect, Qt::TextWordWrap, alert_text); + textRect.moveCenter({alertRect.center().x(), alertRect.center().y()}); + textRect.moveBottom(alertRect.bottom() - alertRect.height() / 7); + p.drawText(textRect, Qt::AlignCenter, alert_text); + + // Alert Image + QPixmap &alert_img = devUiInfo > 0 ? green_light_alert_small_img : green_light_alert_large_img; + QPointF pixmapCenterOffset = QPointF(alert_img.width() / 2.0, alert_img.height() / 2.0); + QPointF drawPoint = center - pixmapCenterOffset; + p.drawPixmap(drawPoint, alert_img); +} diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h index 043536bf4..b91116724 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.h @@ -36,6 +36,7 @@ private: void drawRoadName(QPainter &p, const QRect &surface_rect); void drawSpeedLimitPreActiveArrow(QPainter &p, QRect &sign_rect); void drawSetSpeedSP(QPainter &p, const QRect &surface_rect); + void drawE2eAlert(QPainter &p, const QRect &surface_rect); bool lead_status; float lead_d_rel; @@ -93,4 +94,11 @@ private: int speedLimitAssistFrame; QPixmap plus_arrow_up_img; QPixmap minus_arrow_down_img; + int green_light_alert_small = 250; + int green_light_alert_large = 300; + QPixmap green_light_alert_small_img; + QPixmap green_light_alert_large_img; + bool greenLightAlert; + int e2eAlertFrame; + int e2eAlertDisplayTimer = 0; }; diff --git a/sunnypilot/selfdrive/assets/images/green_light.png b/sunnypilot/selfdrive/assets/images/green_light.png new file mode 100644 index 000000000..2da2c13a8 --- /dev/null +++ b/sunnypilot/selfdrive/assets/images/green_light.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3aa5ec9ac1daee6a549e62647d90bcaa66d2485f7df7f386ff902fcfb04c1716 +size 6583 diff --git a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py new file mode 100644 index 000000000..46657bd47 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py @@ -0,0 +1,50 @@ +""" +Copyright (c) 2021-, 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 messaging, custom + +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD +from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP + +TRIGGER_THRESHOLD = 30 + + +class E2EAlertsHelper: + def __init__(self): + self._params = Params() + self._frame = -1 + + self.green_light_alert = False + self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert") + + def _read_params(self) -> None: + if self._frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: + self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert") + + self._frame += 1 + + def update(self, sm: messaging.SubMaster, events_sp: EventsSP) -> None: + self._read_params() + + if not self.green_light_alert_enabled: + return + + CS = sm['carState'] + CC = sm['carControl'] + + model_x = sm['modelV2'].position.x + max_idx = len(model_x) - 1 + has_lead = sm['radarState'].leadOne.status + + # Green light alert + self.green_light_alert = model_x[max_idx] > TRIGGER_THRESHOLD and \ + not has_lead and CS.standstill and not CS.gasPressed and not CC.enabled + + if self.green_light_alert: + events_sp.add(custom.OnroadEventSP.EventName.e2eChime) diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index ece2ff241..e45d748f3 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -10,6 +10,7 @@ from opendbc.car import structs from openpilot.common.constants import CV from openpilot.selfdrive.car.cruise import V_CRUISE_MAX from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController +from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver @@ -30,6 +31,7 @@ class LongitudinalPlannerSP: self.sla = SpeedLimitAssist(CP) self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None self.source = LongitudinalPlanSource.cruise + self.e2e_alerts_helper = E2EAlertsHelper() self.output_v_target = 0. self.output_a_target = 0. @@ -53,8 +55,6 @@ class LongitudinalPlannerSP: long_enabled = sm['carControl'].enabled long_override = sm['carControl'].cruiseControl.override - self.events_sp.clear() - # Smart Cruise Control self.scc.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise) @@ -78,7 +78,9 @@ class LongitudinalPlannerSP: return self.output_v_target, self.output_a_target def update(self, sm: messaging.SubMaster) -> None: + self.events_sp.clear() self.dec.update(sm) + self.e2e_alerts_helper.update(sm, self.events_sp) def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: plan_sp_send = messaging.new_message('longitudinalPlanSP') @@ -135,4 +137,8 @@ class LongitudinalPlannerSP: assist.vTarget = float(self.sla.output_v_target) assist.aTarget = float(self.sla.output_a_target) + # E2E Alerts + e2eAlerts = longitudinalPlanSP.e2eAlerts + e2eAlerts.greenLightAlert = self.e2e_alerts_helper.green_light_alert + pm.send('longitudinalPlanSP', plan_sp_send) diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index 7700550c2..e9f4b29ba 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/sunnypilot/selfdrive/selfdrived/events.py @@ -224,4 +224,12 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = { AlertStatus.normal, AlertSize.small, Priority.LOW, VisualAlert.none, AudibleAlert.none, 5.), }, + + EventNameSP.e2eChime: { + ET.PERMANENT: Alert( + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.MID, VisualAlert.none, AudibleAlert.prompt, 0.1), + }, }