From 225ce45d31d2fddf269f17c23c40b6ae5a95a524 Mon Sep 17 00:00:00 2001 From: Nayan Date: Thu, 2 Oct 2025 13:53:23 -0400 Subject: [PATCH] feat: Lead Departure Alert (#1302) * init * fix * event * UI * events..for real * SP * ugh * toggles * read params first * stoopid it is * fix green light img * fix green light image. for real this time * move events to longitudinal_planner * move events to longitudinal_planner * move e2e alerts to separate class * fix * fixxxxxxx * blinky blink * blinky blink * refactor * more refactor --------- Co-authored-by: Kumar <36933347+rav4kumar@users.noreply.github.com> Co-authored-by: Jason Wen --- cereal/custom.capnp | 1 + common/params_keys.h | 1 + .../qt/offroad/settings/visuals_panel.cc | 10 +++++++ selfdrive/ui/sunnypilot/qt/onroad/hud.cc | 28 ++++++++++++------- selfdrive/ui/sunnypilot/qt/onroad/hud.h | 9 ++++-- .../selfdrive/assets/images/lead_depart.png | 3 ++ .../controls/lib/e2e_alerts_helper.py | 16 ++++++++--- .../controls/lib/longitudinal_planner.py | 1 + 8 files changed, 53 insertions(+), 16 deletions(-) create mode 100644 sunnypilot/selfdrive/assets/images/lead_depart.png diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 23e26a8ccb..44b8bd69fa 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -250,6 +250,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { struct E2eAlerts { greenLightAlert @0 :Bool; + leadDepartAlert @1 :Bool; } } diff --git a/common/params_keys.h b/common/params_keys.h index b8cda83693..0f5d9cf449 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -154,6 +154,7 @@ inline static std::unordered_map keys = { {"InteractivityTimeout", {PERSISTENT | BACKUP, INT, "0"}}, {"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}}, {"LastGPSPositionLLK", {PERSISTENT, STRING}}, + {"LeadDepartAlert", {PERSISTENT | BACKUP, BOOL, "0"}}, {"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}}, {"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}}, {"OffroadMode", {CLEAR_ON_MANAGER_START, BOOL}}, diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc index 07df99f726..7c1107d653 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc @@ -59,6 +59,16 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) { "", false, }, + { + "LeadDepartAlert", + tr("Lead Departure Alert (Beta)"), + QString("%1
" + "

%2


") + .arg(tr("A chime and on-screen alert will play when you are stopped, and the vehicle in front of you start moving.")) + .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 d05822bb85..607e0cbbb5 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc @@ -15,10 +15,12 @@ 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}); + int small_max = e2e_alert_small * 2 - 40; + int large_max = e2e_alert_large * 2 - 40; + green_light_alert_small_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/green_light.png", {small_max, small_max}); + green_light_alert_large_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/green_light.png", {large_max, large_max}); + lead_depart_alert_small_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/lead_depart.png", {small_max, small_max}); + lead_depart_alert_large_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/lead_depart.png", {large_max, large_max}); } void HudRendererSP::updateState(const UIState &s) { @@ -112,6 +114,7 @@ void HudRendererSP::updateState(const UIState &s) { smartCruiseControlMapActive = lp_sp.getSmartCruiseControl().getMap().getActive(); greenLightAlert = lp_sp.getE2eAlerts().getGreenLightAlert(); + leadDepartAlert = lp_sp.getE2eAlerts().getLeadDepartAlert(); } void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { @@ -204,13 +207,21 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { // Road Name drawRoadName(p, surface_rect); - // Green Light Alert - if (greenLightAlert) { + // Green Light & Lead Depart Alerts + if (greenLightAlert or leadDepartAlert) { e2eAlertDisplayTimer = 3 * UI_FREQ; } if (e2eAlertDisplayTimer > 0) { e2eAlertFrame++; + if (greenLightAlert) { + alert_text = tr("GREEN\nLIGHT"); + alert_img = devUiInfo > 0 ? green_light_alert_small_img : green_light_alert_large_img; + } + else if (leadDepartAlert) { + alert_text = tr("LEAD VEHICLE\nDEPARTING"); + alert_img = devUiInfo > 0 ? lead_depart_alert_small_img : lead_depart_alert_large_img; + } drawE2eAlert(p, surface_rect); } else { e2eAlertFrame = 0; @@ -689,15 +700,13 @@ void HudRendererSP::drawSetSpeedSP(QPainter &p, const QRect &surface_rect) { } void HudRendererSP::drawE2eAlert(QPainter &p, const QRect &surface_rect) { - int size = devUiInfo > 0 ? green_light_alert_small : green_light_alert_large; + int size = devUiInfo > 0 ? e2e_alert_small : e2e_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); @@ -716,7 +725,6 @@ void HudRendererSP::drawE2eAlert(QPainter &p, const QRect &surface_rect) { 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 b911167241..cf840ee3d0 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.h @@ -94,11 +94,16 @@ 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; + int e2e_alert_small = 250; + int e2e_alert_large = 300; QPixmap green_light_alert_small_img; QPixmap green_light_alert_large_img; bool greenLightAlert; int e2eAlertFrame; int e2eAlertDisplayTimer = 0; + bool leadDepartAlert; + QPixmap lead_depart_alert_small_img; + QPixmap lead_depart_alert_large_img; + QString alert_text; + QPixmap alert_img; }; diff --git a/sunnypilot/selfdrive/assets/images/lead_depart.png b/sunnypilot/selfdrive/assets/images/lead_depart.png new file mode 100644 index 0000000000..6030ee67cf --- /dev/null +++ b/sunnypilot/selfdrive/assets/images/lead_depart.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:087db35bd469e85aefe3b45636f11ab3e8b55ceb7bc94ea059cfd9a69c2f338f +size 8914 diff --git a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py index 46657bd47c..eedf8bccbb 100644 --- a/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py +++ b/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py @@ -22,17 +22,20 @@ class E2EAlertsHelper: self.green_light_alert = False self.green_light_alert_enabled = self._params.get_bool("GreenLightAlert") + self.lead_depart_alert = False + self.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert") 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.lead_depart_alert_enabled = self._params.get_bool("LeadDepartAlert") self._frame += 1 def update(self, sm: messaging.SubMaster, events_sp: EventsSP) -> None: self._read_params() - if not self.green_light_alert_enabled: + if not (self.green_light_alert_enabled or self.lead_depart_alert_enabled): return CS = sm['carState'] @@ -41,10 +44,15 @@ class E2EAlertsHelper: model_x = sm['modelV2'].position.x max_idx = len(model_x) - 1 has_lead = sm['radarState'].leadOne.status + lead_vRel: float = sm['radarState'].leadOne.vRel # 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 + self.green_light_alert = (self.green_light_alert_enabled and 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: + # Lead Departure Alert + self.lead_depart_alert = (self.lead_depart_alert_enabled and CS.standstill and model_x[max_idx] > 30 + and has_lead and lead_vRel > 1 and not CS.gasPressed) + + if self.green_light_alert or 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 e45d748f3b..4c2443f253 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -140,5 +140,6 @@ class LongitudinalPlannerSP: # E2E Alerts e2eAlerts = longitudinalPlanSP.e2eAlerts e2eAlerts.greenLightAlert = self.e2e_alerts_helper.green_light_alert + e2eAlerts.leadDepartAlert = self.e2e_alerts_helper.lead_depart_alert pm.send('longitudinalPlanSP', plan_sp_send)