Display the model's stopping point

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc

@@ -7,6 +7,7 @@ FrogPilotAnnotatedCameraWidget::FrogPilotAnnotatedCameraWidget(QWidget *parent)
 
   curveSpeedIcon = loadPixmap("../../frogpilot/assets/other_images/curve_speed.png", {btn_size, btn_size});
   curveSpeedIconFlipped = curveSpeedIcon.transformed(QTransform().scale(-1, 1));
+  stopSignImg = loadPixmap("../../frogpilot/assets/other_images/stop_sign.png", {btn_size, btn_size});
 
   loadGif("../../frogpilot/assets/other_images/curve_icon.gif", cemCurveIcon, QSize(widget_size, widget_size), this);
   loadGif("../../frogpilot/assets/other_images/lead_icon.gif", cemLeadIcon, QSize(widget_size, widget_size), this);
@@ -148,8 +149,10 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil
   cscSpeed = frogpilotPlan.getCscSpeed();
   cscTraining = frogpilotPlan.getCscTraining();
   experimentalMode = selfdriveState.getExperimentalMode();
+  redLight = frogpilotPlan.getRedLight();
   roadCurvature = frogpilotPlan.getRoadCurvature();
   roadName = QString::fromStdString(params_memory.get("RoadName"));
+  stoppingDistance = modelV2.getPosition().getX().size() > 33 - 1 ? modelV2.getPosition().getX()[33 - 1] : 0.0;
 
   hideBottomIcons = selfdriveState.getAlertSize() != cereal::SelfdriveState::AlertSize::NONE;
   hideBottomIcons |= frogpilotSelfdriveState.getAlertSize() != cereal::FrogPilotSelfdriveState::AlertSize::NONE;
@@ -225,6 +228,10 @@ void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState
     paintStandstillTimer(p);
   }
 
+  if (track_vertices.length() >= 1 && redLight && frogpilot_toggles.value("show_stopping_point").toBool()) {
+    paintStoppingPoint(p);
+  }
+
   if ((blinkerLeft || blinkerRight) && signalStyle != "None" && (standstillDuration == 0 || signalStyle != "static")) {
     paintTurnSignals(p);
   }
@@ -512,6 +519,34 @@ void FrogPilotAnnotatedCameraWidget::paintStandstillTimer(QPainter &p) {
   p.restore();
 }
 
+void FrogPilotAnnotatedCameraWidget::paintStoppingPoint(QPainter &p) {
+  p.save();
+
+  QPointF centerPoint = (track_vertices.first() + track_vertices.last()) / 2.0f;
+  QPointF stopSignPosition = centerPoint - QPointF(stopSignImg.width() / 2.0f, stopSignImg.height());
+  p.drawPixmap(stopSignPosition, stopSignImg);
+
+  if (frogpilot_toggles.value("show_stopping_point_metrics").toBool()) {
+    float distance = stoppingDistance * distanceConversion;
+    QString distanceText = QString::number(std::nearbyint(distance)) + leadDistanceUnit;
+
+    QFont font = InterFont(45, QFont::DemiBold);
+    QFontMetrics fm(font);
+
+    QPointF textPosition(centerPoint.x() - fm.horizontalAdvance(distanceText) / 2.0f, centerPoint.y() - stopSignImg.height() - fm.ascent());
+
+    QPainterPath path;
+    path.addText(textPosition, font, distanceText);
+    p.strokePath(path, QPen(Qt::black, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
+
+    p.setFont(font);
+    p.setPen(whiteColor());
+    p.drawText(textPosition, distanceText);
+  }
+
+  p.restore();
+}
+
 void FrogPilotAnnotatedCameraWidget::paintTurnSignals(QPainter &p) {
   int frameIndex = qBound(0, animationFrameIndex, totalFrames - 1);
 

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h

@@ -52,6 +52,7 @@ private:
   void paintCurveSpeedControlTraining(QPainter &p);
   void paintRoadName(QPainter &p);
   void paintStandstillTimer(QPainter &p);
+  void paintStoppingPoint(QPainter &p);
   void paintTurnSignals(QPainter &p);
   void updateSignals();
 
@@ -62,6 +63,7 @@ private:
   bool cscControllingSpeed;
   bool cscTraining;
   bool experimentalMode;
+  bool redLight;
 
   int animationFrameIndex;
   int frogHopCount;
@@ -77,6 +79,7 @@ private:
   float setSpeed;
   float speedConversion;
   float speedConversionMetrics;
+  float stoppingDistance;
 
   Params params;
   Params params_memory{"", true};
@@ -89,6 +92,7 @@ private:
 
   QPixmap curveSpeedIcon;
   QPixmap curveSpeedIconFlipped;
+  QPixmap stopSignImg;
 
   QPoint cemStatusPosition;
   QPoint compassPosition;