Visuals - Custom Onroad UI - Paths - Adjacent

Show the detected adjacent lanes.

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -129,7 +129,7 @@ class FrogPilotPlanner:
       else:
         self.min_accel = A_CRUISE_MIN
 
-    check_lane_width = frogpilot_toggles.lane_detection
+    check_lane_width = frogpilot_toggles.adjacent_lanes or frogpilot_toggles.lane_detection
     if check_lane_width and v_ego >= frogpilot_toggles.minimum_lane_change_speed:
       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]))
@@ -277,6 +277,9 @@ class FrogPilotPlanner:
     frogpilotPlan.conditionalExperimental = self.cem.experimental_mode
     frogpilotPlan.redLight = self.cem.red_light_detected
 
+    frogpilotPlan.laneWidthLeft = self.lane_width_left
+    frogpilotPlan.laneWidthRight = self.lane_width_right
+
     frogpilotPlan.leadDeparting = self.lead_departing
 
     frogpilotPlan.maxAcceleration = self.max_accel

selfdrive/modeld/fill_model_msg.py

@@ -94,10 +94,29 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str,
     PLAN_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx+1] + (1 - p) * ModelConstants.T_IDXS[tidx]
 
   # lane lines
-  modelV2.init('laneLines', 4)
+  modelV2.init('laneLines', 6)
-  for i in range(4):
+  for i in range(6):
-    lane_line = modelV2.laneLines[i]
+    if i < 4:
-    fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1])
+      lane_line = modelV2.laneLines[i]
+      fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1])
+    else:
+      lane_line = modelV2.laneLines[i]
+      far_lane, near_lane, road_edge = (0, 1, 0) if i == 4 else (3, 2, 1)
+
+      lane_diff = np.abs(net_output_data['lane_lines'][0,near_lane] - net_output_data['lane_lines'][0,far_lane])
+      road_edge_diff = np.abs(net_output_data['lane_lines'][0,near_lane] - net_output_data['road_edges'][0,road_edge])
+
+      y_min = net_output_data['lane_lines'][0, near_lane,:,0]
+      z_min = net_output_data['lane_lines'][0, near_lane,:,1]
+
+      y_min += np.where(lane_diff[:,0] < road_edge_diff[:,0], net_output_data['lane_lines'][0,far_lane,:,0], net_output_data['road_edges'][0,road_edge,:,0])
+      z_min += np.where(lane_diff[:,1] < road_edge_diff[:,1], net_output_data['lane_lines'][0,far_lane,:,1], net_output_data['road_edges'][0,road_edge,:,1])
+
+      y_min /= 2
+      z_min /= 2
+
+      fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), y_min, z_min)
+
   modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist()
   modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist()
 

selfdrive/ui/qt/onroad.cc

@@ -641,6 +641,40 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
   painter.setBrush(bg);
   painter.drawPolygon(scene.track_vertices);
 
+  // Paint adjacent lane paths
+  if (scene.adjacent_path && (laneWidthLeft != 0 || laneWidthRight != 0)) {
+    float minLaneWidth = laneDetectionWidth * 0.5;
+    float maxLaneWidth = laneDetectionWidth * 1.5;
+
+    auto paintLane = [&](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);
+
+      if (scene.adjacent_path_metrics) {
+        painter.setFont(InterFont(30, QFont::DemiBold));
+        painter.setPen(Qt::white);
+
+        QRectF boundingRect = lane.boundingRect();
+        QString text = blindspot ? tr("Vehicle in blind spot") : QString("%1%2").arg(laneWidth * distanceConversion, 0, 'f', 2).arg(leadDistanceUnit);
+        painter.drawText(boundingRect, Qt::AlignCenter, text);
+
+        painter.setPen(Qt::NoPen);
+      }
+    };
+
+    paintLane(scene.track_adjacent_vertices[4], laneWidthLeft, blindSpotLeft);
+    paintLane(scene.track_adjacent_vertices[5], laneWidthRight, blindSpotRight);
+  }
+
   painter.restore();
 }
 
