ui: show lead vEgo or distance with chevron (#46)

* ui: show vEgo or lead distance with chevron

* fix

selfdrive/ui/qt/offroad/sunnypilot_settings.cc

@@ -664,6 +664,10 @@ SPVisualsPanel::SPVisualsPanel(QWidget *parent) : QWidget(parent) {
     "",
     "../assets/offroad/icon_openpilot.png"
   ));
+
+  // Visuals: Display Metrics above Chevron
+  main_layout->addWidget(horizontal_line());
+  main_layout->addWidget(new ChevronInfo());
 }
 
 void SPVisualsPanel::showEvent(QShowEvent *event) {
@@ -1761,3 +1765,74 @@ void DevUiRow::refresh() {
   btnminus.setText("-");
   btnplus.setText("+");
 }
+
+// Display Metrics above Chevron
+ChevronInfo::ChevronInfo() : AbstractControl(
+  tr("Display Metrics above Chevron"),
+  tr("Display useful metrics above the chevron that tracks the lead car (only applicable to cars with openpilot longitudinal control)."),
+  "../assets/offroad/icon_calibration.png")
+
+{
+  label.setAlignment(Qt::AlignVCenter|Qt::AlignRight);
+  label.setStyleSheet("color: #e0e879");
+  hlayout->addWidget(&label);
+
+  btnminus.setStyleSheet(R"(
+    padding: 0;
+    border-radius: 50px;
+    font-size: 50px;
+    font-weight: 500;
+    color: #E4E4E4;
+    background-color: #393939;
+  )");
+  btnplus.setStyleSheet(R"(
+    padding: 0;
+    border-radius: 50px;
+    font-size: 50px;
+    font-weight: 500;
+    color: #E4E4E4;
+    background-color: #393939;
+  )");
+  btnminus.setFixedSize(150, 100);
+  btnplus.setFixedSize(150, 100);
+  hlayout->addWidget(&btnminus);
+  hlayout->addWidget(&btnplus);
+
+  QObject::connect(&btnminus, &QPushButton::clicked, [=]() {
+    auto str = QString::fromStdString(params.get("ChevronInfo"));
+    int value = str.toInt();
+    value = value - 1;
+    if (value <= 0 ) {
+      value = 0;
+    }
+    QString values = QString::number(value);
+    params.put("ChevronInfo", values.toStdString());
+    refresh();
+  });
+
+  QObject::connect(&btnplus, &QPushButton::clicked, [=]() {
+    auto str = QString::fromStdString(params.get("ChevronInfo"));
+    int value = str.toInt();
+    value = value + 1;
+    if (value >= 2 ) {
+      value = 2;
+    }
+    QString values = QString::number(value);
+    params.put("ChevronInfo", values.toStdString());
+    refresh();
+  });
+  refresh();
+}
+
+void ChevronInfo::refresh() {
+  QString option = QString::fromStdString(params.get("ChevronInfo"));
+  if (option == "0") {
+    label.setText(tr("OFF"));
+  } else if (option == "1") {
+    label.setText(tr("Distance"));
+  } else if (option == "2") {
+    label.setText(tr("Speed"));
+  }
+  btnminus.setText("-");
+  btnplus.setText("+");
+}

selfdrive/ui/qt/offroad/sunnypilot_settings.h

@@ -337,3 +337,18 @@ private:
 
   void refresh();
 };
+
+class ChevronInfo : public AbstractControl {
+  Q_OBJECT
+
+public:
+  ChevronInfo();
+
+private:
+  QPushButton btnplus;
+  QPushButton btnminus;
+  QLabel label;
+  Params params;
+
+  void refresh();
+};

selfdrive/ui/qt/onroad.cc

@@ -1093,7 +1093,8 @@ void AnnotatedCameraWidget::rocketFuel(QPainter &p) {
   }
 }
 
