From 93b30efa594a46c41d25579c1c45fade43df51bc Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Wed, 12 Jun 2024 12:53:27 -0700
Subject: [PATCH] FrogPilot features - Lane detection

---
 selfdrive/frogpilot/controls/frogpilot_planner.py  | 11 +++++++++++
 .../frogpilot/controls/lib/frogpilot_functions.py  | 14 ++++++++++++++
 2 files changed, 25 insertions(+)

diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py
index ee28ea941..e7d5b8c9e 100644
--- a/selfdrive/frogpilot/controls/frogpilot_planner.py
+++ b/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
 
diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_functions.py b/selfdrive/frogpilot/controls/lib/frogpilot_functions.py
index d2b040c7b..6022aac70 100644
--- a/selfdrive/frogpilot/controls/lib/frogpilot_functions.py
+++ b/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)