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

@@ -19,6 +19,8 @@ struct FrogPilotNavigation @0xf35cc4560bbf6ec2 {
 }
 
 struct FrogPilotPlan @0xda96579883444c35 {
+  laneWidthLeft @3 :Float32;
+  laneWidthRight @4 :Float32;
 }
 
 struct CustomReserved4 @0x80ae746ee2596b11 {

common/params.cc

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

selfdrive/frogpilot/functions/frogpilot_functions.py

@@ -43,3 +43,17 @@ class FrogPilotFunctions:
   @staticmethod
   def get_max_accel_sport(v_ego):
     return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT)
+
+  @staticmethod
+  def calculate_lane_width(lane, current_lane, road_edge):
+    lane_x, lane_y = np.array(lane.x), np.array(lane.y)
+    edge_x, edge_y = np.array(road_edge.x), np.array(road_edge.y)
+    current_x, current_y = np.array(current_lane.x), np.array(current_lane.y)
+
+    lane_y_interp = np.interp(current_x, lane_x[lane_x.argsort()], lane_y[lane_x.argsort()])
+    road_edge_y_interp = np.interp(current_x, edge_x[edge_x.argsort()], edge_y[edge_x.argsort()])
+
+    distance_to_lane = np.mean(np.abs(current_y - lane_y_interp))
+    distance_to_road_edge = np.mean(np.abs(current_y - road_edge_y_interp))
+
+    return min(distance_to_lane, distance_to_road_edge)

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -2,6 +2,7 @@ import cereal.messaging as messaging
 
 from openpilot.common.conversions import Conversions as CV
 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.longitudinal_planner import A_CRUISE_MIN, get_max_accel
 
 from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED, FrogPilotFunctions
@@ -44,6 +45,15 @@ class FrogPilotPlanner:
 
     self.accel_limits = [min_accel, max_accel]
 
+    # Update the current lane widths
+    check_lane_width = 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]))
+    else:
+      self.lane_width_left = 0
+      self.lane_width_right = 0
+
     # Update the max allowed speed
     self.v_cruise = self.update_v_cruise(carState, controlsState, enabled, modelData, v_cruise, v_ego)
 
@@ -65,12 +75,16 @@ class FrogPilotPlanner:
     frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
     frogpilotPlan = frogpilot_plan_send.frogpilotPlan
 
+    frogpilotPlan.laneWidthLeft = self.lane_width_left
+    frogpilotPlan.laneWidthRight = self.lane_width_right
+
     pm.send('frogpilotPlan', frogpilot_plan_send)
 
   def update_frogpilot_params(self, params):
     self.is_metric = params.get_bool("IsMetric")
 
     custom_ui = params.get_bool("CustomUI")
+    self.blind_spot_path = custom_ui and params.get_bool("BlindSpotPath")
 
     longitudinal_tune = params.get_bool("LongitudinalTune")
     self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0

selfdrive/frogpilot/ui/visual_settings.cc

@@ -15,6 +15,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.", ""},
+    {"BlindSpotPath", "Blind Spot Path", "Visualize your blind spots with a red path when another vehicle is detected nearby.", ""},
 
     {"QOLVisuals", "Quality of Life", "Miscellaneous quality of life changes to improve your overall openpilot experience.", "../frogpilot/assets/toggle_icons/quality_of_life.png"},
     {"DriveStats", "Drive Stats In Home Screen", "Display your device's drive stats in the home screen.", ""},

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"};
+  std::set<QString> customOnroadUIKeys = {"AccelerationPath", "BlindSpotPath"};
   std::set<QString> customThemeKeys = {};
   std::set<QString> modelUIKeys = {};
   std::set<QString> qolKeys = {"DriveStats"};

selfdrive/ui/qt/onroad.cc

@@ -537,6 +537,24 @@ 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);
+    if (blindSpotLeft || blindSpotRight) {
+      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 +755,9 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
   alwaysOnLateral = scene.always_on_lateral;
   alwaysOnLateralActive = scene.always_on_lateral_active;
 
+  blindSpotLeft = scene.blind_spot_left;
+  blindSpotRight = scene.blind_spot_right;
+
   experimentalMode = scene.experimental_mode;
 
   if (alwaysOnLateral) {

selfdrive/ui/qt/onroad.h

@@ -120,6 +120,8 @@ private:
 
   bool alwaysOnLateral;
   bool alwaysOnLateralActive;
+  bool blindSpotLeft;
+  bool blindSpotRight;
   bool experimentalMode;
 
 protected:

selfdrive/ui/ui.cc

@@ -114,6 +114,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], scene.blind_spot_path ? (i == 4 ? scene.lane_width_left : scene.lane_width_right) / 2 : 0, 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) {
@@ -202,6 +207,10 @@ static void update_state(UIState *s) {
   }
   if (sm.updated("carState")) {
     auto carState = sm["carState"].getCarState();
+    if (scene.blind_spot_path) {
+      scene.blind_spot_left = carState.getLeftBlindspot();
+      scene.blind_spot_right = carState.getRightBlindspot();
+    }
   }
   if (sm.updated("controlsState")) {
     auto controlsState = sm["controlsState"].getControlsState();
@@ -216,6 +225,10 @@ static void update_state(UIState *s) {
   }
   if (sm.updated("frogpilotPlan")) {
     auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan();
+    if (scene.blind_spot_path) {
+      scene.lane_width_left = frogpilotPlan.getLaneWidthLeft();
+      scene.lane_width_right = frogpilotPlan.getLaneWidthRight();
+    }
   }
   if (sm.updated("liveLocationKalman")) {
     auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman();
@@ -250,6 +263,7 @@ void ui_update_frogpilot_params(UIState *s) {
 
   bool custom_onroad_ui = params.getBool("CustomUI");
   scene.acceleration_path = custom_onroad_ui && params.getBool("AccelerationPath");
+  scene.blind_spot_path = custom_onroad_ui && params.getBool("BlindSpotPath");
 
   bool quality_of_life_controls = params.getBool("QOLControls");
 

selfdrive/ui/ui.h

@@ -172,9 +172,17 @@ typedef struct UIScene {
   bool acceleration_path;
   bool always_on_lateral;
   bool always_on_lateral_active;
+  bool blind_spot_left;
+  bool blind_spot_path;
+  bool blind_spot_right;
   bool enabled;
   bool experimental_mode;
 
+  float lane_width_left;
+  float lane_width_right;
+
+  QPolygonF track_adjacent_vertices[6];
+
 } UIScene;
 
 class UIState : public QObject {