diff --git a/frogpilot/assets/other_images/stop_sign.png b/frogpilot/assets/other_images/stop_sign.png new file mode 100644 index 000000000..159873301 Binary files /dev/null and b/frogpilot/assets/other_images/stop_sign.png differ diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc index 4014a3103..80c3f97d7 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc +++ b/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); diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h index 0eac28b0d..ac89f8915 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h +++ b/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;