Visuals - Custom Onroad UI - Stopping Points

Display the point where openpilot wants to stop for red lights/stop signs.

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -94,7 +94,7 @@ class FrogPilotPlanner:
     lead_distance = self.lead_one.dRel - distance_offset
     stopping_distance = STOP_DISTANCE + distance_offset
 
-    run_cem = frogpilot_toggles.conditional_experimental_mode or frogpilot_toggles.force_stops
+    run_cem = frogpilot_toggles.conditional_experimental_mode or frogpilot_toggles.force_stops or frogpilot_toggles.show_stopping_point
     if run_cem and (controlsState.enabled or frogpilotCarControl.alwaysOnLateral) and driving_gear:
       self.cem.update(carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles)
 
@@ -331,6 +331,7 @@ class FrogPilotPlanner:
     frogpilotPlan.forcingStop = self.forcing_stop
 
     frogpilotPlan.greenLight = not self.model_stopped
+    frogpilotPlan.redLight = self.cem.stop_light_detected
 
     frogpilotPlan.laneWidthLeft = self.lane_width_left
     frogpilotPlan.laneWidthRight = self.lane_width_right

selfdrive/ui/qt/onroad/annotated_camera.cc

@@ -269,7 +269,7 @@ void AnnotatedCameraWidget::updateFrameMat() {
       .translate(-intrinsic_matrix.v[2], -intrinsic_matrix.v[5]);
 }
 
-void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
+void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s, const float v_ego) {
   painter.save();
 
   const UIScene &scene = s->scene;
@@ -326,6 +326,27 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
   painter.setBrush(bg);
   painter.drawPolygon(scene.track_vertices);
 
+  if (scene.show_stopping_point && scene.red_light && v_ego > 1 && !(conditionalStatus == 1 || conditionalStatus == 3 || conditionalStatus == 5)) {
+    QPointF last_point = scene.track_vertices.last();
+
+    QPointF adjusted_point = last_point - QPointF(stopSignImg.width() / 2, stopSignImg.height());
+    painter.drawPixmap(adjusted_point, stopSignImg);
+
+    if (scene.show_stopping_point_metrics) {
+      QString text = QString::number(scene.model_length * distanceConversion) + leadDistanceUnit;
+      QFont font = InterFont(35, QFont::DemiBold);
+      QFontMetrics fm(font);
+      int text_width = fm.horizontalAdvance(text);
+      QPointF text_position = last_point - QPointF(text_width / 2, stopSignImg.height() + 35);
+
+      painter.save();
+      painter.setFont(font);
+      painter.setPen(Qt::white);
+      painter.drawText(text_position, text);
+      painter.restore();
+    }
+  }
+
   // Paint blindspot path
   if (scene.blind_spot_path) {
     QLinearGradient bs(0, height(), 0, 0);
@@ -519,7 +540,7 @@ void AnnotatedCameraWidget::paintGL() {
 
   if (s->scene.world_objects_visible) {
     update_model(s, model, sm["uiPlan"].getUiPlan());
-    drawLaneLines(painter, s);
+    drawLaneLines(painter, s, v_ego);
 
     if (s->scene.longitudinal_control && sm.rcv_frame("modelV2") > s->scene.started_frame) {
       update_leads(s, model);
@@ -585,6 +606,8 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() {
   bottom_layout->addWidget(map_settings_btn_bottom, 0, Qt::AlignBottom | Qt::AlignRight);
 
   main_layout->addLayout(bottom_layout);
+
+  stopSignImg = loadPixmap("../frogpilot/assets/other_images/stop_sign.png", QSize(img_size, img_size));
 }
 
 void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UIScene &scene) {

selfdrive/ui/qt/onroad/annotated_camera.h

@@ -137,6 +137,8 @@ private:
   int conditionalSpeedLead;
   int conditionalStatus;
 
+  QPixmap stopSignImg;
+
   QString accelerationUnit;
   QString leadDistanceUnit;
   QString leadSpeedUnit;
@@ -149,7 +151,7 @@ protected:
   void initializeGL() override;
   void showEvent(QShowEvent *event) override;
   void updateFrameMat() override;
-  void drawLaneLines(QPainter &painter, const UIState *s);
+  void drawLaneLines(QPainter &painter, const UIState *s, const float v_ego);
   void drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd, const float v_ego);
   void drawHud(QPainter &p);
   void drawDriverState(QPainter &painter, const UIState *s);

selfdrive/ui/ui.cc

@@ -84,6 +84,7 @@ void update_model(UIState *s,
                   const cereal::UiPlan::Reader &plan) {
   UIScene &scene = s->scene;
   auto plan_position = plan.getPosition();
+  scene.model_length = model.getPosition().getX()[33 - 1];
   if (plan_position.getX().size() < model.getPosition().getX().size()) {
     plan_position = model.getPosition();
   }
@@ -251,6 +252,7 @@ static void update_state(UIState *s) {
     scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise();
     scene.lane_width_left = frogpilotPlan.getLaneWidthLeft();
     scene.lane_width_right = frogpilotPlan.getLaneWidthRight();
+    scene.red_light = frogpilotPlan.getRedLight();
     scene.speed_limit = frogpilotPlan.getSlcSpeedLimit();
     scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset();
     scene.speed_limit_overridden = frogpilotPlan.getSlcOverridden();
@@ -323,6 +325,8 @@ void ui_update_frogpilot_params(UIState *s, Params &params) {
   scene.static_pedals_on_ui = scene.pedals_on_ui && params.getBool("StaticPedalsOnUI");
   scene.road_name_ui = custom_onroad_ui && params.getBool("RoadNameUI");
   scene.rotating_wheel = custom_onroad_ui && params.getBool("RotatingWheel");
+  scene.show_stopping_point = custom_onroad_ui && params.getBool("ShowStoppingPoint");
+  scene.show_stopping_point_metrics = scene.show_stopping_point && params.getBool("ShowStoppingPointMetrics");
   scene.wheel_icon = custom_onroad_ui ? params.getInt("WheelIcon") : 0;
 
   scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing");

selfdrive/ui/ui.h

@@ -145,6 +145,7 @@ typedef struct UIScene {
   bool onroad_distance_button;
   bool parked;
   bool pedals_on_ui;
+  bool red_light;
   bool reverse;
   bool reverse_cruise;
   bool reverse_cruise_ui;
@@ -155,6 +156,8 @@ typedef struct UIScene {
   bool show_cem_status_bar;
   bool show_slc_offset;
   bool show_slc_offset_ui;
+  bool show_stopping_point;
+  bool show_stopping_point_metrics;
   bool speed_limit_changed;
   bool speed_limit_controller;
   bool speed_limit_overridden;
@@ -182,6 +185,7 @@ typedef struct UIScene {
   int conditional_speed;
   int conditional_speed_lead;
   int conditional_status;
+  int model_length;
   int steering_angle_deg;
   int tethering_config;
   int wheel_icon;