@@ -865,6 +899,9 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() {
   alwaysOnLateralActive = scene.always_on_lateral_active;
   showAlwaysOnLateralStatusBar = scene.show_aol_status_bar;
 
+  blindSpotLeft = scene.blind_spot_left;
+  blindSpotRight = scene.blind_spot_right;
+
   compass = scene.compass;
 
   conditionalStatus = scene.conditional_status;
@@ -876,6 +913,10 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() {
 
   experimentalMode = scene.experimental_mode;
 
+  laneDetectionWidth = scene.lane_detection_width;
+  laneWidthLeft = scene.lane_width_left;
+  laneWidthRight = scene.lane_width_right;
+
   mapOpen = scene.map_open;
 
   onroadDistanceButton = scene.onroad_distance_button;

selfdrive/ui/qt/onroad.h

@@ -173,6 +173,8 @@ private:
   QHBoxLayout *bottom_layout;
 
   bool alwaysOnLateralActive;
+  bool blindSpotLeft;
+  bool blindSpotRight;
   bool compass;
   bool experimentalMode;
   bool mapOpen;
@@ -189,6 +191,9 @@ private:
   float accelerationConversion;
   float cruiseAdjustment;
   float distanceConversion;
+  float laneDetectionWidth;
+  float laneWidthLeft;
+  float laneWidthRight;
   float slcSpeedLimitOffset;
   float speedConversion;
 

selfdrive/ui/ui.cc

@@ -121,6 +121,11 @@ void update_model(UIState *s,
   }
   max_idx = get_path_length_idx(plan_position, max_distance);
   update_line_data(s, plan_position, 0.9, 1.22, &scene.track_vertices, max_idx, false);
+
+  // Update adjacent paths
+  for (int i = 4; i <= 5; i++) {
+    update_line_data(s, lane_lines[i], (i == 4 ? scene.lane_width_left : scene.lane_width_right) / 2.0f, 0, &scene.track_adjacent_vertices[i], max_idx);
+  }
 }
 
 void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &driverstate, float dm_fade_state, bool is_rhd) {
@@ -215,6 +220,8 @@ static void update_state(UIState *s) {
   }
   if (sm.updated("carState")) {
     auto carState = sm["carState"].getCarState();
+    scene.blind_spot_left = carState.getLeftBlindspot();
+    scene.blind_spot_right = carState.getRightBlindspot();
     scene.parked = carState.getGearShifter() == cereal::CarState::GearShifter::PARK;
   }
   if (sm.updated("controlsState")) {
@@ -239,6 +246,8 @@ static void update_state(UIState *s) {
   if (sm.updated("frogpilotPlan")) {
     auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan();
     scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise();
+    scene.lane_width_left = frogpilotPlan.getLaneWidthLeft();
+    scene.lane_width_right = frogpilotPlan.getLaneWidthRight();
     scene.speed_limit = frogpilotPlan.getSlcSpeedLimit();
     scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset();
     scene.speed_limit_overridden = frogpilotPlan.getSlcOverridden();
@@ -291,6 +300,8 @@ void ui_update_frogpilot_params(UIState *s) {
   bool custom_onroad_ui = params.getBool("CustomUI");
   bool custom_paths = custom_onroad_ui && params.getBool("CustomPaths");
   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.compass = custom_onroad_ui && params.getBool("Compass");
 
   scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing");
@@ -302,6 +313,9 @@ void ui_update_frogpilot_params(UIState *s) {
 
   scene.experimental_mode_via_screen = scene.longitudinal_control && params.getBool("ExperimentalModeActivation") && params.getBool("ExperimentalModeViaTap");
 
+  bool lane_detection = params.getBool("NudgelessLaneChange") && params.getInt("LaneDetectionWidth") != 0;
+  scene.lane_detection_width = lane_detection ? params.getInt("LaneDetectionWidth") * (scene.is_metric ? 1 : FOOT_TO_METER) / 10.0f : 2.75f;
+
   bool longitudinal_tune = scene.longitudinal_control && params.getBool("LongitudinalTune");
   bool radarless_model = params.get("Model") == "radical-turtle";
   scene.lead_detection_threshold = longitudinal_tune && !radarless_model ? params.getInt("LeadDetectionThreshold") / 100.0f : 0.5;

selfdrive/ui/ui.h

@@ -187,7 +187,11 @@ 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_right;
   bool compass;
   bool conditional_experimental;
   bool disable_smoothing_mtsc;
@@ -217,6 +221,9 @@ typedef struct UIScene {
   bool vtsc_controlling_curve;
 
   float adjusted_cruise;
+  float lane_detection_width;
+  float lane_width_left;
+  float lane_width_right;
   float lead_detection_threshold;
   float speed_limit;
   float speed_limit_offset;
@@ -229,6 +236,8 @@ typedef struct UIScene {
   int conditional_speed_lead;
   int conditional_status;
 
+  QPolygonF track_adjacent_vertices[6];
+
 } UIScene;
 
 class UIState : public QObject {