From 3027988a4da80be0cc3a28d1fb146a186db7e72e Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 3 Jun 2026 15:03:40 -0500 Subject: [PATCH] Purple Monkey Balls --- .../controls/lib/longitudinal_planner.py | 96 ++++++++++++++----- .../tests/test_longitudinal_planner.py | 52 ++++++++-- 2 files changed, 117 insertions(+), 31 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 195a6c560..654375acb 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -45,8 +45,14 @@ LEAD_DEPART_CONFIDENT_MAX_GAP = 5.25 LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED = 0.3 LEAD_DEPART_CONFIDENT_MIN_LEAD_DELTA = 0.25 LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL = 0.2 -RADAR_ONLY_DEPART_HOLD_MAX_EGO_SPEED = 1.6 -RADAR_ONLY_DEPART_HOLD_MAX_DISTANCE = 18.0 +RADAR_DEPART_CONFLICT_MAX_EGO_SPEED = 1.6 +RADAR_DEPART_CONFLICT_MIN_RADAR_LATERAL = 1.5 +RADAR_DEPART_CONFLICT_MAX_RADAR_DISTANCE = 18.0 +RADAR_DEPART_CONFLICT_MIN_MODEL_PROB = 0.95 +RADAR_DEPART_CONFLICT_MAX_MODEL_DISTANCE = 18.0 +RADAR_DEPART_CONFLICT_MAX_MODEL_LATERAL = 0.9 +RADAR_DEPART_CONFLICT_MAX_MODEL_LEAD_SPEED = 2.0 +RADAR_DEPART_CONFLICT_MAX_DISTANCE_MISMATCH = 4.0 LEAD_DEPART_ACCEL_HOLD_TIME = 1.2 LEAD_DEPART_ACCEL_HOLD_MAX_EGO_SPEED = 1.5 LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED = 0.6 @@ -962,8 +968,6 @@ class LongitudinalPlanner: return False lead_radar = bool(getattr(lead, "radar", False)) - if lead_radar: - return False lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) if not lead_radar and lead_prob < LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_PROB: return False @@ -979,13 +983,68 @@ class LongitudinalPlanner: lead_accel >= LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL ) + @staticmethod + def get_centered_model_lead(model_data): + try: + leads = model_data.leadsV3 + except Exception: + return None + + best_candidate = None + for i in range(3): + try: + lead = leads[i] + prob = float(lead.prob) + x = float(lead.x[0]) + y = float(lead.y[0]) + v = float(lead.v[0]) + except Exception: + continue + + if ( + prob < RADAR_DEPART_CONFLICT_MIN_MODEL_PROB or + x <= 0.0 or + x > RADAR_DEPART_CONFLICT_MAX_MODEL_DISTANCE or + abs(y) > RADAR_DEPART_CONFLICT_MAX_MODEL_LATERAL or + max(v, 0.0) > RADAR_DEPART_CONFLICT_MAX_MODEL_LEAD_SPEED + ): + continue + + if best_candidate is None or x < best_candidate[0]: + best_candidate = (x, y, v, prob) + + return best_candidate + + def has_offcenter_radar_depart_conflict(self, sm): + if float(getattr(sm["carState"], "vEgo", 0.0)) > RADAR_DEPART_CONFLICT_MAX_EGO_SPEED: + return False + + centered_model_lead = self.get_centered_model_lead(sm["modelV2"]) + if centered_model_lead is None: + return False + + centered_model_dist = float(centered_model_lead[0]) + for lead in (self.lead_one, self.lead_two): + if not lead.status or not bool(getattr(lead, "radar", False)): + continue + + lead_dist = float(getattr(lead, "dRel", 0.0)) + if lead_dist <= 0.0 or lead_dist > RADAR_DEPART_CONFLICT_MAX_RADAR_DISTANCE: + continue + if abs(float(getattr(lead, "yRel", 0.0))) < RADAR_DEPART_CONFLICT_MIN_RADAR_LATERAL: + continue + if abs(lead_dist - centered_model_dist) > RADAR_DEPART_CONFLICT_MAX_DISTANCE_MISMATCH: + continue + + return True + + return False + def get_lead_depart_accel_floor(self, lead, v_ego, model_desired_accel): if lead is None or not lead.status: return None lead_radar = bool(getattr(lead, "radar", False)) - if lead_radar: - return None lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) if not lead_radar and lead_prob < LEAD_DEPART_ACCEL_HOLD_MIN_MODEL_PROB: return None @@ -1909,35 +1968,26 @@ class LongitudinalPlanner: standstill_nudge_gap = max(float(getattr(starpilot_toggles, "stop_distance", STOP_DISTANCE)), STOP_DISTANCE) - 0.5 moving_leads = [lead for lead in (self.lead_one, self.lead_two) - if lead.status and not bool(getattr(lead, "radar", False)) and + if lead.status and lead.vLead > STANDSTILL_LEAD_NUDGE_MIN_SPEED and lead.dRel >= standstill_nudge_gap] confident_depart_ready = any(self.is_confident_lead_depart(lead, float(sm['carState'].vEgo)) for lead in (self.lead_one, self.lead_two)) lead_depart_ready = any( lead.status and - not bool(getattr(lead, "radar", False)) and lead.vLead >= STANDSTILL_LEAD_DEPART_MIN_LEAD_SPEED and lead.dRel >= standstill_nudge_gap + STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN for lead in (self.lead_one, self.lead_two) ) - radar_depart_hold = bool( - float(sm['carState'].vEgo) <= RADAR_ONLY_DEPART_HOLD_MAX_EGO_SPEED and - any( - lead.status and - bool(getattr(lead, "radar", False)) and - float(getattr(lead, "dRel", 0.0)) > 0.0 and - float(getattr(lead, "dRel", 0.0)) <= RADAR_ONLY_DEPART_HOLD_MAX_DISTANCE - for lead in (self.lead_one, self.lead_two) - ) - ) + depart_safety_veto = self.has_offcenter_radar_depart_conflict(sm) - if lead_control_active and sm['carState'].standstill and moving_leads: + if lead_control_active and sm['carState'].standstill and moving_leads and not depart_safety_veto: output_a_target = max(output_a_target, STANDSTILL_LEAD_NUDGE_ACCEL) if ( lead_control_active and sm['carState'].standstill and (confident_depart_ready or lead_depart_ready) and + not depart_safety_veto and not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and (confident_depart_ready or model_desired_accel >= STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL) @@ -1946,14 +1996,14 @@ class LongitudinalPlanner: output_should_stop = False output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL) - if lead_control_active and lead_depart_ready and not output_should_stop and float(sm['carState'].vEgo) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED: + if lead_control_active and lead_depart_ready and not depart_safety_veto and not output_should_stop and float(sm['carState'].vEgo) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED: output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL) - if output_should_stop or bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) or bool(getattr(sm['starpilotPlan'], 'redLight', False)): + if depart_safety_veto or output_should_stop or bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) or bool(getattr(sm['starpilotPlan'], 'redLight', False)): self.lead_depart_accel_hold_until = 0.0 lead_depart_accel_floor = None - if lead_control_active and not output_should_stop: + if lead_control_active and not output_should_stop and not depart_safety_veto: lead_depart_accel_floors = [ floor for floor in ( self.get_lead_depart_accel_floor(self.lead_one, scene_v_ego, model_desired_accel), @@ -2145,7 +2195,7 @@ class LongitudinalPlanner: self.a_desired = min(self.a_desired, close_release_hold_cap) output_a_target = min(output_a_target, close_release_hold_cap) - if radar_depart_hold and lead_depart_accel_floor is None and not confident_depart_ready and not lead_depart_ready: + if depart_safety_veto: self.a_desired = min(self.a_desired, 0.0) output_a_target = min(output_a_target, 0.0) if sm['carState'].standstill: diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 78519e48b..416fc0f42 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -17,7 +17,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants, Plan def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead: float = 0.0, - radar: bool = False, model_prob: float = 0.0): + radar: bool = False, model_prob: float = 0.0, y_rel: float = 0.0): lead = log.RadarState.LeadData.new_message() lead.status = status lead.dRel = d_rel @@ -26,6 +26,7 @@ def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead lead.aLeadK = a_lead lead.vRel = 0.0 lead.aRel = 0.0 + lead.yRel = y_rel lead.modelProb = model_prob lead.radar = radar return lead @@ -33,6 +34,7 @@ def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0, brake_press_prob: float = 0.0): model = log.ModelDataV2.new_message() + model.init('leadsV3', 3) t_idxs = ModelConstants.T_IDXS model.position.x = [float(v_ego * t) for t in t_idxs] @@ -57,6 +59,15 @@ def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0, return model +def set_model_lead(model, idx: int, *, prob: float, x0: float, y0: float, v0: float, a0: float = 0.0): + lead = model.leadsV3[idx] + lead.prob = float(prob) + lead.x = [float(x0)] + lead.y = [float(y0)] + lead.v = [float(v0)] + lead.a = [float(a0)] + + def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimental_mode: bool = True, tracking_lead: bool = False, lead_one=None, lead_two=None, gas_press_prob: float = 1.0, brake_press_prob: float = 0.0, disable_throttle: bool = False): @@ -1233,7 +1244,7 @@ def test_standstill_moving_lead_depart_accel_hold_cancels_if_lead_brakes(model_v @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_standstill_radar_only_lead_does_not_trigger_depart_accel(model_version): +def test_standstill_radar_depart_kept_when_radar_lead_is_centered(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) @@ -1243,21 +1254,45 @@ def test_standstill_radar_only_lead_does_not_trigger_depart_accel(model_version) min_accel=-0.5, experimental_mode=False, tracking_lead=False, - lead_one=make_lead(status=True, d_rel=11.2, v_lead=0.63, a_lead=0.36, radar=True, model_prob=0.998), + lead_one=make_lead(status=True, d_rel=11.2, v_lead=0.63, a_lead=0.36, radar=True, model_prob=0.998, y_rel=0.2), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False + set_model_lead(sm["modelV2"], 0, prob=0.999, x0=12.2, y0=0.03, v0=0.4) planner.update(sm, make_toggles(model_version)) - assert planner.output_should_stop - assert planner.output_a_target <= 0.0 + assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) -def test_low_speed_radar_only_lead_does_not_trigger_depart_accel_hold(model_version): +def test_standstill_radar_depart_blocks_offcenter_radar_conflict(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + + sm = make_sm( + 0.0, + desired_accel=0.45, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=11.2, v_lead=0.63, a_lead=0.36, radar=True, model_prob=0.998, y_rel=2.3), + ) + sm["carState"].standstill = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["starpilotPlan"].vCruise = 10.0 + sm["modelV2"].action.shouldStop = False + set_model_lead(sm["modelV2"], 0, prob=0.999, x0=12.2, y0=0.03, v0=0.4) + + planner.update(sm, make_toggles(model_version)) + + assert planner.output_a_target < longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_low_speed_radar_depart_hold_blocks_offcenter_radar_conflict(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=1.25) @@ -1267,16 +1302,17 @@ def test_low_speed_radar_only_lead_does_not_trigger_depart_accel_hold(model_vers min_accel=-0.5, experimental_mode=False, tracking_lead=False, - lead_one=make_lead(status=True, d_rel=9.95, v_lead=0.43, a_lead=0.44, radar=True, model_prob=0.999), + lead_one=make_lead(status=True, d_rel=9.95, v_lead=0.43, a_lead=0.44, radar=True, model_prob=0.999, y_rel=2.2), ) sm["carState"].standstill = False sm["controlsState"].longControlState = LongCtrlState.pid sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False + set_model_lead(sm["modelV2"], 0, prob=0.999, x0=11.4, y0=0.0, v0=0.2) planner.update(sm, make_toggles(model_version)) - assert planner.output_a_target <= 0.0 + assert planner.output_a_target < longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])