Draw adjacent paths in onroad UI

Added toggle to draw the adjacent paths in the onroad UI to visualize what other lanes the model sees.

common/params.cc

@@ -211,6 +211,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     // FrogPilot parameters
     {"AccelerationPath", PERSISTENT},
     {"AccelerationProfile", PERSISTENT},
+    {"AdjacentPath", PERSISTENT},
+    {"AdjacentPathMetrics", PERSISTENT},
     {"AggressiveAcceleration", PERSISTENT},
     {"AggressiveFollow", PERSISTENT},
     {"AggressiveJerk", PERSISTENT},

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -57,7 +57,7 @@ class FrogPilotPlanner:
       self.cem.update(carState, enabled, sm['frogpilotNavigation'], modelData, sm['radarState'], self.road_curvature, self.stop_distance, mpc.t_follow, v_ego)
 
     # Update the current lane widths
-    check_lane_width = self.blind_spot_path
+    check_lane_width = self.adjacent_lanes or self.blind_spot_path
     if check_lane_width and v_ego >= LANE_CHANGE_SPEED_MIN:
       self.lane_width_left = float(self.fpf.calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0]))
       self.lane_width_right = float(self.fpf.calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1]))
@@ -121,6 +121,7 @@ class FrogPilotPlanner:
     self.relaxed_jerk = params.get_float("RelaxedJerk")
 
     custom_ui = params.get_bool("CustomUI")
+    self.adjacent_lanes = custom_ui and params.get_bool("AdjacentPath")
     self.blind_spot_path = custom_ui and params.get_bool("BlindSpotPath")
 
     longitudinal_tune = params.get_bool("LongitudinalTune")

selfdrive/frogpilot/ui/visual_settings.cc

@@ -23,6 +23,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.", ""},
+    {"AdjacentPath", "Adjacent Paths", "Display paths to the left and right of your car, visualizing where the model detects lanes.", ""},
     {"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.", ""},
 
@@ -111,6 +112,10 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
       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 == "AdjacentPath") {
+      std::vector<QString> adjacentPathToggles{"AdjacentPathMetrics"};
+      std::vector<QString> adjacentPathToggleNames{tr("Display Metrics")};
+      toggle = new FrogPilotParamToggleControl(param, title, desc, icon, adjacentPathToggles, adjacentPathToggleNames);
 
     } 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", "LeadInfo"};
+  std::set<QString> customOnroadUIKeys = {"AccelerationPath", "AdjacentPath", "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

@@ -621,6 +621,51 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
     }
   }
 
+  // Paint adjacent lane paths
+  if (scene.adjacent_path && (laneWidthLeft != 0 || laneWidthRight != 0)) {
+    // Set up the units
+    double distanceValue = is_metric ? 1.0 : METER_TO_FOOT;
+    QString unit_d = is_metric ? " meters" : " feet";
+
+    // Declare the lane width thresholds
+    constexpr float minLaneWidth = 2.0f;
+    constexpr float maxLaneWidth = 4.0f;
+
+    // Set gradient colors based on laneWidth and blindspot
+    auto setGradientColors = [](QLinearGradient &gradient, float laneWidth, bool blindspot) {
+      // Make the path red for smaller paths or if there's a car in the blindspot and green for larger paths
+      double hue = (laneWidth < minLaneWidth || laneWidth > maxLaneWidth || blindspot)
+                         ? 0.0 : 120.0 * (laneWidth - minLaneWidth) / (maxLaneWidth - minLaneWidth);
+      double hue_ratio = hue / 360.0;
+      gradient.setColorAt(0.0, QColor::fromHslF(hue_ratio, 0.75, 0.50, 0.6));
+      gradient.setColorAt(0.5, QColor::fromHslF(hue_ratio, 0.75, 0.50, 0.4));
+      gradient.setColorAt(1.0, QColor::fromHslF(hue_ratio, 0.75, 0.50, 0.2));
+    };
+
+    // Paint the lanes
+    auto paintLane = [&](QPainter &painter, const QPolygonF &lane, float laneWidth, bool blindspot) {
+      QLinearGradient gradient(0, height(), 0, 0);
+      setGradientColors(gradient, laneWidth, blindspot);
+
+      painter.setFont(InterFont(30, QFont::DemiBold));
+      painter.setBrush(gradient);
+      painter.setPen(Qt::transparent);
+      painter.drawPolygon(lane);
+      painter.setPen(Qt::white);
+
+      QRectF boundingRect = lane.boundingRect();
+      if (scene.adjacent_path_metrics) {
+        painter.drawText(boundingRect.center(),
+                         blindspot ? "Vehicle in blind spot" :
+                         QString("%1%2").arg(laneWidth * distanceValue, 0, 'f', 2).arg(unit_d));
+      }
+      painter.setPen(Qt::NoPen);
+    };
+
+    paintLane(painter, scene.track_adjacent_vertices[4], laneWidthLeft, blindSpotLeft);
+    paintLane(painter, scene.track_adjacent_vertices[5], laneWidthRight, blindSpotRight);
+  }
+
   painter.restore();
 }
 
@@ -882,6 +927,9 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
 
   experimentalMode = scene.experimental_mode;
 
+  laneWidthLeft = scene.lane_width_left;
+  laneWidthRight = scene.lane_width_right;
+
   leadInfo = scene.lead_info;
   obstacleDistance = scene.obstacle_distance;
   obstacleDistanceStock = scene.obstacle_distance_stock;

selfdrive/ui/qt/onroad.h

@@ -134,6 +134,8 @@ private:
   bool turnSignalRight;
 
   float distanceConversion;
+  float laneWidthLeft;
+  float laneWidthRight;
   float speedConversion;
 
   int cameraView;

selfdrive/ui/ui.cc

@@ -291,6 +291,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.adjacent_path = custom_onroad_ui && params.getBool("AdjacentPath");
+  scene.adjacent_path_metrics = scene.adjacent_path && params.getBool("AdjacentPathMetrics");
   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");

selfdrive/ui/ui.h

@@ -170,6 +170,8 @@ typedef struct UIScene {
 
   // FrogPilot variables
   bool acceleration_path;
+  bool adjacent_path;
+  bool adjacent_path_metrics;
   bool always_on_lateral;
   bool always_on_lateral_active;
   bool blind_spot_left;