-void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd) {
+void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd,
+                                     int num, float radar_d_rel, float v_ego, float radar_v_rel, int chevron_data, bool isMetric) {
   painter.save();
 
   const float speedBuff = 10.;
@@ -1117,15 +1118,35 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState
   float g_xo = sz / 5;
   float g_yo = sz / 10;
 
-  QPointF glow[] = {{x + (sz * 1.35) + g_xo, y + sz + g_yo}, {x, y - g_yo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo}};
+  float homebase_h = 12;
+  QPointF glow[] = {{x + (sz * 1.35) + g_xo, y + sz + g_yo + homebase_h}, {x + (sz * 1.35) + g_xo, y + sz + g_yo},
+                   {x, y - g_yo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo + homebase_h}};
   painter.setBrush(QColor(218, 202, 37, 255));
   painter.drawPolygon(glow, std::size(glow));
 
   // chevron
-  QPointF chevron[] = {{x + (sz * 1.25), y + sz}, {x, y}, {x - (sz * 1.25), y + sz}};
+  QPointF chevron[] = {{x + (sz * 1.25), y + sz + homebase_h}, {x + (sz * 1.25), y + sz}, {x, y}, {x - (sz * 1.25), y + sz},
+                      {x - (sz * 1.25), y + sz + homebase_h}};
   painter.setBrush(redColor(fillAlpha));
   painter.drawPolygon(chevron, std::size(chevron));
 
+  if (num == 0) { // Display the distance to the 0th lead car
+    QString dist = "";
+    if (chevron_data == 1) dist = QString::number(radar_d_rel,'f', 1) + "m";
+    else if (chevron_data == 2) {
+      if (isMetric) dist = QString::number((radar_v_rel + v_ego) * 3.6,'f', 0) + "km/h";
+      else dist = QString::number((radar_v_rel + v_ego) * 2.236936,'f', 0) + "mph";
+    }
+    int str_w = 200;
+    configFont(painter, "Inter", 44, "SemiBold");
+    painter.setPen(QColor(0x0, 0x0, 0x0 , 200)); // Shadow
+    float lock_indicator_dx = 2; // Avoid downward cross sights
+    painter.drawText(QRect(x + 2 + lock_indicator_dx, y - 50 + 2, str_w, 50), Qt::AlignBottom | Qt::AlignLeft, dist);
+    painter.setPen(QColor(0xff, 0xff, 0xff));
+    painter.drawText(QRect(x + lock_indicator_dx, y - 50, str_w, 50), Qt::AlignBottom | Qt::AlignLeft, dist);
+    painter.setPen(Qt::NoPen);
+  }
+
   painter.restore();
 }
 
@@ -1200,11 +1221,14 @@ void AnnotatedCameraWidget::paintEvent(QPaintEvent *event) {
     if (s->scene.longitudinal_control) {
       auto lead_one = radar_state.getLeadOne();
       auto lead_two = radar_state.getLeadTwo();
+      float v_ego = sm["carState"].getCarState().getVEgo();
+      float radar_d_rel = radar_state.getLeadOne().getDRel();
+      float radar_v_rel = radar_state.getLeadOne().getVRel();
       if (lead_one.getStatus()) {
-        drawLead(painter, lead_one, s->scene.lead_vertices[0]);
+        drawLead(painter, lead_one, s->scene.lead_vertices[0], 0, radar_d_rel, v_ego, radar_v_rel, s->scene.chevron_data, s->scene.is_metric);
       }
       if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) {
-        drawLead(painter, lead_two, s->scene.lead_vertices[1]);
+        drawLead(painter, lead_two, s->scene.lead_vertices[1], 1, radar_d_rel, v_ego, radar_v_rel, s->scene.chevron_data, s->scene.is_metric);
       }
 
       rocketFuel(painter);

selfdrive/ui/qt/onroad.h

@@ -198,7 +198,7 @@ protected:
   void showEvent(QShowEvent *event) override;
   void updateFrameMat() override;
   void drawLaneLines(QPainter &painter, const UIState *s);
-  void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd);
+  void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd, int num, float radar_d_rel, float v_ego, float radar_v_rel, int chevron_data, bool isMetric);
   void drawHud(QPainter &p);
   void drawDriverState(QPainter &painter, const UIState *s);
   void paintEvent(QPaintEvent *event) override;

selfdrive/ui/ui.cc

@@ -235,6 +235,7 @@ void ui_update_params(UIState *s) {
   s->scene.speed_limit_style = std::atoi(params.get("SpeedLimitStyle").c_str());
   s->scene.hide_vego_ui = params.getBool("HideVEgoUi");
   s->scene.true_vego_ui = params.getBool("TrueVEgoUi");
+  s->scene.chevron_data = std::atoi(params.get("ChevronInfo").c_str());
 
   if (s->scene.onroadScreenOff > 0) {
     s->scene.osoTimer = s->scene.onroadScreenOff * 60 * UI_FREQ;

selfdrive/ui/ui.h

@@ -164,6 +164,8 @@ typedef struct UIScene {
   bool stand_still_timer;
 
   bool hide_vego_ui, true_vego_ui;
+
+  int chevron_data;
 } UIScene;
 
 class UIState : public QObject {