Display openpilot logics for following lead vehicles

Added toggle to show openpilot's logic in regards to following lead vehicles.

cereal/custom.capnp

@@ -24,8 +24,12 @@ struct FrogPilotNavigation @0xf35cc4560bbf6ec2 {
 
 struct FrogPilotPlan @0xda96579883444c35 {
   conditionalExperimental @1 :Bool;
+  desiredFollowDistance @2 :Int16;
   laneWidthLeft @3 :Float32;
   laneWidthRight @4 :Float32;
+  safeObstacleDistance @6 :Int16;
+  safeObstacleDistanceStock @7 :Int16;
+  stoppedEquivalenceFactor @12 :Int16;
 }
 
 struct CustomReserved4 @0x80ae746ee2596b11 {

common/params.cc

@@ -256,6 +256,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"GoatScream", PERSISTENT},
     {"LaneLinesWidth", PERSISTENT},
     {"LateralTune", PERSISTENT},
+    {"LeadInfo", PERSISTENT},
     {"LongitudinalTune", PERSISTENT},
     {"ManualUpdateInitiated", CLEAR_ON_MANAGER_START},
     {"ModelUI", PERSISTENT},
@@ -281,6 +282,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"UnlimitedLength", PERSISTENT},
     {"UpdateSchedule", PERSISTENT},
     {"UpdateTime", PERSISTENT},
+    {"UseSI", PERSISTENT},
     {"WarningSoftVolume", PERSISTENT},
     {"WarningImmediateVolume", PERSISTENT},
 };

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -373,6 +373,16 @@ class LongitudinalMpc:
     else:
       self.acceleration_offset = 1
 
+    # LongitudinalPlan variables for onroad driving insights
+    if self.status:
+      self.safe_obstacle_distance = int(np.mean(get_safe_obstacle_distance(v_ego, t_follow)))
+      self.safe_obstacle_distance_stock = int(np.mean(get_safe_obstacle_distance(v_ego, self.t_follow)))
+      self.stopped_equivalence_factor = int(np.mean(get_stopped_equivalence_factor(lead_xv_0[:,1])))
+    else:
+      self.safe_obstacle_distance = 0
+      self.safe_obstacle_distance_stock = 0
+      self.stopped_equivalence_factor = 0
+
     # To estimate a safe distance from a moving lead, we calculate how much stopping
     # distance that lead needs as a minimum. We can add that to the current distance
     # and then treat that as a stopped car/obstacle at this new distance.

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -94,6 +94,11 @@ class FrogPilotPlanner:
 
     frogpilotPlan.conditionalExperimental = self.cem.experimental_mode
 
+    frogpilotPlan.desiredFollowDistance = mpc.safe_obstacle_distance - mpc.stopped_equivalence_factor
+    frogpilotPlan.safeObstacleDistance = mpc.safe_obstacle_distance
+    frogpilotPlan.safeObstacleDistanceStock = mpc.safe_obstacle_distance_stock
+    frogpilotPlan.stoppedEquivalenceFactor = mpc.stopped_equivalence_factor
+
     frogpilotPlan.laneWidthLeft = self.lane_width_left
     frogpilotPlan.laneWidthRight = self.lane_width_right
 

selfdrive/frogpilot/ui/visual_settings.cc

@@ -24,6 +24,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
     {"CustomUI", "Custom Onroad UI", "Customize the Onroad UI with some additional visual functions.", "../assets/offroad/icon_road.png"},
     {"AccelerationPath", "Acceleration Path", "Visualize the car's intended acceleration or deceleration with a color-coded path.", ""},
     {"BlindSpotPath", "Blind Spot Path", "Visualize your blind spots with a red path when another vehicle is detected nearby.", ""},
+    {"LeadInfo", "Lead Info and Logics", "Get detailed information about the vehicle ahead, including speed and distance, and the logic behind your following distance.", ""},
 
     {"ModelUI", "Model UI", "Personalize how the model's visualizations appear on your screen.", "../assets/offroad/icon_calibration.png"},
     {"DynamicPathWidth", "Dynamic Path Width", "Have the path width dynamically adjust based on the current engagement state of openpilot.", ""},
@@ -106,6 +107,10 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
         }
       });
       toggle = customUIToggle;
+    } else if (param == "LeadInfo") {
+      std::vector<QString> leadInfoToggles{"UseSI"};
+      std::vector<QString> leadInfoToggleNames{tr("Use SI Values")};
+      toggle = new FrogPilotParamToggleControl(param, title, desc, icon, leadInfoToggles, leadInfoToggleNames);
 
     } else if (param == "ModelUI") {
       FrogPilotParamManageControl *modelUIToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);

