Blind spot path

Added toggle to display a red path in the adjacent lane when a vehicle is detected in the blind spot.

cereal/custom.capnp

@@ -23,6 +23,8 @@ struct FrogPilotNavigation @0xda96579883444c35 {
 
 struct FrogPilotPlan @0x80ae746ee2596b11 {
   jerk @7 :Float32;
+  laneWidthLeft @8 :Float32;
+  laneWidthRight @9 :Float32;
   minAcceleration @10 :Float32;
   maxAcceleration @11 :Float32;
   tFollow @20 :Float32;

common/params.cc

@@ -217,6 +217,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"AlwaysOnLateralMain", PERSISTENT},
     {"ApiCache_DriveStats", PERSISTENT},
     {"AutomaticUpdates", PERSISTENT},
+    {"BlindSpotPath", PERSISTENT},
     {"CustomAlerts", PERSISTENT},
     {"CustomPaths", PERSISTENT},
     {"CustomUI", PERSISTENT},

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -7,6 +7,7 @@ from openpilot.common.numpy_fast import interp
 from openpilot.common.params import Params
 
 from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
 from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, STOP_DISTANCE, get_jerk_factor, get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
 from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel
@@ -75,6 +76,14 @@ class FrogPilotPlanner:
     else:
       self.min_accel = ACCEL_MIN
 
+    check_lane_width = 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]))
+    else:
+      self.lane_width_left = 0
+      self.lane_width_right = 0
+
     road_curvature = calculate_road_curvature(modelData, v_ego)
 
     if radarState.leadOne.status and self.CP.openpilotLongitudinalControl:
@@ -124,6 +133,8 @@ class FrogPilotPlanner:
     frogpilotPlan = frogpilot_plan_send.frogpilotPlan
 
     frogpilotPlan.jerk = float(self.jerk)
+    frogpilotPlan.laneWidthLeft = self.lane_width_left
+    frogpilotPlan.laneWidthRight = self.lane_width_right
     frogpilotPlan.minAcceleration = self.min_accel
     frogpilotPlan.maxAcceleration = self.max_accel
     frogpilotPlan.tFollow = float(self.t_follow)
@@ -137,6 +148,7 @@ class FrogPilotPlanner:
     custom_alerts = self.params.get_bool("CustomAlerts")
 
     custom_ui = self.params.get_bool("CustomUI")
+    self.blind_spot_path = custom_ui and self.params.get_bool("BlindSpotPath")
 
     longitudinal_tune = self.CP.openpilotLongitudinalControl and self.params.get_bool("LongitudinalTune")
     self.acceleration_profile = self.params.get_int("AccelerationProfile") if longitudinal_tune else 0

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

@@ -62,8 +62,8 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
       });
       toggle = customUIToggle;
     } else if (param == "CustomPaths") {
-      std::vector<QString> pathToggles{"AccelerationPath"};
-      std::vector<QString> pathToggleNames{tr("Acceleration")};
+      std::vector<QString> pathToggles{"AccelerationPath", "BlindSpotPath"};
+      std::vector<QString> pathToggleNames{tr("Acceleration"), tr("Blind Spot")};
       toggle = new FrogPilotParamToggleControl(param, title, desc, icon, pathToggles, pathToggleNames);
 
     } else if (param == "QOLVisuals") {

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)
-  for i in range(4):
-    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])
+  modelV2.init('laneLines', 6)
+  for i in range(6):
+    if i < 4:
+      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

@@ -538,6 +538,22 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
   painter.setBrush(bg);
   painter.drawPolygon(scene.track_vertices);
 
+  // Paint blindspot path
+  if (scene.blind_spot_path) {
+    QLinearGradient bs(0, height(), 0, 0);
+    bs.setColorAt(0.0, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.6));
+    bs.setColorAt(0.5, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.4));
+    bs.setColorAt(1.0, QColor::fromHslF(0 / 360., 0.75, 0.50, 0.2));
+
+    painter.setBrush(bs);
+    if (blindSpotLeft) {
+      painter.drawPolygon(scene.track_adjacent_vertices[4]);
+    }
+    if (blindSpotRight) {
+      painter.drawPolygon(scene.track_adjacent_vertices[5]);
+    }
+  }
+
   painter.restore();
 }
 
@@ -737,6 +753,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;
+
   experimentalMode = scene.experimental_mode;
 
   mapOpen = scene.map_open;

selfdrive/ui/qt/onroad.h

@@ -119,6 +119,8 @@ private:
   QHBoxLayout *bottom_layout;
 
   bool alwaysOnLateralActive;
+  bool blindSpotLeft;
+  bool blindSpotRight;
   bool experimentalMode;
   bool mapOpen;
   bool showAlwaysOnLateralStatusBar;

selfdrive/ui/ui.cc

@@ -115,6 +115,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) {
@@ -207,6 +212,8 @@ static void update_state(UIState *s) {
   if (sm.updated("carState")) {
     auto carState = sm["carState"].getCarState();
     scene.acceleration = carState.getAEgo();
+    scene.blind_spot_left = carState.getLeftBlindspot();
+    scene.blind_spot_right = carState.getRightBlindspot();
   }
   if (sm.updated("controlsState")) {
     auto controlsState = sm["controlsState"].getControlsState();
@@ -223,6 +230,8 @@ static void update_state(UIState *s) {
   }
   if (sm.updated("frogpilotPlan")) {
     auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan();
+    scene.lane_width_left = frogpilotPlan.getLaneWidthLeft();
+    scene.lane_width_right = frogpilotPlan.getLaneWidthRight();
   }
   if (sm.updated("liveLocationKalman")) {
     auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman();
@@ -257,6 +266,7 @@ 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.blind_spot_path = custom_paths && params.getBool("BlindSpotPath");
 
   bool quality_of_life_controls = params.getBool("QOLControls");
 

selfdrive/ui/ui.h

@@ -173,6 +173,9 @@ typedef struct UIScene {
   // FrogPilot variables
   bool acceleration_path;
   bool always_on_lateral_active;
+  bool blind_spot_left;
+  bool blind_spot_path;
+  bool blind_spot_right;
   bool enabled;
   bool experimental_mode;
   bool map_open;
@@ -180,9 +183,13 @@ typedef struct UIScene {
   bool show_aol_status_bar;
 
   float acceleration;
+  float lane_width_left;
+  float lane_width_right;
 
   int alert_size;
 
+  QPolygonF track_adjacent_vertices[6];
+
 } UIScene;
 
 class UIState : public QObject {