feat: Green Traffic Light Alert (#1287)

* 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

* green light alert only for this PR

* fix

* fixxxxxxx

* blinky blink

* blinky blink

* slight cleanup

* only used for params

* a bit more

* only when long is not engaged

* too long

* update description

* always 3 seconds if not moving

* initialize in constructor instead

* less

* rename

* always init at 0

---------

Co-authored-by: Kumar <36933347+rav4kumar@users.noreply.github.com>
Co-authored-by: Jason Wen <haibin.wen3@gmail.com>

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;
   }
 }
 

common/params_keys.h

@@ -148,6 +148,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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"}},

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<br>"
+        "<h4>%2</h4><br>")
+        .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

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);
+}

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;
 };

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)

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)

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),
+  },
 }