selfdrive/frogpilot/ui/visual_settings.h

@@ -30,7 +30,7 @@ private:
 
   std::set<QString> alertVolumeControlKeys = {"EngageVolume", "DisengageVolume", "RefuseVolume", "PromptVolume", "PromptDistractedVolume", "WarningSoftVolume", "WarningImmediateVolume"};
   std::set<QString> customAlertsKeys = {};
-  std::set<QString> customOnroadUIKeys = {"AccelerationPath", "BlindSpotPath"};
+  std::set<QString> customOnroadUIKeys = {"AccelerationPath", "BlindSpotPath", "LeadInfo"};
   std::set<QString> customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"};
   std::set<QString> modelUIKeys = {"DynamicPathWidth", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"};
   std::set<QString> qolKeys = {"DriveStats"};

selfdrive/ui/qt/onroad.cc

@@ -223,7 +223,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) {
 
 // ExperimentalButton
 ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent), scene(uiState()->scene) {
-  setFixedSize(btn_size, btn_size);
+  setFixedSize(btn_size, btn_size + 10);
 
   engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size});
   experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size, img_size});
@@ -240,7 +240,7 @@ void ExperimentalButton::changeMode() {
   }
 }
 
-void ExperimentalButton::updateState(const UIState &s) {
+void ExperimentalButton::updateState(const UIState &s, bool leadInfo) {
   const auto cs = (*s.sm)["controlsState"].getControlsState();
   bool eng = cs.getEngageable() || cs.getEnabled() || scene.always_on_lateral_active;
   if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) {
@@ -250,12 +250,13 @@ void ExperimentalButton::updateState(const UIState &s) {
   }
 
   // FrogPilot variables
+  y_offset = leadInfo ? 10 : 0;
 }
 
 void ExperimentalButton::paintEvent(QPaintEvent *event) {
   QPainter p(this);
   QPixmap img = experimental_mode ? experimental_img : engage_img;
-  drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || !(engageable || scene.always_on_lateral_active)) ? 0.6 : 1.0);
+  drawIcon(p, QPoint(btn_size / 2, btn_size / 2 + y_offset), img, QColor(0, 0, 0, 166), (isDown() || !(engageable || scene.always_on_lateral_active)) ? 0.6 : 1.0);
 }
 
 
@@ -331,7 +332,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
   status = s.status;
 
   // update engageability/experimental mode button
-  experimental_btn->updateState(s);
+  experimental_btn->updateState(s, leadInfo);
 
   // update DM icon
   auto dm_state = sm["driverMonitoringState"].getDriverMonitoringState();
@@ -700,6 +701,28 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState
   }
   painter.drawPolygon(chevron, std::size(chevron));
 
+  // Add lead info
+  if (leadInfo) {
+    // Declare the variables
+    float lead_speed = std::max(lead_data.getVLead(), 0.0f);  // Ensure lead speed doesn't go under 0 m/s cause that's dumb
+
+    // Form the text and center it below the chevron
+    painter.setPen(Qt::white);
+    painter.setFont(InterFont(35, QFont::Bold));
+
+    QString text = QString("%1 %2 | %3 %4")
+                           .arg(qRound(d_rel * distanceConversion))
+                           .arg(leadDistanceUnit)
+                           .arg(qRound(lead_speed * speedConversion))
+                           .arg(leadSpeedUnit);
+
+    // Calculate the text starting position
+    QFontMetrics metrics(painter.font());
+    int middle_x = (chevron[2].x() + chevron[0].x()) / 2;
+    int textWidth = metrics.horizontalAdvance(text);
+    painter.drawText(middle_x - textWidth / 2, chevron[0].y() + metrics.height() + 5, text);
+  }
+
   painter.restore();
 }
 
@@ -859,11 +882,19 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
 
   experimentalMode = scene.experimental_mode;
 
+  leadInfo = scene.lead_info;
+  obstacleDistance = scene.obstacle_distance;
+  obstacleDistanceStock = scene.obstacle_distance_stock;
+
   mapOpen = scene.map_open;
 
   turnSignalLeft = scene.turn_signal_left;
   turnSignalRight = scene.turn_signal_right;
 
+  if (leadInfo) {
+    drawLeadInfo(p);
+  }
+
   if (alwaysOnLateral || conditionalExperimental) {
     drawStatusBar(p);
   }
