Lead Info

frogpilot/controls/frogpilot_planner.py

@@ -130,6 +130,8 @@ class FrogPilotPlanner:
     frogpilotPlan.cscSpeed = float(self.frogpilot_vcruise.csc_target)
     frogpilotPlan.cscTraining = self.frogpilot_vcruise.csc.enable_training
 
+    frogpilotPlan.desiredFollowDistance = int(self.frogpilot_following.desired_follow_distance)
+
     frogpilotPlan.experimentalMode = self.frogpilot_cem.experimental_mode
 
     frogpilotPlan.forcingStop = self.frogpilot_vcruise.forcing_stop

frogpilot/controls/lib/frogpilot_following.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 import numpy as np
 
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, LEAD_DANGER_FACTOR, STOP_DISTANCE, get_jerk_factor, get_T_FOLLOW
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, LEAD_DANGER_FACTOR, STOP_DISTANCE, desired_follow_distance, get_jerk_factor, get_T_FOLLOW
 
 class FrogPilotFollowing:
   def __init__(self, FrogPilotPlanner):
@@ -11,6 +11,7 @@ class FrogPilotFollowing:
 
     self.acceleration_jerk = 0
     self.danger_jerk = 0
+    self.desired_follow_distance = 0
     self.speed_jerk = 0
     self.t_follow = 0
 
@@ -53,6 +54,9 @@ class FrogPilotFollowing:
     if long_control_active and self.frogpilot_planner.tracking_lead:
       if frogpilot_toggles.human_following:
         self.update_follow_values(self.frogpilot_planner.lead_one.dRel, v_ego, self.frogpilot_planner.lead_one.vLead)
+      self.desired_follow_distance = desired_follow_distance(v_ego, self.frogpilot_planner.lead_one.vLead, self.t_follow)
+    else:
+      self.desired_follow_distance = 0
 
   def update_follow_values(self, lead_distance, v_ego, v_lead):
     # Offset by FrogAi for FrogPilot for a more natural approach to a faster lead

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc

@@ -154,6 +154,7 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil
   cscControllingSpeed = frogpilotPlan.getCscControllingSpeed();
   cscSpeed = frogpilotPlan.getCscSpeed();
   cscTraining = frogpilotPlan.getCscTraining();
+  desiredFollowDistance = frogpilotPlan.getDesiredFollowDistance();
   experimentalMode = selfdriveState.getExperimentalMode();
   forceCoast = frogpilotCarState.getForceCoast();
   laneWidthLeft = frogpilotPlan.getLaneWidthLeft();
@@ -525,6 +526,73 @@ void FrogPilotAnnotatedCameraWidget::paintCurveSpeedControlTraining(QPainter &p)
   p.restore();
 }
 
