From 90883515fb4fd3357e2706921b45265246722cfd Mon Sep 17 00:00:00 2001 From: James <91348155+FrogAi@users.noreply.github.com> Date: Mon, 1 Dec 2025 12:00:00 -0700 Subject: [PATCH] Human-Like Lane Changes --- selfdrive/controls/radard.py | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index e94f7baa6..736f7644f 100644 --- a/selfdrive/controls/radard.py +++ b/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)