Blind spot path

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

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"};