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 <haibin.wen3@gmail.com>
2026-02-18 20:03:53 +08:00 · 2025-10-02 13:53:23 -04:00
parent 92214b69d8
commit 225ce45d31
8 changed files with 53 additions and 16 deletions
--- a/cereal/custom.capnp
+++ b/cereal/custom.capnp
@@ -250,6 +250,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {

  struct E2eAlerts {
    greenLightAlert @0 :Bool;
+    leadDepartAlert @1 :Bool;
  }
 }

--- a/common/params_keys.h
+++ b/common/params_keys.h
@@ -154,6 +154,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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}},
--- 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<br>"
+        "<h4>%2</h4><br>")
+        .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
--- 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);
--- 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;
 };
--- a/sunnypilot/selfdrive/assets/images/lead_depart.png
+++ 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
--- 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)
--- 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)