Human-Like Lane Changes

selfdrive/controls/radard.py

@@ -12,6 +12,7 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.simple_kalman import KF1D
+from openpilot.selfdrive.controls.lib.desire_helper import LaneChangeDirection, LaneChangeState
 
 from openpilot.frogpilot.common.frogpilot_variables import THRESHOLD, get_frogpilot_toggles
 
@@ -155,8 +156,20 @@ def laplacian_pdf(x: float, mu: float, b: float):
   return math.exp(-abs(x-mu)/b)
 
 
-def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track], frogpilot_toggles: SimpleNamespace):
+def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader, tracks: dict[int, Track], frogpilot_toggles: SimpleNamespace):
   # FrogPilot variables
+  if model_data.meta.laneChangeState == LaneChangeState.laneChangeStarting and frogpilot_toggles.human_lane_changes:
+    direction = model_data.meta.laneChangeDirection
+
+    if direction == LaneChangeDirection.left:
+      left_tracks = [track for track in tracks.values() if track.leadLeft]
+      if left_tracks:
+        return min(left_tracks, key=lambda c: c.dRel)
+
+    elif direction == LaneChangeDirection.right:
+      right_tracks = [track for track in tracks.values() if track.leadRight]
+      if right_tracks:
+        return min(right_tracks, key=lambda c: c.dRel)
 
   offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
 
@@ -204,7 +217,7 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn
              low_speed_override: bool = True) -> dict[str, Any]:
   # Determine leads, this is where the essential logic happens
   if len(tracks) > 0 and ready and lead_msg.prob > .5:
-    track = match_vision_to_track(v_ego, lead_msg, tracks, frogpilot_toggles)
+    track = match_vision_to_track(v_ego, lead_msg, model_data, tracks, frogpilot_toggles)
   else:
     track = None
 
@@ -314,7 +327,7 @@ class RadarD:
       self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'], self.frogpilot_toggles, low_speed_override=False)
 
     # FrogPilot variables
-    if self.ready and (self.frogpilot_toggles.adjacent_lead_tracking):
+    if self.ready and (self.frogpilot_toggles.adjacent_lead_tracking or self.frogpilot_toggles.human_lane_changes):
       self.frogpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['modelV2'], left=True)
       self.frogpilot_radar_state.leadRight = get_adjacent_lead(self.tracks, sm['modelV2'], left=False)