FrogPilot features - Lane detection

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -14,6 +14,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHA
                                                                            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
 
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import calculate_lane_width
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED
 
 GearShifter = car.CarState.GearShifter
@@ -60,6 +61,13 @@ class FrogPilotPlanner:
       self.acceleration_jerk = self.base_acceleration_jerk
       self.speed_jerk = self.base_speed_jerk
 
+    if 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]))
+    else:
+      self.lane_width_left = 0
+      self.lane_width_right = 0
+
     self.v_cruise = self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego, frogpilot_toggles)
 
     if v_ego > CRUISING_SPEED:
@@ -89,6 +97,9 @@ class FrogPilotPlanner:
     frogpilotPlan.speedJerk = J_EGO_COST * float(self.speed_jerk)
     frogpilotPlan.speedJerkStock = J_EGO_COST * float(self.base_speed_jerk)
 
+    frogpilotPlan.laneWidthLeft = self.lane_width_left
+    frogpilotPlan.laneWidthRight = self.lane_width_right
+
     frogpilotPlan.maxAcceleration = self.max_accel
     frogpilotPlan.minAcceleration = self.min_accel
 

selfdrive/frogpilot/controls/lib/frogpilot_functions.py

@@ -1,6 +1,7 @@
 import datetime
 import filecmp
 import glob
+import numpy as np
 import os
 import shutil
 import subprocess
@@ -10,6 +11,19 @@ from openpilot.common.params_pyx import Params, UnknownKeyName
 from openpilot.system.hardware import HARDWARE
 from openpilot.system.version import get_build_metadata
 
+def calculate_lane_width(lane, current_lane, road_edge):
+  current_x, current_y = np.array(current_lane.x), np.array(current_lane.y)
+  edge_x, edge_y = np.array(road_edge.x), np.array(road_edge.y)
+  lane_x, lane_y = np.array(lane.x), np.array(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)
+
 def run_cmd(cmd, success_msg, fail_msg):
   try:
     subprocess.check_call(cmd)