Draw adjacent paths in onroad UI

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

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/controls/frogpilot_planner.py

@@ -80,7 +80,7 @@ class FrogPilotPlanner:
     else:
       self.min_accel = ACCEL_MIN
 
-    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(calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0]))
       self.lane_width_right = float(calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1]))
@@ -182,6 +182,7 @@ class FrogPilotPlanner:
     self.relaxed_follow = self.params.get_float("RelaxedFollow")
 
     custom_ui = self.params.get_bool("CustomUI")
+    self.adjacent_lanes = custom_ui and self.params.get_bool("AdjacentPath")
     self.blind_spot_path = custom_ui and self.params.get_bool("BlindSpotPath")
 
     longitudinal_tune = self.CP.openpilotLongitudinalControl and self.params.get_bool("LongitudinalTune")

selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc

@@ -125,8 +125,8 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
       std::vector<QString> leadInfoToggleNames{tr("Use SI Values")};
       toggle = new FrogPilotParamToggleControl(param, title, desc, icon, leadInfoToggles, leadInfoToggleNames);
     } else if (param == "CustomPaths") {
-      std::vector<QString> pathToggles{"AccelerationPath", "BlindSpotPath"};
-      std::vector<QString> pathToggleNames{tr("Acceleration"), tr("Blind Spot")};
+      std::vector<QString> pathToggles{"AccelerationPath", "AdjacentPath", "BlindSpotPath", "AdjacentPathMetrics"};
+      std::vector<QString> pathToggleNames{tr("Acceleration"), tr("Adjacent"), tr("Blind Spot"), tr("Metrics")};
       toggle = new FrogPilotParamToggleControl(param, title, desc, icon, pathToggles, pathToggleNames);
 
     } else if (param == "ModelUI") {

selfdrive/ui/qt/onroad.cc

@@ -658,6 +658,42 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
     }
   }
 
+  // Paint adjacent lane paths
+  if (scene.adjacent_path && (laneWidthLeft != 0 || laneWidthRight != 0)) {
+    QString unit_d = is_metric ? tr(" meters") : tr(" feet");
+
+    float minLaneWidth = laneDetectionWidth * 0.5;
+    float maxLaneWidth = laneDetectionWidth * 1.5;
+
+    auto paintLane = [=](QPainter &painter, const QPolygonF &lane, float laneWidth, bool blindspot) {
+      QLinearGradient al(0, height(), 0, 0);
+
+      bool redPath = laneWidth < minLaneWidth || laneWidth > maxLaneWidth || blindspot;
+      float hue = redPath ? 0.0 : 120.0 * (laneWidth - minLaneWidth) / (maxLaneWidth - minLaneWidth);
+
+      al.setColorAt(0.0, QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.6));
+      al.setColorAt(0.5, QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.4));
+      al.setColorAt(1.0, QColor::fromHslF(hue / 360.0, 0.75, 0.50, 0.2));
+
+      painter.setBrush(al);
+      painter.drawPolygon(lane);
+
+      painter.setFont(InterFont(30, QFont::DemiBold));
+      painter.setPen(Qt::white);
+
+      QRectF boundingRect = lane.boundingRect();
+      if (scene.adjacent_path_metrics) {
+        QString text = blindspot ? tr("Vehicle in blind spot") :
+                       QString("%1%2").arg(laneWidth * distanceConversion, 0, 'f', 2).arg(unit_d);
+        painter.drawText(boundingRect, Qt::AlignCenter, text);
+      }
+      painter.setPen(Qt::NoPen);
+    };
+
+    paintLane(painter, scene.track_adjacent_vertices[4], laneWidthLeft, blindSpotLeft);
+    paintLane(painter, scene.track_adjacent_vertices[5], laneWidthRight, blindSpotRight);
+  }
+
   painter.restore();
 }
 
@@ -910,6 +946,9 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() {
 
   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 alertSize;

selfdrive/ui/ui.cc

@@ -303,6 +303,8 @@ void ui_update_frogpilot_params(UIState *s) {
   bool custom_paths = custom_onroad_ui && params.getBool("CustomPaths");
   bool developer_ui = !isRelease && custom_onroad_ui && params.getBool("DeveloperUI");
   scene.acceleration_path = custom_paths && params.getBool("AccelerationPath");
+  scene.adjacent_path = custom_paths && params.getBool("AdjacentPath");
+  scene.adjacent_path_metrics = scene.adjacent_path && params.getBool("AdjacentPathMetrics");
   scene.blind_spot_path = custom_paths && params.getBool("BlindSpotPath");
   scene.lead_info = scene.longitudinal_control && custom_onroad_ui && params.getBool("LeadInfo");
   scene.show_jerk = scene.longitudinal_control && developer_ui && params.getBool("ShowJerk");

selfdrive/ui/ui.h

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