Visuals - Custom Onroad UI - Paths - Adjacent

Show the detected adjacent lanes.

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -98,7 +98,7 @@ class FrogPilotPlanner:
     if run_cem and (controlsState.enabled or frogpilotCarControl.alwaysOnLateral) and driving_gear:
       self.cem.update(carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles)
 
-    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 = calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0])
       self.lane_width_right = calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1])

selfdrive/modeld/fill_model_msg.py

@@ -94,10 +94,31 @@ 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)
-  for i in range(4):
+  modelV2.init('laneLines', 6)
+  for i in range(6):
     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])
+    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])
+    else:
+      far_lane, near_lane, road_edge = (0, 1, 0) if i == 4 else (3, 2, 1)
+
+      near_lane_y = net_output_data['lane_lines'][0,near_lane,:,0]
+      road_edge_y = net_output_data['road_edges'][0,road_edge,:,0]
+      far_lane_y = net_output_data['lane_lines'][0,far_lane,:,0]
+
+      road_edge_distance = abs(np.linalg.norm(road_edge_y - near_lane_y))
+      far_lane_distance = abs(np.linalg.norm(far_lane_y - near_lane_y))
+
+      if road_edge_distance < far_lane_distance:
+        closest_lane_y = road_edge_y
+      else:
+        closest_lane_y = far_lane_y
+
+      diff_y = closest_lane_y - near_lane_y
+      new_lane_y = near_lane_y + diff_y / 2
+
+      fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), new_lane_y, net_output_data['lane_lines'][0,near_lane,:,1])
+
   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/annotated_camera.cc

@@ -313,6 +313,41 @@ 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)) {
+    const float minLaneWidth = laneDetectionWidth * 0.5f;
+    const float maxLaneWidth = laneDetectionWidth * 1.5f;
+
+    auto paintLane = [&](const QPolygonF &lane, float laneWidth, bool blindspot) {
+      QLinearGradient gradient(0, height(), 0, 0);
+
+      bool redPath = laneWidth < minLaneWidth || laneWidth > maxLaneWidth || blindspot;
+      float hue = redPath ? 0.0f : 120.0f * (laneWidth - minLaneWidth) / (maxLaneWidth - minLaneWidth);
+      float hueF = hue / 360.0f;
+
+      gradient.setColorAt(0.0, QColor::fromHslF(hueF, 0.75f, 0.50f, 0.6f));
+      gradient.setColorAt(0.5, QColor::fromHslF(hueF, 0.75f, 0.50f, 0.4f));
+      gradient.setColorAt(1.0, QColor::fromHslF(hueF, 0.75f, 0.50f, 0.2f));
+
+      painter.setBrush(gradient);
+      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::number(laneWidth * distanceConversion, 'f', 2) + 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();
 }
 
@@ -549,6 +584,9 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce
     drawStatusBar(painter);
   }
 
+  blindSpotLeft = scene.blind_spot_left;
+  blindSpotRight = scene.blind_spot_right;
+
   compass = scene.compass;
   bool enableCompass = compass && !hideBottomIcons;
   compass_img->setVisible(enableCompass);
@@ -568,6 +606,10 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce
 
   experimentalMode = scene.experimental_mode;
 
+  laneDetectionWidth = scene.lane_detection_width;
+  laneWidthLeft = scene.lane_width_left;
+  laneWidthRight = scene.lane_width_right;
+
   mapOpen = scene.map_open;
   map_settings_btn_bottom->setEnabled(map_settings_btn->isEnabled());
   if (map_settings_btn_bottom->isEnabled()) {

selfdrive/ui/qt/onroad/annotated_camera.h

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

selfdrive/ui/ui.cc

@@ -107,6 +107,11 @@ void update_model(UIState *s,
     update_line_data(s, road_edges[i], 0.025, 0, &scene.road_edge_vertices[i], max_idx);
   }
 
+  // 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, false);
+  }
+
   // update path
   auto lead_count = model.getLeadsV3().size();
   if (lead_count > 0) {
@@ -214,6 +219,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")) {
@@ -238,6 +245,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();
@@ -301,6 +310,8 @@ void ui_update_frogpilot_params(UIState *s, Params &params) {
   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");
@@ -312,6 +323,9 @@ void ui_update_frogpilot_params(UIState *s, Params &params) {
 
   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

@@ -123,7 +123,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;
@@ -155,6 +159,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;
@@ -168,6 +175,8 @@ typedef struct UIScene {
   int conditional_status;
   int tethering_config;
 
+  QPolygonF track_adjacent_vertices[6];
+
 } UIScene;
 
 class UIState : public QObject {