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

@@ -313,6 +313,8 @@ class FrogPilotPlanner:
     frogpilotPlan.maxAcceleration = self.max_accel
     frogpilotPlan.minAcceleration = self.min_accel
 
+    frogpilotPlan.roadCurvature = self.road_curvature
+
     frogpilotPlan.slcOverridden = bool(self.override_slc)
     frogpilotPlan.slcOverriddenSpeed = float(self.overridden_speed)
     frogpilotPlan.slcSpeedLimit = self.slc_target

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;
@@ -325,6 +325,31 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
   painter.setBrush(bg);
   painter.drawPolygon(scene.track_vertices);
 
+  if (scene.show_stopping_point) {
+    bool curve_detected = sqrt(1.0 / scene.road_curvature) < v_ego;
+    bool following_lead = scene.has_lead && (scene.lead_distance < fmax(scene.model_length, 25));
+    bool model_stopping = scene.model_length < v_ego * (10 - 3);
+
+    if (model_stopping && !curve_detected && !following_lead) {
+      QPointF last_point = scene.track_vertices.last();
+
+      QPointF adjusted_point = last_point - QPointF(stopSignImg.width() / 2, stopSignImg.height());
+      painter.drawPixmap(adjusted_point, stopSignImg);
+
+      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);
@@ -514,7 +539,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);
@@ -581,6 +606,8 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() {
   bottom_layout->addWidget(map_settings_btn_bottom);
 
   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

@@ -50,6 +50,7 @@ void update_leads(UIState *s, const cereal::ModelDataV2::Reader &model_data) {
     const auto &lead = model_data.getLeadsV3()[i];
     if (s->scene.has_lead) {
       float d_rel = lead.getX()[0];
+      s->scene.lead_distance = d_rel;
       float y_rel = lead.getY()[0];
       float z = line.getZ()[get_path_length_idx(line, d_rel)];
       calib_frame_to_full_frame(s, d_rel, y_rel, z + 1.22, &s->scene.lead_vertices[i]);
@@ -84,6 +85,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 +253,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.road_curvature = frogpilotPlan.getRoadCurvature();
     scene.speed_limit = frogpilotPlan.getSlcSpeedLimit();
     scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset();
     scene.speed_limit_overridden = frogpilotPlan.getSlcOverridden();
@@ -322,6 +325,7 @@ void ui_update_frogpilot_params(UIState *s) {
   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.wheel_icon = custom_onroad_ui ? params.getInt("WheelIcon") : 0;
 
   scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing");

selfdrive/ui/ui.h

@@ -153,6 +153,7 @@ typedef struct UIScene {
   bool show_cem_status_bar;
   bool show_slc_offset;
   bool show_slc_offset_ui;
+  bool show_stopping_point;
   bool speed_limit_changed;
   bool speed_limit_controller;
   bool speed_limit_overridden;
@@ -170,6 +171,7 @@ typedef struct UIScene {
   float lane_width_left;
   float lane_width_right;
   float lead_detection_threshold;
+  float road_curvature;
   float speed_limit;
   float speed_limit_offset;
   float speed_limit_overridden_speed;
@@ -180,6 +182,8 @@ typedef struct UIScene {
   int conditional_speed;
   int conditional_speed_lead;
   int conditional_status;
+  int lead_distance;
+  int model_length;
   int steering_angle_deg;
   int tethering_config;
   int wheel_icon;