@@ -912,6 +943,113 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
   }
 }
 
+void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) {
+  // Declare the variables
+  static QElapsedTimer timer;
+  static bool isFiveSecondsPassed = false;
+  constexpr int maxAccelDuration = 5000;
+
+  // Constants for units and conversions
+  QString accelerationUnit = " m/s²";
+  leadDistanceUnit = mapOpen ? "m" : "meters";
+  leadSpeedUnit = "m/2";
+
+  float accelerationConversion = 1.0f;
+  distanceConversion = 1.0f;
+  speedConversion = 1.0f;
+
+  if (!scene.use_si) {
+    if (is_metric) {
+      // Metric conversion
+      leadSpeedUnit = "kph";
+      speedConversion = MS_TO_KPH;
+    } else {
+      // US imperial conversion
+      accelerationUnit = " ft/s²";
+      leadDistanceUnit = mapOpen ? "ft" : "feet";
+      leadSpeedUnit = "mph";
+
+      accelerationConversion = METER_TO_FOOT;
+      distanceConversion = METER_TO_FOOT;
+      speedConversion = MS_TO_MPH;
+    }
+  }
+
+  // Update acceleration
+  double currentAcceleration = std::round(scene.acceleration * 100) / 100;
+  static double maxAcceleration = 0.0;
+
+  if (currentAcceleration > maxAcceleration && status == STATUS_ENGAGED) {
+    maxAcceleration = currentAcceleration;
+    isFiveSecondsPassed = false;
+    timer.start();
+  } else {
+    isFiveSecondsPassed = timer.hasExpired(maxAccelDuration);
+  }
+
+  // Construct text segments
+  auto createText = [&](const QString &title, const double data) {
+    return title + QString::number(std::round(data * distanceConversion)) + " " + leadDistanceUnit;
+  };
+
+  // Create segments for insights
+  QString accelText = QString("Accel: %1%2")
+    .arg(currentAcceleration * accelerationConversion, 0, 'f', 2)
+    .arg(accelerationUnit);
+
+  QString maxAccSuffix = QString(mapOpen ? "" : " - Max: %1%2")
+    .arg(maxAcceleration * accelerationConversion, 0, 'f', 2)
+    .arg(accelerationUnit);
+
+  QString obstacleText = createText(mapOpen ? " | Obstacle: " : "  |  Obstacle Factor: ", obstacleDistance);
+  QString stopText = createText(mapOpen ? " - Stop: " : "  -  Stop Factor: ", scene.stopped_equivalence);
+  QString followText = " = " + createText(mapOpen ? "Follow: " : "Follow Distance: ", scene.desired_follow);
+
+  // Check if the longitudinal toggles have an impact on the driving logics
+  auto createDiffText = [&](const double data, const double stockData) {
+    double difference = std::round((data - stockData) * distanceConversion);
+    return difference != 0 ? QString(" (%1%2)").arg(difference > 0 ? "+" : "").arg(difference) : QString();
+  };
+
+  // Prepare rectangle for insights
+  p.save();
+  QRect insightsRect(rect().left() - 1, rect().top() - 60, rect().width() + 2, 100);
+  p.setBrush(QColor(0, 0, 0, 150));
+  p.drawRoundedRect(insightsRect, 30, 30);
+  p.setFont(InterFont(30, QFont::DemiBold));
+  p.setRenderHint(QPainter::TextAntialiasing);
+
+  // Calculate positioning for text drawing
+  QRect adjustedRect = insightsRect.adjusted(0, 27, 0, 27);
+  int textBaseLine = adjustedRect.y() + (adjustedRect.height() + p.fontMetrics().height()) / 2 - p.fontMetrics().descent();
+
+  // Calculate the entire text width to ensure perfect centering
+  int totalTextWidth = p.fontMetrics().horizontalAdvance(accelText)
+                     + p.fontMetrics().horizontalAdvance(maxAccSuffix)
+                     + p.fontMetrics().horizontalAdvance(obstacleText)
+                     + p.fontMetrics().horizontalAdvance(createDiffText(obstacleDistance, obstacleDistanceStock))
+                     + p.fontMetrics().horizontalAdvance(stopText)
+                     + p.fontMetrics().horizontalAdvance(followText);
+
+  int textStartPos = adjustedRect.x() + (adjustedRect.width() - totalTextWidth) / 2;
+
+  // Draw the text
+  auto drawText = [&](const QString &text, const QColor color) {
+    p.setPen(color);
+    p.drawText(textStartPos, textBaseLine, text);
+    textStartPos += p.fontMetrics().horizontalAdvance(text);
+  };
+
+  drawText(accelText, Qt::white);
+  drawText(maxAccSuffix, isFiveSecondsPassed ? Qt::white : Qt::red);
+  drawText(obstacleText, Qt::white);
+  drawText(createDiffText(obstacleDistance, obstacleDistanceStock), (obstacleDistance - obstacleDistanceStock) > 0 ? Qt::green : Qt::red);
+  drawText(stopText, Qt::white);
+  drawText(followText, Qt::white);
+
+  p.restore();
+}
+
 void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
   p.save();
 