+void FrogPilotAnnotatedCameraWidget::paintLeadMetrics(QPainter &p, bool adjacent, QPointF *chevron, const cereal::RadarState::LeadData::Reader &lead_data) {
+  float leadDistance = lead_data.getDRel() + (adjacent ? std::abs(lead_data.getYRel()) : 0.0f);
+  float leadSpeed = std::max(lead_data.getVLead(), 0.0f);
+
+  QString distanceString = QString::number(qRound(leadDistance * distanceConversion));
+  QString speedString = QString::number(qRound(leadSpeed * speedConversionMetrics));
+
+  QVector<QString> textLines;
+  textLines.reserve(3);
+  if (adjacent) {
+    textLines.append(QString("%1 %2").arg(distanceString, leadDistanceUnit));
+    textLines.append(QString("%1 %2").arg(speedString, leadSpeedUnit));
+  } else {
+    if (frogpilot_toggles.value("openpilot_longitudinal").toBool()) {
+      int desiredDistance = std::max(0, qRound(desiredFollowDistance * distanceConversion));
+      textLines.append(QString("%1 %2 (%3)").arg(distanceString, leadDistanceUnit, tr("Desired: %1").arg(desiredDistance)));
+    } else {
+      textLines.append(QString("%1 %2").arg(distanceString, leadDistanceUnit));
+    }
+    textLines.append(QString("%1 %2").arg(speedString, leadSpeedUnit));
+
+    float timeGap = leadDistance / std::max(speed / speedConversion, 1.0f);
+    textLines.append(QString("%1 %2").arg(QString::number(timeGap, 'f', 2), tr("seconds")));
+  }
+
+  p.setFont(InterFont(45, QFont::DemiBold));
+  p.setPen(whiteColor());
+
+  QFontMetrics metrics(p.font());
+  int lineHeight = metrics.lineSpacing();
+
+  int maxTextWidth = 0;
+  for (QString &line : textLines) {
+    maxTextWidth = std::max(maxTextWidth, metrics.horizontalAdvance(line));
+  }
+
+  int centerX = (chevron[2].x() + chevron[0].x()) / 2;
+  int startY = chevron[0].y() + lineHeight + 5;
+
+  int xMargin = maxTextWidth * 0.1;
+  int yMargin = lineHeight * 0.1;
+
+  QRect textRect(centerX - maxTextWidth / 2, startY - lineHeight, maxTextWidth, textLines.size() * lineHeight);
+  textRect.adjust(-xMargin, -yMargin, xMargin, yMargin);
+
+  if (adjacent) {
+    if (textRect.intersects(adjacentLeadTextRect) || textRect.intersects(leadTextRect)) {
+      return;
+    }
+    adjacentLeadTextRect = textRect;
+  } else {
+    leadTextRect = textRect;
+  }
+
+  for (int i = 0; i < textLines.size(); ++i) {
+    int lineX = centerX - metrics.horizontalAdvance(textLines[i]) / 2;
+    int lineY = startY + (i * lineHeight);
+
+    QPainterPath path;
+    path.addText(lineX, lineY, p.font(), textLines[i]);
+    p.strokePath(path, QPen(Qt::black, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
+
+    p.setPen(whiteColor());
+    p.drawText(lineX, lineY, textLines[i]);
+  }
+}
+
 void FrogPilotAnnotatedCameraWidget::paintLongitudinalPaused(QPainter &p) {
   if (dmIconPosition == QPoint(0, 0)) {
     return;

frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h

@@ -15,6 +15,7 @@ public:
   void paintAdjacentPaths(QPainter &p);
   void paintBlindSpotPath(QPainter &p);
   void paintFrogPilotWidgets(QPainter &p, UIState &s);
+  void paintLeadMetrics(QPainter &p, bool adjacent, QPointF *chevron, const cereal::RadarState::LeadData::Reader &lead_data);
   void updateState(const UIState &s, const FrogPilotUIState &fs);
 
   bool hideBottomIcons;
@@ -41,6 +42,8 @@ public:
 
   QPolygonF track_adjacent_vertices[2];
 
+  QRect adjacentLeadTextRect;
+  QRect leadTextRect;
   QRect setSpeedRect;
 
   QSize defaultSize;
@@ -76,6 +79,7 @@ private:
   bool redLight;
 
   int animationFrameIndex;
+  int desiredFollowDistance;
   int frogHopCount;
   int signalAnimationLength;
   int signalHeight;

selfdrive/ui/qt/onroad/model.cc

@@ -35,7 +35,7 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) {
   drawLaneLines(painter);
   drawPath(painter, model, surface_rect.height());
 
-  if (longitudinal_control && sm.alive("radarState") && !frogpilot_toggles.value("hide_lead_marker").toBool()) {
+  if ((longitudinal_control || frogpilot_toggles.value("lead_info").toBool()) && sm.alive("radarState") && !frogpilot_toggles.value("hide_lead_marker").toBool()) {
     update_leads(radar_state, model.getPosition());
     const auto &lead_two = radar_state.getLeadTwo();
     if (lead_one.getStatus()) {
@@ -46,6 +46,8 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) {
       }
     } else {
       // FrogPilot variables
+      frogpilot_nvg->leadTextRect = QRect();
+    }
     if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) {
       drawLead(painter, lead_two, lead_vertices[1], surface_rect, QColor(frogpilot_toggles.value("lead_marker_color").toString()));
     }
@@ -59,11 +61,18 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) {
 
     updateAdjacentLeads(frogpilot_radar_state, model.getPosition());
 
-    if (lead_left.getStatus()) {
+    frogpilot_nvg->adjacentLeadTextRect = QRect();
+
+    if (lead_left.getStatus() && lead_right.getStatus() && (lead_left.getDRel() < lead_right.getDRel())) {
       drawLead(painter, reinterpret_cast<const cereal::RadarState::LeadData::Reader&>(lead_left), adjacent_lead_vertices[0], surface_rect, frogpilot_nvg->blueColor(), true);
-    }
-    if (lead_right.getStatus()) {
       drawLead(painter, reinterpret_cast<const cereal::RadarState::LeadData::Reader&>(lead_right), adjacent_lead_vertices[1], surface_rect, frogpilot_nvg->purpleColor(), true);
+    } else {
+      if (lead_left.getStatus()) {
+        drawLead(painter, reinterpret_cast<const cereal::RadarState::LeadData::Reader&>(lead_left), adjacent_lead_vertices[0], surface_rect, frogpilot_nvg->blueColor(), true);
+      }
+      if (lead_right.getStatus()) {
+        drawLead(painter, reinterpret_cast<const cereal::RadarState::LeadData::Reader&>(lead_right), adjacent_lead_vertices[1], surface_rect, frogpilot_nvg->purpleColor(), true);
+      }
     }
   }
 
@@ -277,6 +286,9 @@ void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadDa
   painter.drawPolygon(chevron, std::size(chevron));
 
   // FrogPilot variables
+  if (frogpilot_toggles.value("lead_info").toBool()) {
+    frogpilot_nvg->paintLeadMetrics(painter, adjacent, chevron, lead_data);
+  }
 }
 
 // Projects a point in car to space to the corresponding point in full frame image space.