selfdrive/ui/qt/onroad.h

@@ -40,7 +40,7 @@ class ExperimentalButton : public QPushButton {
 
 public:
   explicit ExperimentalButton(QWidget *parent = 0);
-  void updateState(const UIState &s);
+  void updateState(const UIState &s, bool leadInfo);
 
 private:
   void paintEvent(QPaintEvent *event) override;
@@ -54,6 +54,8 @@ private:
 
   // FrogPilot variables
   UIScene &scene;
+
+  int y_offset;
 };
 
 
@@ -109,6 +111,7 @@ private:
   void initializeFrogPilotWidgets();
   void updateFrogPilotWidgets(QPainter &p);
 
+  void drawLeadInfo(QPainter &p);
   void drawStatusBar(QPainter &p);
   void drawTurnSignals(QPainter &p);
 
@@ -125,18 +128,27 @@ private:
   bool blindSpotRight;
   bool conditionalExperimental;
   bool experimentalMode;
+  bool leadInfo;
   bool mapOpen;
   bool turnSignalLeft;
   bool turnSignalRight;
 
+  float distanceConversion;
+  float speedConversion;
+
   int cameraView;
   int conditionalSpeed;
   int conditionalSpeedLead;
   int conditionalStatus;
   int customColors;
   int customSignals;
+  int obstacleDistance;
+  int obstacleDistanceStock;
   int totalFrames = 8;
 
+  QString leadDistanceUnit;
+  QString leadSpeedUnit;
+
   size_t animationFrameIndex;
 
   std::unordered_map<int, std::tuple<QString, int, QColor, std::map<double, QBrush>>> themeConfiguration;

selfdrive/ui/ui.cc

@@ -246,6 +246,12 @@ static void update_state(UIState *s) {
       scene.lane_width_left = frogpilotPlan.getLaneWidthLeft();
       scene.lane_width_right = frogpilotPlan.getLaneWidthRight();
     }
+    if (scene.lead_info) {
+      scene.desired_follow = frogpilotPlan.getDesiredFollowDistance();
+      scene.obstacle_distance = frogpilotPlan.getSafeObstacleDistance();
+      scene.obstacle_distance_stock = frogpilotPlan.getSafeObstacleDistanceStock();
+      scene.stopped_equivalence = frogpilotPlan.getStoppedEquivalenceFactor();
+    }
   }
   if (sm.updated("liveLocationKalman")) {
     auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman();
@@ -286,6 +292,8 @@ void ui_update_frogpilot_params(UIState *s) {
   bool custom_onroad_ui = params.getBool("CustomUI");
   scene.acceleration_path = custom_onroad_ui && params.getBool("AccelerationPath");
   scene.blind_spot_path = custom_onroad_ui && params.getBool("BlindSpotPath");
+  scene.lead_info = custom_onroad_ui && params.getBool("LeadInfo");
+  scene.use_si = scene.lead_info && params.getBool("UseSI");
 
   bool custom_theme = params.getBool("CustomTheme");
   scene.custom_colors = custom_theme ? params.getInt("CustomColors") : 0;

selfdrive/ui/ui.h

@@ -179,11 +179,13 @@ typedef struct UIScene {
   bool dynamic_path_width;
   bool enabled;
   bool experimental_mode;
+  bool lead_info;
   bool map_open;
   bool model_ui;
   bool turn_signal_left;
   bool turn_signal_right;
   bool unlimited_road_ui_length;
+  bool use_si;
 
   float lane_line_width;
   float lane_width_left;
@@ -199,6 +201,10 @@ typedef struct UIScene {
   int custom_colors;
   int custom_icons;
   int custom_signals;
+  int desired_follow;
+  int obstacle_distance;
+  int obstacle_distance_stock;
+  int stopped_equivalence;
 
   QPolygonF track_adjacent_vertices[6];
   QPolygonF track_edge_vertices;