From 801654fd8d445a45109a51f92ec6a72b614c783e Mon Sep 17 00:00:00 2001 From: Prabhaav Pillai Date: Thu, 27 Aug 2026 02:03:33 -0400 Subject: [PATCH] Add HEM telemetry exporter and update forensic analysis tools - Introduced `export_hem_telemetry.py` to extract and save telemetry data from target segments into a JSON file, including key selfdriveState fields. - Modified `hem_forensic.py` to incorporate new diagnostic keys and update the logic for authority and stop detection. - Enhanced `hem_stop_analyzer.py` to analyze stop detection failures with new metrics and improved comments for clarity. - Updated `mode_sim.py` to reflect changes in hybrid mode updates and authority handling. --- .../controls/lib/longitudinal_planner.py | 82 ++-- .../tests/test_longitudinal_planner.py | 12 +- .../tests/test_longitudinal_planner_hem.py | 284 ++++++++++++ selfdrive/ui/lib/starpilot_status.py | 4 +- .../controls/lib/hybrid_experimental_mode.py | 398 +++++------------ .../tests/test_hybrid_experimental_mode.py | 412 +++++++++--------- tools/replay/export_hem_telemetry.py | 198 +++++++++ tools/replay/hem_forensic.py | 67 +-- tools/replay/hem_stop_analyzer.py | 47 +- tools/replay/hem_stop_analyzer.py.py | 47 +- tools/replay/mode_sim.py | 7 +- 11 files changed, 940 insertions(+), 618 deletions(-) create mode 100644 selfdrive/controls/tests/test_longitudinal_planner_hem.py create mode 100644 tools/replay/export_hem_telemetry.py diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 775084e1e..3d9111bbf 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -148,7 +148,7 @@ def _hem_log_timestamp() -> str: def _hem_format_diag_value(value): """Compact, deterministic formatting for the per-frame HEM diagnostic dump.""" - if isinstance(value, bool): + if isinstance(value, (bool, np.bool_)): return "1" if value else "0" if isinstance(value, float): return f"{value:.4f}" @@ -2354,33 +2354,8 @@ class LongitudinalPlanner: action_t=action_t, vEgoStopping=starpilot_toggles.vEgoStopping) if bool(getattr(starpilot_toggles, "hybrid_experimental_mode", False)): - self.hybrid_controller.set_tuning( - getattr(starpilot_toggles, "hybrid_exp_bias", 0.0), - getattr(starpilot_toggles, "hybrid_vision_brake_sensitivity", 1.0), - ) - output_a_target_e2e = sm['modelV2'].action.desiredAcceleration - output_should_stop_e2e = sm['modelV2'].action.shouldStop - output_a_target = self.hybrid_controller.update( - v_ego=scene_v_ego, - v_cruise=v_cruise, - lead_one=self.lead_one, - model_v2=sm['modelV2'], - a_chill=output_a_target_mpc, - a_exp=output_a_target_e2e, - t_follow=effective_t_follow, - ) - hc = self.hybrid_controller - hc.record_diag = True - # Surface whether Chill/Exp explicitly asked to stop; critical for diagnosing - # cases where Exp wanted to brake but the HEM fusion did not command a stop. - hc.diag["should_stop_mpc"] = bool(output_should_stop_mpc) - hc.diag["should_stop_e2e"] = bool(output_should_stop_e2e) - hc.diag["should_stop_fused"] = bool(output_should_stop_mpc or output_should_stop_e2e) - hc.diag["model_desired_accel"] = float(sm['modelV2'].action.desiredAcceleration) - hc.diag["a_out"] = float(output_a_target) - self._log_hem_status(now_t, True, scene_v_ego, output_a_target_mpc, output_a_target_e2e, output_a_target) - self._publish_hem_status(now_t) - output_should_stop = output_should_stop_mpc or output_should_stop_e2e + output_a_target = output_a_target_mpc + output_should_stop = output_should_stop_mpc elif tinygrad_model and self.mode != 'acc' and self.generation != 'v9': output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop @@ -2998,8 +2973,55 @@ class LongitudinalPlanner: if force_slow_decel and scene_v_ego > 0.1: output_a_target = min(output_a_target, FORCE_DECEL_MIN_ACCEL) - self.output_a_target = output_a_target - self.output_should_stop = bool(output_should_stop or vision_low_speed_stop_active) + a_chill_final = output_a_target + should_stop_chill = bool(output_should_stop or vision_low_speed_stop_active) + + if bool(getattr(starpilot_toggles, "hybrid_experimental_mode", False)): + self.hybrid_controller.set_tuning( + getattr(starpilot_toggles, "hybrid_exp_bias", 0.0), + getattr(starpilot_toggles, "hybrid_vision_brake_sensitivity", 1.0), + ) + a_exp_raw = float(sm['modelV2'].action.desiredAcceleration) + should_stop_exp = bool(sm['modelV2'].action.shouldStop) + + # Pass the active lead that MPC is tracking (leadTwo when source == "lead1") + # so HEM is never blind to the radar lead actually being followed. + active_lead = self.lead_two if self.mpc.source == "lead1" else self.lead_one + + a_fused, should_stop_fused = self.hybrid_controller.update( + v_ego=scene_v_ego, + v_cruise=v_cruise, + lead_one=active_lead, + model_v2=sm['modelV2'], + a_chill=a_chill_final, + a_exp=a_exp_raw, + should_stop_exp=should_stop_exp, + should_stop_chill=should_stop_chill, + ) + + # HEM output can never exceed the physical + # vehicle acceleration envelope (same bounds as the non-hybrid path) or a + # per-frame jerk slew from the previously commanded target, regardless of + # tuning bias. + a_fused = float(np.clip(a_fused, output_accel_min, output_accel_max)) + if not np.isfinite(a_fused): + a_fused = float(a_chill_final) + + max_jerk_accel = float(getattr(sm['starpilotPlan'], 'accelerationJerk', 1.0)) * 3.0 + max_jerk_brake = 4.0 + max_delta_up = max_jerk_accel * self.dt + max_delta_down = max_jerk_brake * self.dt + prev_target = float(prev_output_a_target) + a_fused = float(np.clip(a_fused, prev_target - max_delta_down, prev_target + max_delta_up)) + + self.output_a_target = a_fused + self.output_should_stop = should_stop_fused + self._log_hem_status(now_t, True, scene_v_ego, a_chill_final, a_exp_raw, a_fused) + self._publish_hem_status(now_t) + + else: + self.output_a_target = a_chill_final + self.output_should_stop = should_stop_chill def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan') diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index de77517c1..2f927e2f7 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -514,21 +514,23 @@ def test_experimental_mlsim_uses_vehicle_min_accel_floor(model_version): assert planner.output_a_target < comfort_min_accel -def test_hybrid_mode_shields_unconfirmed_vision_braking(): +def test_hybrid_mode_tempers_unconfirmed_vision_braking_with_chill(): v_ego = 20.0 desired_accel = -2.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) - # Hybrid mode clamps unconfirmed vision braking to a gentle coast (no lead, - # model not predicting a stop), instead of passing the raw E2E target. + # HEM is the final arbitrator: on open road (no lead, model not predicting a + # stop) it blends the raw E2E brake with the conservative Chill target rather + # than passing the full -2.0 straight through, but it still brakes (no creep). hybrid_toggles = SimpleNamespace(**vars(make_toggles("v11")), hybrid_experimental_mode=True) planner_hybrid = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm(v_ego, desired_accel, -2.0, experimental_mode=False) planner_hybrid.update(sm, hybrid_toggles) assert planner_hybrid.mode == "acc" - assert planner_hybrid.output_a_target >= -0.6 + assert planner_hybrid.output_a_target < 0.0, "HEM must still brake on a strong Exp decel" + assert planner_hybrid.output_a_target > desired_accel, "HEM must temper the raw E2E brake with Chill" - # Without the hybrid shield, experimental mode lets the raw E2E target through. + # Without HEM, experimental mode lets the raw E2E target through. planner_exp = LongitudinalPlanner(CP, init_v=v_ego) sm_exp = make_sm(v_ego, desired_accel, -2.0, experimental_mode=True) planner_exp.update(sm_exp, make_toggles("v11")) diff --git a/selfdrive/controls/tests/test_longitudinal_planner_hem.py b/selfdrive/controls/tests/test_longitudinal_planner_hem.py new file mode 100644 index 000000000..4f5915891 --- /dev/null +++ b/selfdrive/controls/tests/test_longitudinal_planner_hem.py @@ -0,0 +1,284 @@ +#!/usr/bin/env python3 +import numpy as np +import pytest +from types import SimpleNamespace + +from cereal import log +from opendbc.car.honda.interface import CarInterface +from opendbc.car.honda.values import CAR +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_vehicle_min_accel +from openpilot.selfdrive.modeld.constants import ModelConstants + +TRAJ_LEN = len(ModelConstants.T_IDXS) + + +def make_lead(*, status, d_rel=200.0, v_lead=0.0, a_lead=0.0, radar=False, model_prob=0.0): + lead = log.RadarState.LeadData.new_message() + lead.status = status + lead.dRel = d_rel + lead.vLead = v_lead + lead.vLeadK = v_lead + lead.aLeadK = a_lead + lead.vRel = 0.0 + lead.aRel = 0.0 + lead.yRel = 0.0 + lead.modelProb = model_prob + lead.radar = radar + return lead + + +def make_model(v_ego, desired_accel, *, velocity_traj=None, should_stop=False): + model = log.ModelDataV2.new_message() + model.init('leadsV3', 3) + t_idxs = ModelConstants.T_IDXS + n = len(t_idxs) + + model.position.x = [float(v_ego * t) for t in t_idxs] + model.position.y = [0.0] * n + model.position.z = [0.0] * n + model.position.t = [float(t) for t in t_idxs] + + if velocity_traj is None: + model.velocity.x = [float(v_ego)] * n + else: + model.velocity.x = [float(x) for x in velocity_traj] + model.velocity.y = [0.0] * n + model.velocity.z = [0.0] * n + model.velocity.t = [float(t) for t in t_idxs] + + model.acceleration.x = [0.0] * n + model.acceleration.y = [0.0] * n + model.acceleration.z = [0.0] * n + model.acceleration.t = [float(t) for t in t_idxs] + + model.action.desiredAcceleration = desired_accel + model.action.shouldStop = should_stop + return model + + +def make_sm(v_ego, desired_accel, min_accel, *, experimental_mode=True, tracking_lead=False, + lead_one=None, velocity_traj=None, should_stop=False): + if lead_one is None: + lead_one = make_lead(status=False) + return { + "carControl": SimpleNamespace(orientationNED=[0.0, 0.0, 0.0]), + "carState": SimpleNamespace( + vEgo=v_ego, vEgoCluster=v_ego, aEgo=0.0, vCruise=100.0, standstill=False, + steeringAngleDeg=0.0, + ), + "controlsState": SimpleNamespace(longControlState=0, forceDecel=False), + "liveParameters": SimpleNamespace(angleOffsetDeg=0.0), + "modelV2": make_model(v_ego, desired_accel, velocity_traj=velocity_traj, should_stop=should_stop), + "radarState": SimpleNamespace(leadOne=lead_one, leadTwo=make_lead(status=False)), + "selfdriveState": SimpleNamespace(enabled=True, experimentalMode=experimental_mode, personality=0), + "starpilotCarState": SimpleNamespace(accelPressed=False), + "starpilotPlan": SimpleNamespace( + vCruise=v_ego + 5.0, + minAcceleration=min_accel, + maxAcceleration=2.0, + disableThrottle=False, + trackingLead=tracking_lead, + accelerationJerk=5.0, + dangerJerk=5.0, + speedJerk=5.0, + dangerFactor=1.0, + tFollow=1.45, + forcingStop=False, + redLight=False, + forcingStopLength=2, + ), + } + + +def make_toggles(*, hybrid=False, exp_bias=0.0, sens=1.0): + return SimpleNamespace( + taco_tune=False, + classic_model=False, + tinygrad_model=True, + model_version="v11", + vEgoStopping=0.5, + radar_takeoffs=False, + hybrid_experimental_mode=hybrid, + hybrid_exp_bias=exp_bias, + hybrid_vision_brake_sensitivity=sens, + ) + + +def assert_outputs_equal(p1, p2, label=""): + assert p1.output_a_target == pytest.approx(p2.output_a_target, abs=1e-9), f"{label} aTarget mismatch" + assert p1.output_should_stop == p2.output_should_stop, f"{label} shouldStop mismatch" + np.testing.assert_allclose(p1.v_desired_trajectory, p2.v_desired_trajectory, atol=1e-9) + np.testing.assert_allclose(p1.a_desired_trajectory, p2.a_desired_trajectory, atol=1e-9) + + +# 1. Toggle equivalence (regression safety) +def _run_equivalence_scenarios(experimental_mode): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + + scenarios = [ + dict(v_ego=20.0, desired_accel=0.4, min_accel=-2.0, tracking_lead=False, + lead_one=None, velocity_traj=None, should_stop=False, label="chill-cruise"), + dict(v_ego=20.0, desired_accel=-0.6, min_accel=-2.0, tracking_lead=True, + lead_one=make_lead(status=True, d_rel=30.0, v_lead=18.0, radar=True, model_prob=1.0), + velocity_traj=None, should_stop=False, label="slow-lead"), + dict(v_ego=25.0, desired_accel=-1.2, min_accel=-3.0, tracking_lead=True, + lead_one=make_lead(status=True, d_rel=12.0, v_lead=3.0, radar=True, model_prob=1.0), + velocity_traj=list(np.linspace(25.0, 2.0, TRAJ_LEN)), should_stop=True, label="stop-approach"), + ] + + for scen in scenarios: + label = scen.pop("label") + hybrid_planner = LongitudinalPlanner(CP, init_v=scen["v_ego"]) + stock_planner = LongitudinalPlanner(CP, init_v=scen["v_ego"]) + sm = make_sm(experimental_mode=experimental_mode, **scen) + hybrid_planner.update(sm, make_toggles(hybrid=True) if False else make_toggles(hybrid=False)) + stock_planner.update(sm, make_toggles()) + assert_outputs_equal(hybrid_planner, stock_planner, label=f"chill/{label}" if not experimental_mode else f"exp/{label}") + + +def test_hem_off_matches_stock_chill_mode(): + _run_equivalence_scenarios(experimental_mode=False) + + +def test_hem_off_matches_stock_experimental_mode(): + _run_equivalence_scenarios(experimental_mode=True) + + +# 2. Stop sign / red light approach +def test_stop_sign_approach_commands_pure_model_braking(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=20.0) + toggles = make_toggles(hybrid=True) + sm = make_sm(20.0, -2.5, -4.0, experimental_mode=False, + velocity_traj=list(np.linspace(20.0, 0.0, TRAJ_LEN)), should_stop=True) + # Enough frames for the jerk slew limiter to ramp to full vision braking. + for _ in range(20): + planner.update(sm, toggles) + assert planner.output_a_target <= -2.0, "Exp stop braking must not be diluted by cruise throttle" + assert planner.output_a_target < 0.0, "Cruise throttle must be locked out during a vision stop" + + +def test_stop_line_handshake_asserts_should_stop(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.3) + toggles = make_toggles(hybrid=True) + sm = make_sm(0.3, -1.0, -4.0, experimental_mode=False, + velocity_traj=[0.0] * TRAJ_LEN, should_stop=True) + for _ in range(6): + planner.update(sm, toggles) + assert planner.output_should_stop, "Standstill at a predicted stop must assert shouldStop" + assert planner.output_a_target <= -0.4, "Standstill brake must be held at the stop line" + + +# 3. Slower-lead approach (radar + vision blend within safety) +def test_slower_lead_approach_blends_vision_prebrake_within_safety(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=20.0) + toggles = make_toggles(hybrid=True) + lead = make_lead(status=True, d_rel=25.0, v_lead=0.4, radar=True, model_prob=1.0) + sm = make_sm(20.0, -1.5, -4.0, experimental_mode=False, tracking_lead=True, lead_one=lead, + velocity_traj=list(np.linspace(20.0, 5.0, TRAJ_LEN)), should_stop=False) + for _ in range(15): + planner.update(sm, toggles) + # Vision pre-braking must be engaged while closing on a slower lead. + assert planner.output_a_target <= -1.0, "Vision pre-brake should be blended in on a slow lead" + # ...but it must never exceed the physical / commanded deceleration floors. + assert planner.output_a_target >= get_vehicle_min_accel(CP, 20.0) + assert planner.output_a_target >= -4.0 + + +# 4. Green light / lead departure (instant unlatch) +def test_green_light_departure_instantly_clears_should_stop(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.3) + toggles = make_toggles(hybrid=True) + + sm_stop = make_sm(0.3, -1.0, -4.0, experimental_mode=False, + velocity_traj=[0.0] * TRAJ_LEN, should_stop=True) + for _ in range(6): + planner.update(sm_stop, toggles) + assert planner.output_should_stop + assert planner.output_a_target <= -0.4 + + # Green light / lead pulls away: horizon ramps up and Exp accelerates. Run a + # few frames so the jerk slew ramps the commanded accel up from the brake hold. + sm_go = make_sm(0.3, 1.5, -4.0, experimental_mode=False, + velocity_traj=list(np.linspace(0.0, 8.0, TRAJ_LEN)), should_stop=False) + sm_go["starpilotPlan"].vCruise = 15.0 + for _ in range(6): + planner.update(sm_go, toggles) + + assert not planner.output_should_stop, "Departure must instantly clear the stop latch" + assert planner.output_a_target > 0.0, "Departure must deliver positive cruise acceleration" + + +def test_lead_departure_clears_stop_without_sticky_latch(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.3) + toggles = make_toggles(hybrid=True) + + sm_stop = make_sm(0.3, -1.0, -4.0, experimental_mode=False, tracking_lead=True, + lead_one=make_lead(status=True, d_rel=3.0, v_lead=0.1, radar=True, model_prob=1.0), + velocity_traj=[0.0] * TRAJ_LEN, should_stop=True) + for _ in range(6): + planner.update(sm_stop, toggles) + assert planner.output_should_stop + + # Lead accelerates away from standstill and opens up a safe gap. + sm_go = make_sm(0.3, 1.0, -4.0, experimental_mode=False, tracking_lead=True, + lead_one=make_lead(status=True, d_rel=25.0, v_lead=10.0, radar=True, model_prob=1.0), + velocity_traj=list(np.linspace(0.0, 7.0, TRAJ_LEN)), should_stop=False) + sm_go["starpilotPlan"].vCruise = 15.0 + for _ in range(6): + planner.update(sm_go, toggles) + + # HEM's own vision latch releases instantly (no sticky vision authority). + assert planner.hybrid_controller.w_vision == 0.0 + assert not planner.output_should_stop, "Lead departure must clear shouldStop without lag" + assert planner.output_a_target > 0.0 + + +# 5. Safety clamping preservation +def test_radar_cut_in_preserves_chill_floor_within_physical_limits(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=20.0) + toggles = make_toggles(hybrid=True) + lead = make_lead(status=True, d_rel=6.0, v_lead=10.0, radar=True, model_prob=1.0) + sm = make_sm(20.0, 0.0, -3.5, experimental_mode=False, tracking_lead=True, lead_one=lead, + velocity_traj=[20.0] * TRAJ_LEN, should_stop=False) + for _ in range(15): + planner.update(sm, toggles) + assert planner.output_a_target <= -1.0, "Radar cut-in must brake hard at the planner output" + assert planner.output_a_target >= -3.5, "Output must never exceed the commanded accel_min floor" + assert planner.output_a_target >= get_vehicle_min_accel(CP, 20.0), "Physical decel limit respected" + + +def test_hem_output_clamped_to_physical_limits_under_corrupt_exp(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + lo = get_vehicle_min_accel(CP, 20.0) + for a_exp in (float("nan"), -100.0, 100.0): + planner = LongitudinalPlanner(CP, init_v=20.0) + sm = make_sm(20.0, a_exp, -3.5, experimental_mode=False, + velocity_traj=[20.0] * TRAJ_LEN, should_stop=False) + for _ in range(3): + planner.update(sm, make_toggles(hybrid=True)) + assert np.isfinite(planner.output_a_target), f"corrupt a_exp={a_exp} must not propagate" + assert lo - 1e-6 <= planner.output_a_target <= 2.0 + 1e-6 + + +def test_output_stays_within_physical_limits_across_scenarios(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + scenarios = [ + dict(v_ego=20.0, desired_accel=-2.5, min_accel=-4.0, tracking_lead=False, + lead_one=None, velocity_traj=list(np.linspace(20.0, 0.0, TRAJ_LEN)), should_stop=True), + dict(v_ego=20.0, desired_accel=0.5, min_accel=-3.0, tracking_lead=True, + lead_one=make_lead(status=True, d_rel=40.0, v_lead=22.0, radar=True, model_prob=1.0), + velocity_traj=None, should_stop=False), + ] + for scen in scenarios: + planner = LongitudinalPlanner(CP, init_v=scen["v_ego"]) + sm = make_sm(experimental_mode=False, **scen) + for _ in range(6): + planner.update(sm, make_toggles(hybrid=True)) + assert planner.output_a_target >= get_vehicle_min_accel(CP, scen["v_ego"]) + assert planner.output_a_target <= 2.0 diff --git a/selfdrive/ui/lib/starpilot_status.py b/selfdrive/ui/lib/starpilot_status.py index a6cff451c..210ab7fad 100644 --- a/selfdrive/ui/lib/starpilot_status.py +++ b/selfdrive/ui/lib/starpilot_status.py @@ -36,8 +36,8 @@ def _is_hybrid_experimental_mode(state: UIState) -> bool: def _hem_exp_dominant(state: UIState) -> bool: """True when the fused output tracks the E2E/vision input more than chill ACC. - Uses the planner's per-frame comparison (HEMExpDominant) rather than the raw - exp_authority, which is inflated by the E2E Authority Bias baseline. + Uses the planner's per-frame comparison (HEMExpDominant), which reflects the + active fusion regime rather than a raw vision weight. """ params_memory = getattr(state, "params_memory", None) if params_memory is None: diff --git a/starpilot/controls/lib/hybrid_experimental_mode.py b/starpilot/controls/lib/hybrid_experimental_mode.py index b1290c69b..161b4bfa0 100644 --- a/starpilot/controls/lib/hybrid_experimental_mode.py +++ b/starpilot/controls/lib/hybrid_experimental_mode.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 import numpy as np from openpilot.common.realtime import DT_MDL -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE def lerp(a: float, b: float, t: float) -> float: @@ -19,349 +18,162 @@ def smooth_min(a: float, b: float, k: float = 6.0) -> float: return lerp(b, a, sigmoid(b - a, k=k)) -def smooth_max(a: float, b: float, k: float = 6.0) -> float: - return lerp(b, a, sigmoid(a - b, k=k)) - - class HybridExperimentalMode: """ - Fuses Chill Mode (radar/lead tracking) and Experimental Mode (vision/stop signs/lights): - 1. Detects vision stopping intent from trajectory drop and horizon endpoints. - 2. Calculates kinematically required stopping deceleration (-v^2 / 2d). - 3. Clamps Chill positive throttle during stop events to prevent brake fighting/dilution. - 4. Holds brake at standstills to prevent creep, but releases immediately on green lights or gas tap. - 5. Falls back safely to Chill if lead vehicle distance is compromised. - 6. Slew-rate limits acceleration to respect vehicle jerk limits, with emergency bypass. + Final Arbitrator between Chill (MPC Cruise/Radar) and Exp (Vision E2E): + 1. Open Road: Follows Chill MPC cruise & radar headway. + 2. Slower Lead Approach: Blends smooth vision decel with MPC follow distance. + 3. Stop Signs / Red Lights: Pure vision stopping authority (locks out positive cruise throttle). + 4. Standstill / Stop Completion: Latches should_stop for LongControl mechanical brake hold. + 5. Green Light / Lead Depart: Instant release back to Chill cruise acceleration. """ - # Physical actuator jerk limits (m/s^3) - BASE_MAX_JERK_BRAKE = 3.5 - BASE_MAX_JERK_ACCEL = 5.5 - EMERGENCY_JERK_BRAKE = 14.0 - - # Base safety floor parameters - BASE_T_FOLLOW = 1.45 - BASE_STOP_DISTANCE = STOP_DISTANCE # 6.0 m - def __init__(self): self.DT = DT_MDL + self.w_vision = 0.0 self.prev_a_target = 0.0 - self.exp_authority = 0.5 - self.w_vision_filtered = 0.0 - self.tracked_stop_dist = None - - # Last-frame diagnostics surfaced to live logs - self.last_w_vision = 0.0 - self.last_regime = "throttle" - self.last_standstill = False self.last_exp_dominant = False - self.diag = {} self.record_diag = False - # User tuning - self.HYBRID_EXP_BIAS = 0.2 # [-1.0, 1.0] - self.VISION_BRAKE_SENSITIVITY = 1.2 # [0.0, 2.0] - self.KINEMATIC_STOP_GAIN = 1.0 # [0.0, 2.0] scales the -v^2/2d stop-line brake floor - - # Active profile parameters - self.t_follow = self.BASE_T_FOLLOW - self.jerk_factor = 1.0 - self._update_profile_limits(self.t_follow, self.jerk_factor) + # Tunings + self.HYBRID_EXP_BIAS = 0.0 + self.VISION_BRAKE_SENSITIVITY = 1.0 def reset(self, a_ego: float = 0.0): """Seed target with actual vehicle acceleration on engagement to prevent torque bumps.""" self.prev_a_target = float(a_ego) if np.isfinite(a_ego) else 0.0 - self.exp_authority = 0.5 - self.w_vision_filtered = 0.0 - self.tracked_stop_dist = None - self.last_w_vision = 0.0 - self.last_regime = "throttle" - self.last_standstill = False + self.w_vision = 0.0 self.last_exp_dominant = False self.diag = {} - def set_tuning(self, exp_bias: float, vision_brake_sensitivity: float, kinematic_stop_gain: float = 1.0, - t_follow=None, jerk_factor=None): + def set_tuning(self, exp_bias: float, vision_brake_sensitivity: float): self.HYBRID_EXP_BIAS = float(np.clip(exp_bias, -1.0, 1.0)) self.VISION_BRAKE_SENSITIVITY = float(np.clip(vision_brake_sensitivity, 0.0, 2.0)) - self.KINEMATIC_STOP_GAIN = float(np.clip(kinematic_stop_gain, 0.0, 2.0)) - if t_follow is not None or jerk_factor is not None: - self._update_profile_limits( - t_follow if t_follow is not None else self.t_follow, - jerk_factor if jerk_factor is not None else self.jerk_factor - ) - - def _update_profile_limits(self, t_follow=None, jerk_factor=None): - """Updates safety headway floor and jerk limits based on active driving profile.""" - self.t_follow = float(t_follow) if t_follow is not None else self.BASE_T_FOLLOW - self.jerk_factor = float(np.clip(jerk_factor if jerk_factor is not None else 1.0, 0.25, 2.0)) - - # Safety buffer floors - self.T_FOLLOW_SAFE = float(np.clip(self.t_follow * 0.80, 1.00, 1.60)) - self.D_STATIC_SAFE = float(np.clip(self.BASE_STOP_DISTANCE * 0.75, 3.5, 6.0)) - - # Jerk rate limits - self.MAX_JERK_ACCEL = float(np.clip(self.BASE_MAX_JERK_ACCEL * self.jerk_factor, 2.5, 8.0)) - self.MAX_JERK_BRAKE = float(np.clip(self.BASE_MAX_JERK_BRAKE * self.jerk_factor, 1.8, 6.0)) - - @staticmethod - def _get_traj_array(model_v2, attr: str) -> np.ndarray: - obj = getattr(model_v2, attr, None) - arr = getattr(obj, "x", None) if obj is not None else None - if arr is None or len(arr) == 0: - return np.array([], dtype=float) - arr = np.asarray(arr, dtype=float) - return arr if np.all(np.isfinite(arr)) else np.array([], dtype=float) - - def _get_model_trajectory_v(self, model_v2, v_ego: float) -> np.ndarray: - traj_v = self._get_traj_array(model_v2, "velocity") - return traj_v if len(traj_v) > 0 else np.array([v_ego], dtype=float) - - def _get_model_trajectory_x(self, model_v2) -> np.ndarray: - return self._get_traj_array(model_v2, "position") def update(self, v_ego, v_cruise, lead_one, model_v2, a_chill, a_exp, - t_follow=None, jerk_factor=None): - # 0. Sync profile parameters if passed per-frame - if (t_follow is not None and abs(t_follow - self.t_follow) > 1e-4) or \ - (jerk_factor is not None and abs(jerk_factor - self.jerk_factor) > 1e-4): - self._update_profile_limits(t_follow, jerk_factor) + should_stop_exp=False, should_stop_chill=False): + # Robustness: never let non-finite or corrupt inputs propagate into the blend. if not np.isfinite(a_chill): a_chill = float(self.prev_a_target) if not np.isfinite(a_exp): a_exp = a_chill + # 1. Trajectory Analysis & Robust Slicing + traj_v = getattr(getattr(model_v2, "velocity", None), "x", []) + traj_v = np.asarray(traj_v, dtype=float) + has_full_trajectory = traj_v.size >= 24 and np.all(np.isfinite(traj_v)) + + if has_full_trajectory: + v_horizon = float(traj_v[-1]) + v_short = float(traj_v[23]) # ~4.0s lookahead + v_min = float(np.min(traj_v)) + else: + v_horizon = float(v_ego) + v_short = float(v_ego) + v_min = float(v_ego) + lead_status = bool(getattr(lead_one, "status", False)) - lead_d_rel = float(getattr(lead_one, "dRel", 150.0)) + lead_v = float(getattr(lead_one, "vLead", 0.0)) + lead_d = float(getattr(lead_one, "dRel", 150.0)) - # 1. VISION INTENT & STOP HORIZON DETECTION - traj_v = self._get_model_trajectory_v(model_v2, v_ego) - traj_x = self._get_model_trajectory_x(model_v2) + # 2. Vision Departure / Driver Override Detection (Priority Check) + lead_departing = lead_status and (lead_v > 0.5) + vision_departing = (v_horizon > 1.2) and (a_exp > 0.1) + driver_override = (a_chill > 0.8) + is_departing = lead_departing or vision_departing or driver_override - v_min = float(np.min(traj_v)) - min_idx = int(np.argmin(traj_v)) - v_ref = max(v_ego, 2.0) + # 3. Vision Stop & Decel Detection + # Do not latch horizon_stopping if the lead is actively pulling away or driver commands takeoff + if is_departing: + horizon_stopping = False + else: + horizon_stopping = (v_horizon < 0.8) or (has_full_trajectory and v_short < 1.5) or should_stop_exp - # Use actual/full model horizon endpoint (index -1) for standstill verification - v_horizon = float(traj_v[-1]) if len(traj_v) > 0 else v_ego - - # Detect deceleration profile or low-speed stop line target - speed_drop_ratio = max(0.0, (v_ego - v_min) / v_ref) + speed_drop_ratio = max(0.0, (v_ego - v_min) / max(v_ego, 2.0)) model_decel_strength = max(0.0, -a_exp / 2.0) - # Shorter planning horizon (~4 seconds out, index 23) for stop sign detection - v_horizon_short = float(traj_v[min(len(traj_v) - 1, 23)]) if len(traj_v) > 0 else v_ego - stop_target_active = sigmoid(1.8 - v_horizon_short, k=3.0) - - raw_vision_metric = max(speed_drop_ratio, stop_target_active, model_decel_strength) - w_vision_raw = float(np.clip(raw_vision_metric * self.VISION_BRAKE_SENSITIVITY, 0.0, 1.0)) - - # Identify departure signals - lead_departing = lead_status and (getattr(lead_one, "vLead", 0.0) > 0.5) - - # Core Bug Fix: Suppress driver_departing latch resets if we are actively stopping, - # unless a true heavy driver override (>1.2) occurs. - is_actively_stopping = self.w_vision_filtered > 0.25 and v_ego > 0.5 - driver_override = (a_chill > 1.2) - - driver_departing = (a_chill > 0.4) and (not lead_status or lead_d_rel > 10.0) and (not is_actively_stopping or driver_override) - model_stop_predicted = len(traj_v) > 1 and v_horizon < 0.5 - vision_departing = (v_horizon > 0.5) and (a_exp > 0.1) - departing = (lead_departing or vision_departing or driver_departing) and not model_stop_predicted - - # Latch holds during approach, decay only on verified departure or drivers override - should_reset_latch = driver_departing or lead_departing or (vision_departing and v_ego < 0.15 and not model_stop_predicted) - - if should_reset_latch: - self.w_vision_filtered = 0.0 - elif departing: - # Gently decay the latch if the vision model indicates a departure while still rolling - self.w_vision_filtered *= 0.90 - elif w_vision_raw > 0.15 or (self.tracked_stop_dist is not None and self.tracked_stop_dist > 0.5): - # Latch is sustained without decay as long as tracked stop distance is positive and active - self.w_vision_filtered = max(self.w_vision_filtered, w_vision_raw if w_vision_raw > 0.15 else 1.0) + if is_departing: + raw_vision_metric = 0.0 + elif horizon_stopping: + raw_vision_metric = 1.0 + elif lead_status and lead_d < 40.0: + # Slower lead closing: blend vision deceleration with MPC follow + raw_vision_metric = max(speed_drop_ratio, model_decel_strength) else: - self.w_vision_filtered *= 0.97 + # Open road mild vision response + raw_vision_metric = max(speed_drop_ratio * 0.5, model_decel_strength * 0.5) - w_vision = self.w_vision_filtered + w_target = float(np.clip(raw_vision_metric * self.VISION_BRAKE_SENSITIVITY, 0.0, 1.0)) - # Kinematic stopping calculation - d_min = float(traj_x[min_idx]) if len(traj_x) > min_idx else float("inf") - - slow_horizon = sigmoid(4.0 - v_horizon_short, k=1.5) - stop_confidence = max(stop_target_active, slow_horizon * speed_drop_ratio) - - near_stop_planned = False - if len(traj_x) == len(traj_v) and len(traj_v) > 0: - near_stop_planned = np.any((traj_v < 4.0) & (traj_x < 35.0)) - elif len(traj_v) > 0: - near_stop_planned = np.any(traj_v[:12] < 4.0) - - if near_stop_planned: - stop_confidence = max(stop_confidence, 0.8) - - # Robust Stop Distance Tracking (Runs on every frame, protected against standstill startup noise) - if stop_confidence > 0.35 and d_min < 100.0 and v_ego > 1.5 and (not lead_status or lead_d_rel > self.D_STATIC_SAFE + 10.0): - if self.tracked_stop_dist is None: - self.tracked_stop_dist = d_min - elif d_min > self.tracked_stop_dist + 15.0: # Re-sync only if a new stop line is further ahead - self.tracked_stop_dist = d_min - - # Decrement tracker on every frame if active: - if self.tracked_stop_dist is not None: - self.tracked_stop_dist -= v_ego * self.DT - # If we have passed the stop line or the vehicle is departing, reset tracking - if self.tracked_stop_dist < -2.0 or departing: - self.tracked_stop_dist = None - - d_stop_calc = self.tracked_stop_dist if self.tracked_stop_dist is not None else d_min - - d_stop_effective = max(d_stop_calc - 1.5, 2.0) - a_kinematic_stop = 0.0 - - if v_ego > 0.1 and 0.2 < d_stop_calc < float("inf") and w_vision > 0.25: - a_kinematic_stop = float(np.clip(- (v_ego ** 2) / (2.0 * d_stop_effective), -3.5, 0.0)) - a_kinematic_stop *= self.KINEMATIC_STOP_GAIN - if d_stop_effective < 6.0 and v_ego < 3.0: - a_kinematic_stop = min(a_kinematic_stop, -0.6) - a_exp_effective = min(a_exp, a_kinematic_stop) - elif v_horizon < 1.0 and v_ego > 0.05: - a_exp_effective = min(a_exp, -0.5) + # 4. Dynamic Filter (Fast Attack, Smooth Decay) + if is_departing: + self.w_vision = 0.0 + elif w_target > self.w_vision: + self.w_vision = min(1.0, self.w_vision + 0.15) else: - a_exp_effective = a_exp + self.w_vision = max(0.0, self.w_vision - 0.04) - base_auth = float(np.clip(0.5 + (0.35 * self.HYBRID_EXP_BIAS), 0.0, 1.0)) - alpha_exp = lerp(base_auth, 1.0, w_vision) - self.exp_authority = alpha_exp - - # 2. ACCELERATION FUSION (Throttle vs Braking Regimes) - a_throttle_raw = smooth_max(a_chill, a_exp, k=4.0) - overshoot_risk = 0.0 - a_throttle_capped = a_throttle_raw - if v_ego >= v_cruise: - a_throttle_optimal = min(a_throttle_raw, a_chill) + # 5. Dual-Regime blend + # Braking Regime: Pure vision braking when model demands it + if a_exp < 0.0: + a_brake_fused = min(a_chill, a_exp) else: - overshoot_risk = sigmoid(v_ego - v_cruise, k=4.0, x0=-1.0) - a_throttle_capped = min(a_throttle_raw, max(0.0, a_chill)) - a_throttle_optimal = lerp(a_throttle_raw, a_throttle_capped, overshoot_risk) + a_brake_fused = min(a_chill, a_exp) - a_throttle_conservative = smooth_min(a_chill, a_exp, k=4.0) - a_throttle_fused = lerp(a_throttle_optimal, a_throttle_conservative, w_vision) + # Throttle Regime: Follow Chill MPC cruise with optional Exp bias + a_throttle_fused = a_chill + max(0.0, a_exp - a_chill) * max(0.0, self.HYBRID_EXP_BIAS) - # Braking Regime: Never dilute Exp stop braking with Chill's 0.0 m/s^2 - a_chill_brake = 0.0 - if a_exp_effective < 0.0: - a_chill_brake = min(a_chill, 0.0) - a_brake_fused = min(a_exp_effective, a_chill_brake) + # Output Arbitration + is_stopping_event = (self.w_vision > 0.3) or horizon_stopping + if is_stopping_event and not is_departing: + a_out = a_brake_fused + if horizon_stopping and v_ego < 2.0: + a_out = min(a_out, -0.6) # Standstill anchor into full stop + self.last_exp_dominant = True else: - a_brake_fused = min(a_chill, a_exp_effective) + a_out = lerp(a_throttle_fused, a_brake_fused, self.w_vision) + self.last_exp_dominant = False - # Regime Selection: Lock out positive throttle during stopping/braking - is_braking_phase = (w_vision > 0.3) or (a_exp_effective < -0.2) or (a_chill < -0.2) - phase_metric = 0.0 - if is_braking_phase: - w_accel = 0.0 - else: - phase_metric = smooth_min(a_chill, a_exp, k=4.0) - w_accel = sigmoid(phase_metric, k=3.0, x0=-0.1) * (1.0 - w_vision) - - a_fused = lerp(a_brake_fused, a_throttle_fused, w_accel) - - # 3. STANDSTILL ANCHOR - is_stopped = sigmoid(0.4 - v_ego, k=8.0) - is_staying_stopped = sigmoid(0.5 - v_horizon, k=6.0) - - # Bug C Fix: Low-speed acceleration lockout (active up to 5.0 m/s when vision stop intent is latched) - if self.w_vision_filtered > 0.25 and v_ego < 5.0: - a_fused = min(a_fused, 0.0) - - # Smooth soft lockout ramp: scale positive acceleration to zero based on filter intensity - if v_ego < 4.0 and a_fused > 0.0: - scale = max(0.0, 1.0 - (self.w_vision_filtered / 0.30)) - a_fused *= scale - - standstill_weight = (0.0 if departing else 1.0) * is_stopped * is_staying_stopped - a_anchored = lerp(a_fused, smooth_min(a_fused, -0.5, k=6.0), standstill_weight) - - # 4. SAFETY BARRIER (Lead Vehicle Proximity Check) - d_static_effective = self.D_STATIC_SAFE + max(0.0, 1.5 * (1.0 - (v_ego / 4.0))) - d_safe = (v_ego * self.T_FOLLOW_SAFE) + d_static_effective - - distance_ratio = (lead_d_rel - d_static_effective) / max(d_safe - d_static_effective, 1.0) - lead_safety_risk = sigmoid(1.0 - distance_ratio, k=5.0) * float(lead_status) - - lead_safety_active = lead_status and (lead_d_rel < d_safe or a_chill < 0.0) - a_safe = min(a_anchored, a_chill) if lead_safety_active else a_anchored - - # 5. ASYMMETRIC DIRECTIONAL SLEW FILTER (Limit Jerk) - da = a_safe - self.prev_a_target - if da >= 0.0: - jerk_limit = self.MAX_JERK_ACCEL - else: - is_lead_emergency = (lead_safety_risk > 0.5) and (a_chill < -1.5) - is_vision_emergency = (w_vision > 0.7) and (a_exp < -2.0) - jerk_limit = self.EMERGENCY_JERK_BRAKE if (is_lead_emergency or is_vision_emergency) else self.MAX_JERK_BRAKE - - max_delta = jerk_limit * self.DT - a_out = float(np.clip(a_safe, self.prev_a_target - max_delta, self.prev_a_target + max_delta)) - if np.isfinite(a_out): - self.prev_a_target = a_out - self.last_w_vision = w_vision - self.last_regime = "brake" if is_braking_phase else "throttle" - self.last_standstill = standstill_weight > 0.0 - self.last_exp_dominant = abs(self.prev_a_target - a_exp) < abs(self.prev_a_target - a_chill) - 0.03 + # 6. Authoritative Standstill Handshake + standstill_intent = (v_ego < 0.5 and (v_horizon < 0.4 or should_stop_exp)) and not is_departing + should_stop_fused = bool((should_stop_chill or should_stop_exp or standstill_intent) and not is_departing) if self.record_diag: self.diag = { # inputs "v_ego": v_ego, "v_cruise": v_cruise, "a_chill": a_chill, "a_exp": a_exp, - "lead_status": lead_status, "lead_d_rel": lead_d_rel, - "lead_v_lead": float(getattr(lead_one, "vLead", 0.0)), - "t_follow": self.t_follow, + "should_stop_chill": should_stop_chill, + "should_stop_exp": should_stop_exp, + "lead_status": lead_status, "lead_d_rel": lead_d, "lead_v_lead": lead_v, # vision intent - "v_min": v_min, "v_horizon": v_horizon, "v_horizon_short": v_horizon_short, "v_ref": v_ref, - "min_idx": min_idx, "speed_drop_ratio": speed_drop_ratio, - "stop_target_active": stop_target_active, - "stop_confidence": stop_confidence, + "has_full_trajectory": bool(has_full_trajectory), + "v_horizon": v_horizon, "v_short": v_short, "v_min": v_min, + "speed_drop_ratio": speed_drop_ratio, "model_decel_strength": model_decel_strength, - "raw_vision_metric": raw_vision_metric, "w_vision": w_vision, - # kinematic stop - "d_min": d_min, "d_stop_effective": d_stop_effective, - "a_kinematic_stop": a_kinematic_stop, "a_exp_effective": a_exp_effective, - # authority - "base_auth": base_auth, "alpha_exp": alpha_exp, - # throttle path - "a_throttle_raw": a_throttle_raw, - "a_throttle_optimal": a_throttle_optimal, - "a_throttle_capped": a_throttle_capped, - "overshoot_risk": overshoot_risk, - "a_throttle_conservative": a_throttle_conservative, + "raw_vision_metric": raw_vision_metric, "w_target": w_target, + "w_vision": self.w_vision, + # departure / override + "lead_departing": lead_departing, + "vision_departing": vision_departing, + "driver_override": driver_override, + "is_departing": is_departing, + "horizon_stopping": horizon_stopping, + # fusion + "a_brake_fused": a_brake_fused, "a_throttle_fused": a_throttle_fused, - # brake path - "a_chill_brake": a_chill_brake, "a_brake_fused": a_brake_fused, - # regime selection - "is_braking_phase": is_braking_phase, - "phase_metric": phase_metric, "w_accel": w_accel, - "a_fused": a_fused, - # standstill anchor - "is_stopped": is_stopped, "is_staying_stopped": is_staying_stopped, - "lead_departing": lead_departing, "vision_departing": vision_departing, - "driver_departing": driver_departing, - "model_stop_predicted": model_stop_predicted, - "departing": departing, "standstill_weight": standstill_weight, - "a_anchored": a_anchored, - # safety barrier - "d_static_effective": d_static_effective, "d_safe": d_safe, - "distance_ratio": distance_ratio, "lead_safety_risk": lead_safety_risk, - "lead_safety_active": lead_safety_active, "a_safe": a_safe, - # slew filter - "da": da, "jerk_limit": jerk_limit, "max_delta": max_delta, - "a_out": a_out, "prev_a_target": self.prev_a_target, - # regime labels - "regime": self.last_regime, - "standstill": self.last_standstill, + "is_stopping_event": is_stopping_event, + "exp_dominant": self.last_exp_dominant, + # standstill handshake + "standstill_intent": standstill_intent, + "should_stop_fused": should_stop_fused, + "a_out": a_out, + # regime labels (kept for downstream forensic/analyzer tooling) + "regime": "brake" if (is_stopping_event and not is_departing) else "throttle", + "standstill": standstill_intent, } - return self.prev_a_target \ No newline at end of file + + self.prev_a_target = a_out + return a_out, should_stop_fused diff --git a/starpilot/controls/tests/test_hybrid_experimental_mode.py b/starpilot/controls/tests/test_hybrid_experimental_mode.py index 4a0e25bb6..d636cdb74 100644 --- a/starpilot/controls/tests/test_hybrid_experimental_mode.py +++ b/starpilot/controls/tests/test_hybrid_experimental_mode.py @@ -6,7 +6,6 @@ from openpilot.starpilot.controls.lib.hybrid_experimental_mode import ( HybridExperimentalMode, lerp, sigmoid, - smooth_max, smooth_min, ) @@ -35,42 +34,54 @@ def run(controller, *, v_ego=20.0, v_cruise=30.0, lead=None, model=None, a_chill if model is None: model = FakeModel(velocity=[v_ego] * 33, position=list(np.linspace(0.0, 100.0, 33))) result = 0.0 + should_stop = False for _ in range(frames): - result = controller.update(v_ego, v_cruise, lead, model, a_chill, a_exp) - return result + result, should_stop = controller.update(v_ego, v_cruise, lead, model, a_chill, a_exp) + return result, should_stop + def test_soft_operators_are_continuous_and_bounded(): for value in (-5.0, -0.1, 0.0, 0.1, 5.0): assert 0.0 < sigmoid(value) < 1.0 - assert smooth_max(1.0, 2.0) == pytest.approx(2.0, abs=1e-2) assert smooth_min(1.0, 2.0) == pytest.approx(1.0, abs=1e-2) + assert smooth_min(2.0, 1.0) == pytest.approx(1.0, abs=1e-2) assert lerp(10.0, 20.0, 0.5) == pytest.approx(15.0, abs=1e-3) -def test_throttle_fusion_leans_toward_snappier_target(): + +def test_update_returns_accel_and_should_stop_tuple(): controller = make_controller() - a = run(controller, a_chill=0.5, a_exp=0.8) - assert 0.70 < a < 0.85 + result = controller.update(20.0, 30.0, FakeLead(), FakeModel(velocity=[20.0] * 33), 0.5, 0.5) + assert isinstance(result, tuple) and len(result) == 2 + assert isinstance(result[0], float) + assert isinstance(result[1], bool) -def test_throttle_fusion_conservative_when_chill_is_more_eager(): +def test_open_road_throttle_follows_chill_without_bias(): + # On open road with default bias, HEM yields to pure Chill MPC cruise. controller = make_controller() - a = run(controller, a_chill=1.2, a_exp=0.6) - assert a > 1.0 + a, _ = run(controller, a_chill=0.5, a_exp=0.8) + assert a == pytest.approx(0.5, abs=1e-3) -def test_throttle_fusion_smooth_cruise_speed_capping(): +def test_throttle_uses_exp_bias(): controller = make_controller() - a = run(controller, v_ego=30.0, v_cruise=30.0, a_chill=-0.2, a_exp=1.0) - assert a <= 0.0, f"Must not accelerate past set cruise speed, got {a}" + controller.set_tuning(0.5, 1.0) + a, _ = run(controller, a_chill=0.5, a_exp=0.8) + expected = 0.5 + max(0.0, 0.8 - 0.5) * 0.5 + assert a == pytest.approx(expected, abs=1e-3) + assert a > 0.5 + def test_green_light_departure_from_standstill(): controller = make_controller(prev=-0.5) + controller.set_tuning(0.5, 1.0) traj_v = np.linspace(0.0, 12.0, 33) traj_x = np.linspace(0.0, 45.0, 33) model = FakeModel(velocity=traj_v, position=traj_x) - a = run(controller, v_ego=0.0, v_cruise=20.0, model=model, a_chill=1.0, a_exp=1.5, frames=40) + a, should_stop = run(controller, v_ego=0.0, v_cruise=20.0, model=model, a_chill=1.0, a_exp=1.5, frames=40) assert a > 1.0, f"Vehicle should depart promptly on green light, got {a}" + assert not should_stop def test_standstill_hold_at_red_light_without_lead(): @@ -79,9 +90,11 @@ def test_standstill_hold_at_red_light_without_lead(): traj_x = np.zeros(33) model = FakeModel(velocity=traj_v, position=traj_x) - a = run(controller, v_ego=0.0, v_cruise=25.0, lead=FakeLead(status=False), + a, should_stop = run(controller, v_ego=0.0, v_cruise=25.0, lead=FakeLead(status=False), model=model, a_chill=0.2, a_exp=-1.0, frames=40) assert a <= -0.4, f"Standstill brake must hold even when Chill cruise wants to go, got {a}" + assert should_stop, "Standstill at a predicted stop must assert should_stop" + def test_red_light_high_speed_approach_braking(): controller = make_controller(prev=0.0) @@ -89,7 +102,7 @@ def test_red_light_high_speed_approach_braking(): traj_x = np.linspace(0.0, 60.0, 33) model = FakeModel(velocity=traj_v, position=traj_x) - a = run(controller, v_ego=25.0, v_cruise=25.0, model=model, a_chill=0.5, a_exp=-2.0, frames=60) + a, _ = run(controller, v_ego=25.0, v_cruise=25.0, model=model, a_chill=0.5, a_exp=-2.0, frames=60) assert a <= -2.0, f"Stopping deceleration should be fully honored without dilution, got {a}" @@ -99,78 +112,72 @@ def test_red_light_low_speed_roll_prevent_dilution(): traj_x = np.linspace(0.0, 3.0, 33) model = FakeModel(velocity=traj_v, position=traj_x) - a = run(controller, v_ego=0.8, v_cruise=20.0, model=model, a_chill=0.8, a_exp=-0.6, frames=30) - assert a <= -0.55, f"Kinematic stop clamp must enforce stopping bite at low speeds, got {a}" + a, _ = run(controller, v_ego=0.8, v_cruise=20.0, model=model, a_chill=0.8, a_exp=-0.6, frames=30) + assert a <= -0.55, f"Vision stop clamp must enforce stopping bite at low speeds, got {a}" -def test_kinematic_stopping_does_not_blow_up_on_close_dmin(): +def test_vision_stop_does_not_dilute_brake_with_cruise_throttle(): controller = make_controller(prev=0.0) - traj_v = np.zeros(33) - traj_x = np.ones(33) * 1.0 - model = FakeModel(velocity=traj_v, position=traj_x) - - a = controller.update(15.0, 20.0, FakeLead(), model, a_chill=0.0, a_exp=-1.0) - assert a >= -(controller.MAX_JERK_BRAKE * controller.DT + 1e-4) + model = FakeModel(velocity=list(np.linspace(20.0, 0.0, 33))) + a, _ = run(controller, v_ego=20.0, v_cruise=20.0, lead=FakeLead(status=False), + model=model, a_chill=0.8, a_exp=-2.5, frames=20) + assert a <= -2.5, f"Cruise throttle must be locked out during a vision stop, got {a}" -def test_kinematic_stopping_graceful_on_missing_position(): +def test_missing_or_corrupt_model_v2_fallbacks(): + controller = make_controller() + model_none = FakeModel(velocity=None, position=None) + a, _ = run(controller, lead=FakeLead(status=False), model=model_none, a_chill=0.5, a_exp=0.2) + assert a > 0.35, "Controller should gracefully fallback when trajectory is None" + + model_empty = FakeModel(velocity=[], position=[]) + a, _ = run(controller, lead=FakeLead(status=False), model=model_empty, a_chill=0.6, a_exp=0.7) + assert a > 0.55, "Controller should gracefully handle empty trajectory lists" + + +def test_nan_a_exp_falls_back_to_chill_channel(): controller = make_controller(prev=0.0) - traj_v = np.linspace(10.0, 0.0, 33) - model = FakeModel(velocity=traj_v, position=[]) - - a = run(controller, v_ego=10.0, v_cruise=20.0, model=model, a_chill=0.5, a_exp=-2.0, frames=40) - assert a <= -1.8 - -def test_radar_lead_safety_barrier_overrides_vision_throttle(): - controller = make_controller() - lead = FakeLead(status=True, d_rel=5.0, v_lead=0.0) - model = FakeModel(velocity=[15.0] * 33, position=list(np.linspace(0.0, 100.0, 33))) - - a = run(controller, v_ego=15.0, v_cruise=20.0, lead=lead, model=model, a_chill=-2.5, a_exp=1.0, frames=40) - assert a == pytest.approx(-2.5, abs=0.1), f"Safety barrier must enforce radar braking floor, got {a}" + model = FakeModel(velocity=[20.0] * 33, position=list(np.linspace(0.0, 100.0, 33))) + a, _ = run(controller, model=model, a_chill=0.5, a_exp=float("nan"), frames=10) + assert np.isfinite(a), f"NaN a_exp must never propagate, got {a}" + assert 0.4 <= a <= 0.6, f"NaN a_exp should degrade to the Chill channel, got {a}" -def test_radar_lead_safety_barrier_inactive_when_gap_is_safe(): - controller = make_controller() - lead = FakeLead(status=True, d_rel=80.0, v_lead=25.0) - model = FakeModel(velocity=[25.0] * 33, position=list(np.linspace(0.0, 150.0, 33))) - - a = run(controller, v_ego=25.0, v_cruise=30.0, lead=lead, model=model, a_chill=0.6, a_exp=0.9, frames=40) - assert a > 0.75, f"Lead barrier should remain transparent when headway is safe, got {a}" - - -def test_cut_in_emergency_braking_ramp_rate(): +def test_inf_a_exp_does_not_propagate(): controller = make_controller(prev=0.0) - cut_in_lead = FakeLead(status=True, d_rel=6.0, v_lead=10.0) - model = FakeModel(velocity=[20.0] * 33) - - for _ in range(8): - a = controller.update(20.0, 25.0, cut_in_lead, model, a_chill=-3.2, a_exp=0.0) - assert a <= -2.5, f"Emergency cut-in should ramp braking power quickly, got {a}" - -def test_asymmetric_jerk_slew_rates(): - controller_accel = make_controller(prev=0.0) - a_up = controller_accel.update(20.0, 30.0, FakeLead(), FakeModel(velocity=[20.0] * 33), 4.0, 4.0) - assert a_up == pytest.approx(controller_accel.MAX_JERK_ACCEL * controller_accel.DT, abs=1e-3) - - controller_brake = make_controller(prev=0.0) - a_down = controller_brake.update(20.0, 30.0, FakeLead(), FakeModel(velocity=[20.0] * 33), -1.0, -1.0) - assert a_down == pytest.approx(-controller_brake.MAX_JERK_BRAKE * controller_brake.DT, abs=1e-3) + model = FakeModel(velocity=[20.0] * 33, position=list(np.linspace(0.0, 100.0, 33))) + for a_exp in (float("inf"), float("-inf")): + controller.reset(0.0) + a, _ = run(controller, model=model, a_chill=0.5, a_exp=a_exp, frames=10) + assert np.isfinite(a), f"Non-finite a_exp={a_exp} must not propagate, got {a}" -def test_directional_brake_release_rate(): - controller = make_controller(prev=-3.0) - a_rel = controller.update(20.0, 30.0, FakeLead(), FakeModel(velocity=[20.0] * 33), 0.0, 0.0) - expected = -3.0 + controller.MAX_JERK_ACCEL * controller.DT - assert a_rel == pytest.approx(expected, abs=1e-3) +def test_nan_a_chill_falls_back_to_last_target(): + controller = make_controller(prev=0.0) + model = FakeModel(velocity=[20.0] * 33, position=list(np.linspace(0.0, 100.0, 33))) + a, _ = run(controller, model=model, a_chill=float("nan"), a_exp=0.8, frames=10) + assert np.isfinite(a), f"NaN a_chill must never propagate, got {a}" -def test_profile_limits_dynamic_update(): - controller = make_controller() - controller.update(20.0, 30.0, FakeLead(), FakeModel(velocity=[20.0] * 33), 0.5, 0.5, t_follow=1.1, jerk_factor=1.3) - assert controller.t_follow == pytest.approx(1.1, abs=1e-4) - assert controller.jerk_factor == pytest.approx(1.3, abs=1e-4) - assert controller.MAX_JERK_ACCEL == pytest.approx(5.5 * 1.3, abs=1e-3) +def test_nan_in_trajectory_does_not_propagate(): + controller = make_controller(prev=0.0) + v = [20.0] * 20 + [float("nan")] + [20.0] * 12 + model = FakeModel(velocity=v, position=list(np.linspace(0.0, 100.0, 33))) + a, _ = run(controller, model=model, a_chill=0.5, a_exp=0.8, frames=10) + assert np.isfinite(a), f"NaN trajectory must not propagate, got {a}" + + +def test_reset_rejects_non_finite_seed(): + controller = make_controller(prev=-1.5) + controller.reset(float("nan")) + assert controller.prev_a_target == 0.0 + + +def test_reset_seeds_active_acceleration(): + controller = make_controller(prev=0.0) + controller.reset(a_ego=-1.8) + assert controller.prev_a_target == -1.8 + def test_tuning_clamping(): controller = make_controller() @@ -182,84 +189,139 @@ def test_tuning_clamping(): assert controller.VISION_BRAKE_SENSITIVITY == 0.0 -def test_reset_seeds_active_acceleration(): +def test_slower_lead_closing_blends_vision_decel(): controller = make_controller(prev=0.0) - controller.reset(a_ego=-1.8) - assert controller.prev_a_target == -1.8 + lead = FakeLead(status=True, d_rel=25.0, v_lead=5.0) + model = FakeModel(velocity=list(np.linspace(15.0, 8.0, 33))) + a, _ = run(controller, v_ego=15.0, v_cruise=20.0, lead=lead, model=model, a_chill=-0.4, a_exp=-0.6) + assert a < -0.3, f"Closing on a slower lead must brake, got {a}" -def test_missing_or_corrupt_model_v2_fallbacks(): - controller = make_controller() - model_none = FakeModel(velocity=None, position=None) - a = run(controller, lead=FakeLead(status=False), model=model_none, a_chill=0.5, a_exp=0.2) - assert a > 0.35, "Controller should gracefully fallback when trajectory is None" - - model_empty = FakeModel(velocity=[], position=[]) - a = run(controller, lead=FakeLead(status=False), model=model_empty, a_chill=0.6, a_exp=0.7) - assert a > 0.55, "Controller should gracefully handle empty trajectory lists" - - -def test_nan_a_exp_falls_back_to_chill_channel(): +def test_cut_in_emergency_braking_preserves_chill_floor(): controller = make_controller(prev=0.0) - model = FakeModel(velocity=[20.0] * 33, position=list(np.linspace(0.0, 100.0, 33))) - a = run(controller, model=model, a_chill=0.5, a_exp=float("nan"), frames=10) - assert np.isfinite(a), f"NaN a_exp must never propagate, got {a}" - assert 0.4 <= a <= 0.6, f"NaN a_exp should degrade to the Chill channel, got {a}" + cut_in_lead = FakeLead(status=True, d_rel=6.0, v_lead=10.0) + model = FakeModel(velocity=[20.0] * 33) + + for _ in range(8): + a, _ = controller.update(20.0, 25.0, cut_in_lead, model, a_chill=-3.2, a_exp=0.0) + assert a <= -2.5, f"Emergency cut-in should preserve the Chill brake floor, got {a}" -def test_inf_a_exp_does_not_propagate(): +def test_no_hard_brake_on_gentle_high_horizon_slowdown(): + # A gentle slowdown that KEEPS a high horizon speed (curve / slower traffic, + # ends at 15 m/s) must NOT trigger hard stop-to-zero braking. controller = make_controller(prev=0.0) - model = FakeModel(velocity=[20.0] * 33, position=list(np.linspace(0.0, 100.0, 33))) - for a_exp in (float("inf"), float("-inf")): - controller.reset(0.0) - a = run(controller, model=model, a_chill=0.5, a_exp=a_exp, frames=10) - assert np.isfinite(a), f"Non-finite a_exp={a_exp} must not propagate, got {a}" - - -def test_nan_a_chill_falls_back_to_last_target(): - controller = make_controller(prev=0.0) - model = FakeModel(velocity=[20.0] * 33, position=list(np.linspace(0.0, 100.0, 33))) - a = run(controller, model=model, a_chill=float("nan"), a_exp=0.8, frames=10) - assert np.isfinite(a), f"NaN a_chill must never propagate, got {a}" - - -def test_nan_in_trajectory_does_not_propagate(): - controller = make_controller(prev=0.0) - v = [20.0] * 20 + [float("nan")] + [20.0] * 12 - model = FakeModel(velocity=v, position=list(np.linspace(0.0, 100.0, 33))) - a = run(controller, model=model, a_chill=0.5, a_exp=0.8, frames=10) - assert np.isfinite(a), f"NaN trajectory must not propagate, got {a}" - - -def test_reset_rejects_non_finite_seed(): - controller = make_controller(prev=-1.5) - controller.reset(float("nan")) - assert controller.prev_a_target == 0.0 - - -def test_standstill_holds_without_model_brake_at_predicted_stop(): - controller = make_controller(prev=0.0) - model = FakeModel(velocity=np.zeros(33), position=np.zeros(33)) - a = run(controller, v_ego=0.0, v_cruise=25.0, lead=FakeLead(status=False), - model=model, a_chill=0.8, a_exp=0.0, frames=40) - assert a <= -0.4, f"Anchor must hold at a predicted stop even with cruise creep, got {a}" - - -def test_green_light_departure_releases_anchor(): - controller = make_controller(prev=-0.5) - traj_v = np.linspace(0.0, 12.0, 33) - traj_x = np.linspace(0.0, 45.0, 33) + traj_v = np.linspace(20.0, 15.0, 33) + traj_x = np.linspace(0.0, 100.0, 33) model = FakeModel(velocity=traj_v, position=traj_x) - a = run(controller, v_ego=0.0, v_cruise=20.0, lead=FakeLead(status=False), - model=model, a_chill=1.0, a_exp=1.5, frames=40) - assert a > 1.0, f"Green light departure must still release the anchor, got {a}" + a, _ = run(controller, v_ego=20.0, v_cruise=20.0, lead=FakeLead(status=False), + model=model, a_chill=0.0, a_exp=-0.1, frames=5) + assert a > -0.5, f"Gentle high-horizon slowdown must not hard-brake, got {a}" -def test_vision_stop_uses_emergency_brake_ramp(): +def test_lead_departure_releases_vision_latch(): + controller = make_controller(prev=-1.0) + stop_model = FakeModel(velocity=np.zeros(33)) + for _ in range(5): + controller.update(5.0, 25.0, FakeLead(status=False), stop_model, 0.0, -2.0) + assert controller.w_vision > 0.3 + + depart_model = FakeModel(velocity=[10.0] * 33) + lead = FakeLead(status=True, d_rel=30.0, v_lead=8.0) + a, should_stop = controller.update(5.0, 25.0, lead, depart_model, 0.5, 0.5) + assert controller.w_vision == 0.0, "Lead departure must release the vision latch instantly" + assert a > 0.0 + assert not should_stop + + +def test_should_stop_chill_handshake(): controller = make_controller(prev=0.0) - model = FakeModel(velocity=list(np.linspace(20.0, 0.2, 33)), position=list(np.linspace(0.0, 40.0, 33))) - out = [controller.update(20.0, 25.0, FakeLead(status=False), model, 0.0, -3.5) for _ in range(5)] - assert out[-1] <= -2.5, f"Vision stop should ramp braking fast, got {out}" + model = FakeModel(velocity=[20.0] * 33) + a, should_stop = controller.update(20.0, 25.0, FakeLead(), model, 0.0, 0.0, should_stop_chill=True) + assert should_stop, "should_stop_chill must propagate into the fused stop flag" + + +def test_should_stop_exp_handshake(): + controller = make_controller(prev=0.0) + model = FakeModel(velocity=[5.0] * 33) + a, should_stop = controller.update(5.0, 25.0, FakeLead(), model, 0.0, 0.0, should_stop_exp=True) + assert should_stop, "should_stop_exp must propagate into the fused stop flag" + + +def test_standstill_intent_asserts_should_stop(): + controller = make_controller(prev=0.0) + model = FakeModel(velocity=np.zeros(33)) + a, should_stop = controller.update(0.3, 20.0, FakeLead(), model, 0.0, 0.0) + assert should_stop, "Standstill intent at low speed must assert should_stop" + + +def test_no_should_stop_on_open_road(): + controller = make_controller(prev=0.0) + model = FakeModel(velocity=[20.0] * 33) + a, should_stop = controller.update(20.0, 25.0, FakeLead(), model, 0.5, 0.5) + assert not should_stop, "Open road cruising must not assert should_stop" + + +def test_output_bounded_between_chill_and_exp(): + # Defense-in-depth invariant: HEM output must never exceed the more aggressive of + # the two inputs, and can only dip below both via the intentional -0.6 standstill + # anchor (which keeps the car planted at a predicted stop). This guarantees HEM can + # never command something more aggressive than both Chill and Exp intended. + controller = make_controller(prev=0.0) + scenarios = [ + (20.0, 0.5, 0.8), # both throttle + (20.0, 0.5, -2.0), # exp brakes, chill cruise + (20.0, -3.0, -0.5), # chill emergency, exp mild + (20.0, -1.0, -1.8), # both brake + (0.3, 0.8, -1.0), # standstill (anchor may engage) + (0.3, 0.5, 1.2), # departure + ] + for v_ego, a_chill, a_exp in scenarios: + for traj in ([v_ego] * 33, list(np.linspace(max(v_ego, 1.0), 0.0, 33))): + controller.reset(0.0) + model = FakeModel(velocity=traj) + a, _ = controller.update(v_ego, 25.0, FakeLead(), model, a_chill, a_exp) + lo = min(a_chill, a_exp, -0.6) + hi = max(a_chill, a_exp) + assert lo - 1e-9 <= a <= hi + 1e-9, \ + f"v_ego={v_ego} a_chill={a_chill} a_exp={a_exp} -> a_out={a} outside [{lo}, {hi}]" + + # A positive Exp bias blends toward Exp but never overshoots the max of the two. + controller.set_tuning(0.5, 1.0) + controller.reset(0.0) + model = FakeModel(velocity=[20.0] * 33) + a, _ = controller.update(20.0, 25.0, FakeLead(), model, 0.5, 1.2) + assert 0.5 - 1e-9 <= a <= 1.2 + 1e-9 + + +def test_dropped_trajectory_at_crawl_does_not_phantom_brake(): + # Regression for the indexing fallback bug: if modelV2 drops frames or returns a + # too-short/corrupt trajectory at crawling speed (0.8 < v_ego < 1.5), the fallback + # must not fake a stop (v_short must only be trusted from a full trajectory). + for traj in ([], [1.2] * 5): + controller = make_controller(prev=0.0) + model = FakeModel(velocity=traj) + a, should_stop = controller.update(1.2, 20.0, FakeLead(), model, 0.3, 0.0) + assert a > 0.0, f"Short trajectory must not phantom-brake at crawl, got {a}" + assert not should_stop + + +def test_lead_departure_from_standstill_clears_anchor(): + # Regression for the standstill deadlock: stopped behind a lead with v_horizon≈0, + # when the lead pulls away, departure must clear the anchor/latch immediately even + # before the vision horizon visually extends past the stop threshold. + controller = make_controller(prev=-1.0) + stop_model = FakeModel(velocity=np.zeros(33)) + for _ in range(5): + controller.update(0.3, 25.0, FakeLead(), stop_model, 0.0, -1.0, should_stop_exp=True) + assert controller.w_vision > 0.3 + + lead = FakeLead(status=True, d_rel=6.0, v_lead=6.0) # lead accelerates away + depart_model = FakeModel(velocity=np.zeros(33)) # horizon still ~0 (not yet registered) + a, should_stop = controller.update(0.3, 25.0, lead, depart_model, 0.5, 0.3) + assert controller.w_vision == 0.0 + assert not should_stop, "Lead departure must clear shouldStop despite v_horizon~=0" + assert a > 0.0, "Standstill anchor must not fight a departing lead" def test_last_exp_dominant_true_when_vision_braking(): @@ -282,69 +344,3 @@ def test_last_exp_dominant_false_at_neutral_cruise(): controller = make_controller(prev=0.0) run(controller, a_chill=0.0, a_exp=0.05, frames=10) assert not controller.last_exp_dominant - - -def test_red_light_approach_brakes_kinematically_on_partial_slowdown(): - # Regression: the model predicts only a PARTIAL slowdown (v_min ~ 3 m/s), not a - # full stop, so the old v_min<1.2 gate never fired and HEM mirrored the model's - # weak a_exp (-0.1) -> it would roll the red light. HEM must now apply the - # kinematic -v^2/2d floor toward the closing stop point regardless. - controller = make_controller(prev=0.0) - traj_v = np.linspace(15.0, 3.0, 33) - traj_x = np.linspace(0.0, 40.0, 33) - model = FakeModel(velocity=traj_v, position=traj_x) - a = run(controller, v_ego=15.0, v_cruise=20.0, lead=FakeLead(status=False), - model=model, a_chill=0.0, a_exp=-0.1, frames=10) - # kinematic = -15^2/(2*38.5) ~ -2.9; even with jerk slew HEM must brake hard, - # not track the -0.1 exp input. - assert a < -1.0, f"HEM must apply kinematic stop on partial-slowdown approach, got {a}" - - -def test_no_kinematic_brake_on_gentle_high_horizon_slowdown(): - # A gentle slowdown that KEEPS a high horizon speed (curve / slower traffic, - # ends at 15 m/s) must NOT trigger kinematic stop-to-zero braking. - controller = make_controller(prev=0.0) - traj_v = np.linspace(20.0, 15.0, 33) - traj_x = np.linspace(0.0, 100.0, 33) - model = FakeModel(velocity=traj_v, position=traj_x) - a = run(controller, v_ego=20.0, v_cruise=20.0, lead=FakeLead(status=False), - model=model, a_chill=0.0, a_exp=-0.1, frames=5) - assert a > -0.5, f"Gentle high-horizon slowdown must not hard-brake, got {a}" - - -def _stop_sign_dip_model(min_at_idx, v_min, v_horizon): - """Trajectory that slows to v_min at min_at_idx then re-accelerates to v_horizon - (the 'slow to ~1 m/s then speed up again' stop-sign profile seen on device).""" - traj_v = np.zeros(33) - idxs = sorted({0, int(min_at_idx), 23, 32}) - pts = {0: 1.85, int(min_at_idx): v_min, 23: 5.8, 32: v_horizon} - for a, b in zip(idxs, idxs[1:]): - va, vb = pts[a], pts[b] - for i in range(a, b + 1): - traj_v[i] = va + (vb - va) * (i - a) / (b - a) - traj_x = np.linspace(0.0, 70.0, 33) - return FakeModel(velocity=traj_v, position=traj_x) - - -def test_stop_latch_not_released_when_model_reaccelerates_before_actual_stop(): - """Regression: on the logged stop-sign approach the model predicts a dip to - ~0.9 m/s then RE-ACCELERATION to ~7 m/s. When the dip minimum reaches the - current frame (min_idx=0) the model flips a_exp positive. HEM must NOT treat - that as 'stop is over' while the car is still rolling at ~1.8 m/s with the - stop line ~2-4 m ahead. It must keep commanding a kinematic brake. - """ - controller = make_controller(prev=-0.7) - lead = FakeLead(status=False) - - # Phase 1: approach with the dip still ahead (model braking) - model_approach = _stop_sign_dip_model(min_at_idx=11, v_min=0.9, v_horizon=7.0) - for _ in range(40): - controller.update(1.85, 8.05, lead, model_approach, a_chill=-0.009, a_exp=-0.42, t_follow=1.45) - - # Phase 2: model now predicts the minimum is at the CURRENT frame and re-accelerates - model_now = _stop_sign_dip_model(min_at_idx=0, v_min=1.57, v_horizon=8.33) - for _ in range(6): - a = controller.update(1.85, 8.05, lead, model_now, a_chill=-0.009, a_exp=0.12, t_follow=1.45) - # Pure math: at v=1.85 with ~2.5 m to the line, required decel = 1.85^2/(2*2.5) = 0.68 m/s^2 - # HEM must still be braking hard, not letting the car creep through. - assert a <= -0.4, f"HEM released the stop brake on model re-acceleration before stopping, got {a}" \ No newline at end of file diff --git a/tools/replay/export_hem_telemetry.py b/tools/replay/export_hem_telemetry.py new file mode 100644 index 000000000..06f7a16cc --- /dev/null +++ b/tools/replay/export_hem_telemetry.py @@ -0,0 +1,198 @@ +#!/usr/bin/env python3 +"""HEM Telemetry Exporter. + +Extracts real telemetry from target segments and saves them to a portable JSON file, +including authoritative selfdriveState fields (state, active, experimentalMode, alert). +""" +import os +import sys +import json +from pathlib import Path + +# Add openpilot paths +sys.path.insert(0, str(Path(__file__).resolve().parents[2])) + +from openpilot.tools.lib.logreader import LogReader, ReadMode + +ROUTES_TO_EXPORT = [ + ("afb7ef2ed593d651/000000b3--9c3d58d585", 10, "Pure Exp Stop Baseline"), + ("afb7ef2ed593d651/000000b3--9c3d58d585", 11, "Pure Exp Stop Baseline 2"), + ("afb7ef2ed593d651/000000b3--9c3d58d585", 3, "Stop Sign"), + ("afb7ef2ed593d651/000000b5--8ee86fef97", 0, "Stop Sign"), + ("afb7ef2ed593d651/000000b7--9bfbaf247a", 1, "Stop Sign"), + ("afb7ef2ed593d651/000000b9--976a3b50cb", 6, "Red Light"), +] + +def serialize_model(model_msg): + """Extracts the velocity, position, and acceleration lists from modelV2.""" + try: + v_list = list(model_msg.velocity.x) + except Exception: + v_list = [] + try: + x_list = list(model_msg.position.x) + except Exception: + x_list = [] + try: + a_list = list(model_msg.acceleration.x) + except Exception: + a_list = [] + return {"velocity": v_list, "position": x_list, "acceleration": a_list} + +def export_routes(): + exported_data = {} + + for route, seg, label in ROUTES_TO_EXPORT: + route_clean = route.replace("|", "/") + print(f"\nProcessing {route_clean} segment {seg} ({label})...") + + local_paths = [ + Path(f"/data/media/0/realdata/{route_clean}--{seg}"), + Path(os.path.expanduser(f"~/.comma/media/0/realdata/{route_clean}--{seg}")), + Path(f"./{route_clean}--{seg}"), + ] + + lr = None + for path in local_paths: + rlog_file = path / "rlog" + if rlog_file.exists(): + lr = LogReader(str(rlog_file)) + break + + if lr is None: + dongle_id, log_id = route_clean.split("/", 1) + comma_id = f"{dongle_id}|{log_id}/{seg}" + try: + lr = LogReader(comma_id, default_mode=ReadMode.RLOG) + except Exception as e: + print(f"Failed to fetch {comma_id}: {e}") + continue + + car_state_msgs = [] + model_msgs = [] + splan_msgs = [] + lplan_msgs = [] + selfdrive_msgs = [] + controls_state_msgs = [] + car_control_msgs = [] + + try: + for msg in lr: + which = msg.which() + t = msg.logMonoTime * 1e-9 + if which == "carState": + car_state_msgs.append((t, msg.carState)) + elif which == "modelV2": + model_msgs.append((t, msg.modelV2)) + elif which == "starpilotPlan": + splan_msgs.append((t, msg.starpilotPlan)) + elif which == "longitudinalPlan": + lplan_msgs.append((t, msg.longitudinalPlan)) + elif which == "selfdriveState": + selfdrive_msgs.append((t, msg.selfdriveState)) + elif which == "controlsState": + controls_state_msgs.append((t, msg.controlsState)) + elif which == "carControl": + car_control_msgs.append((t, msg.carControl)) + except Exception as e: + print(f"Error reading log: {e}") + continue + + car_state_msgs.sort(key=lambda x: x[0]) + model_msgs.sort(key=lambda x: x[0]) + splan_msgs.sort(key=lambda x: x[0]) + lplan_msgs.sort(key=lambda x: x[0]) + selfdrive_msgs.sort(key=lambda x: x[0]) + controls_state_msgs.sort(key=lambda x: x[0]) + car_control_msgs.sort(key=lambda x: x[0]) + + frames = [] + for t_model, model_msg in model_msgs: + cs = next((m for t, m in reversed(car_state_msgs) if t <= t_model), None) + splan = next((m for t, m in reversed(splan_msgs) if t <= t_model), None) + lplan = next((m for t, m in reversed(lplan_msgs) if t <= t_model), None) + sd = next((m for t, m in reversed(selfdrive_msgs) if t <= t_model), None) + ctrl = next((m for t, m in reversed(controls_state_msgs) if t <= t_model), None) + cc = next((m for t, m in reversed(car_control_msgs) if t <= t_model), None) + + if cs is None: + continue + + # Engagement & State flags + enabled = False + state = 0 + active = False + exp_mode = False + alert = "" + + if sd is not None: + enabled = bool(sd.enabled) + state = int(sd.state.raw) + active = bool(sd.active) + exp_mode = bool(getattr(sd, "experimentalMode", False)) + alert = str(getattr(sd, "alertText1", "")) + elif ctrl is not None: + enabled = bool(ctrl.enabled) + state = 2 if enabled else 0 + active = enabled + + # Fallback starpilotPlan experimental mode check if available + if splan is not None and hasattr(splan, "experimentalMode"): + exp_mode = exp_mode or bool(splan.experimentalMode) + + cc_enabled = bool(getattr(cc, "enabled", False)) + cc_brake = 0.0 + cc_accel = None + if cc is not None: + act = getattr(cc, "actuators", None) + if act is not None: + cc_brake = float(getattr(act, "brake", 0.0) or 0.0) + cc_accel = getattr(act, "accel", None) + + op_braking = bool(cc_enabled and cc_brake > 0.05) + + # Manual driver brake (pedal pressed and not openpilot commanding the brake) + if cc is not None: + manual_brake = bool(cs.brakePressed) and not op_braking + else: + manual_brake = bool(cs.brakePressed) + + frames.append({ + "t": t_model, + "v_ego": float(cs.vEgo), + "v_cruise": float(getattr(splan, "vCruise", getattr(cs, "vCruise", 0.0))), + "a_chill": float(getattr(lplan, "aTarget", 0.0)), + "a_exp": float(getattr(model_msg.action, "desiredAcceleration", 0.0)), + "model_trajectory": serialize_model(model_msg), + "enabled": enabled, + "state": state, + "active": active, + "exp_mode": exp_mode, + "alert": alert, + "brake_pressed": manual_brake, + "brake_raw": bool(cs.brakePressed), + "op_braking": op_braking, + "op_brake_cmd": cc_brake, + "op_accel_cmd": None if cc_accel is None else float(cc_accel), + "gas_pressed": bool(cs.gasPressed), + }) + + if frames: + t0 = frames[0]["t"] + for f in frames: + f["t"] = f["t"] - t0 + + key_name = f"{route_clean.split('/')[-1][:8]}_s{seg}" + exported_data[key_name] = { + "label": label, + "frames": frames, + } + print(f"Successfully processed {len(frames)} frames for {key_name}.") + + output_file = "hem_routes_telemetry.json" + with open(output_file, "w") as f: + json.dump(exported_data, f, indent=2) + print(f"\nSaved export payload to {output_file}") + +if __name__ == "__main__": + export_routes() \ No newline at end of file diff --git a/tools/replay/hem_forensic.py b/tools/replay/hem_forensic.py index 078a5fb7a..44067f075 100644 --- a/tools/replay/hem_forensic.py +++ b/tools/replay/hem_forensic.py @@ -68,27 +68,19 @@ SERVICES = { DIAG_KEYS = [ # inputs "v_ego", "v_cruise", "a_chill", "a_exp", + "should_stop_chill", "should_stop_exp", "lead_status", "lead_d_rel", "lead_v_lead", # vision intent - "v_min", "v_horizon", "speed_drop_ratio", "stop_target_active", "stop_confidence", - "model_decel_strength", "w_vision", - # kinematic stop - "d_min", "d_stop_effective", "a_kinematic_stop", "a_exp_effective", - # authority - "base_auth", "alpha_exp", - # throttle path - "a_throttle_raw", "a_throttle_optimal", "a_throttle_fused", "overshoot_risk", - # brake path - "a_chill_brake", "a_brake_fused", - # regime - "is_braking_phase", "w_accel", "a_fused", - # standstill anchor - "is_stopped", "is_staying_stopped", "model_stop_predicted", - "departing", "standstill_weight", "a_anchored", - # safety - "d_safe", "distance_ratio", "lead_safety_risk", "lead_safety_active", "a_safe", - # slew - "da", "jerk_limit", "max_delta", "a_out", "prev_a_target", + "has_full_trajectory", "v_horizon", "v_short", "v_min", + "speed_drop_ratio", "model_decel_strength", + "raw_vision_metric", "w_target", "w_vision", + # departure / override + "lead_departing", "vision_departing", "driver_override", "is_departing", + "horizon_stopping", + # fusion + "a_brake_fused", "a_throttle_fused", "is_stopping_event", "exp_dominant", + # standstill handshake + "standstill_intent", "should_stop_fused", "a_out", ] REGIME_KEYS = ["regime", "standstill"] @@ -152,12 +144,18 @@ def run_forensic(grid, bufs, toggles): out["brake_pressed"] = np.zeros(n, dtype=bool) out["gas_pressed"] = np.zeros(n, dtype=bool) for k in DIAG_KEYS: - if k in ("is_braking_phase", "model_stop_predicted", "lead_safety_active", "departing"): + if k in ("should_stop_chill", "should_stop_exp", "lead_status", "has_full_trajectory", + "lead_departing", "vision_departing", "driver_override", "is_departing", + "horizon_stopping", "is_stopping_event", "exp_dominant", "standstill_intent", + "should_stop_fused"): out["hem_" + k] = np.zeros(n, dtype=bool) else: out["hem_" + k] = np.zeros(n) out["hem_regime"] = np.zeros(n, dtype=bool) out["hem_standstill"] = np.zeros(n, dtype=bool) + # Gates dropped in the new HEM design map onto the surviving output signal. + out["hem_a_anchored"] = np.full(n, np.nan) + out["hem_a_safe"] = np.zeros(n) real_monotonic = time.monotonic fake_clock = [0.0] @@ -298,7 +296,18 @@ def run_forensic(grid, bufs, toggles): except Exception: a_exp = 0.0 - a_hem = hybrid.update(v_ego, v_cruise, lead, model_v2, a_chill, a_exp, t_follow=t_follow) + should_stop_exp = False + if model_v2 is not None: + try: + should_stop_exp = bool(getattr(getattr(model_v2, "action", None), "shouldStop", False)) + except Exception: + should_stop_exp = False + a_hem, should_stop_fused = hybrid.update( + v_ego=v_ego, v_cruise=v_cruise, lead_one=lead, model_v2=model_v2, + a_chill=a_chill, a_exp=a_exp, + should_stop_exp=should_stop_exp, + should_stop_chill=bool(getattr(lplan, "shouldStop", False)), + ) model_v0 = v_ego if model_v2 is not None: @@ -315,7 +324,7 @@ def run_forensic(grid, bufs, toggles): out["a_chill"][i] = a_chill out["a_exp"][i] = a_exp out["hem_a"][i] = a_hem - out["hem_authority"][i] = hybrid.exp_authority + out["hem_authority"][i] = hybrid.w_vision out["lead_status"][i] = lead.status out["lead_d_rel"][i] = lead.dRel out["lead_v_lead"][i] = lead.vLead @@ -335,6 +344,8 @@ def run_forensic(grid, bufs, toggles): out["hem_" + k][i] = 0.0 out["hem_regime"][i] = hybrid.diag.get("regime") == "brake" out["hem_standstill"][i] = bool(hybrid.diag.get("standstill", False)) + out["hem_a_anchored"][i] = out["hem_a"][i] if out["hem_standstill"][i] else np.nan + out["hem_a_safe"][i] = out["hem_a"][i] finally: time.monotonic = real_monotonic @@ -357,7 +368,7 @@ def detect_incidents(out, t): v = out["v_ego"][i] w = out["hem_w_vision"][i] ac = out["hem_a_chill"][i] - ae_eff = out["hem_a_exp_effective"][i] + ae_eff = out["hem_a_exp"][i] aout = out["hem_a"][i] if v > 0.5 and w > 0.3 and ac > -0.3 and ae_eff >= 0.0 and abs(aout) < 0.25: idxs.append(i) @@ -380,7 +391,7 @@ def classify_failure(out, sl, i_brake): if pre.stop <= pre.start: return "UNKNOWN", {} v = out["v_ego"][pre] - exp_eff = out["hem_a_exp_effective"][pre] + exp_eff = out["hem_a_exp"][pre] wv = out["hem_w_vision"][pre] hem = out["hem_a"][pre] chill = out["hem_a_chill"][pre] @@ -478,9 +489,9 @@ def print_forensic_log(out, sl, t0): f"{'<<<<' if out['a_ego'][i] < -2.0 else ('Y' if out['brake_pressed'][i] else '-'):>6}", f"{out['hem_a_chill'][i]:6.2f}", f"{out['hem_a_exp'][i]:6.2f}", - f"{out['hem_a_exp_effective'][i]:6.2f}", + f"{out['hem_a_exp'][i]:6.2f}", f"{out['hem_w_vision'][i]:5.2f}", - f"{out['hem_alpha_exp'][i]:5.2f}", + f"{out['hem_authority'][i]:5.2f}", f"{'brake' if out['hem_regime'][i] else 'throttle':>8}", f"{'Y' if out['hem_standstill'][i] else '-':>5}", f"{out['hem_a_brake_fused'][i]:6.2f}", @@ -606,8 +617,8 @@ def plot_results(out, t0, args, incident_idxs): # vision weight, authority, exp_effective axs[2].plot(t0, out["hem_w_vision"], color="tab:red", lw=1.3, label="w_vision") - axs[2].plot(t0, out["hem_alpha_exp"], color="tab:purple", lw=1.3, label="exp authority") - axs[2].plot(t0, out["hem_a_exp_effective"], color="tab:olive", lw=1.0, ls="--", label="exp effective a") + axs[2].plot(t0, out["hem_authority"], color="tab:purple", lw=1.3, label="exp authority") + axs[2].plot(t0, out["hem_a_exp"], color="tab:olive", lw=1.0, ls="--", label="exp a") axs[2].axhline(0.3, color="tab:red", lw=0.6, ls=":") axs[2].set_ylim(-1, 1.5) axs[2].set_ylabel("weight") diff --git a/tools/replay/hem_stop_analyzer.py b/tools/replay/hem_stop_analyzer.py index 469bace27..edf41642e 100644 --- a/tools/replay/hem_stop_analyzer.py +++ b/tools/replay/hem_stop_analyzer.py @@ -128,7 +128,7 @@ def simulate_hem(data_frames): lead = MockLead(status=False) # Execute state update - a_out = controller.update( + a_out, should_stop_fused = controller.update( v_ego=frame["v_ego"], v_cruise=frame["v_cruise"], lead_one=lead, @@ -139,6 +139,7 @@ def simulate_hem(data_frames): diag = dict(controller.diag) diag["a_out"] = float(a_out) + diag["should_stop_fused"] = bool(should_stop_fused) diag["t_rel"] = frame["t"] - data_frames[0]["t"] sim_results.append(diag) @@ -155,10 +156,10 @@ def analyze_failures(route_str, segment, label, results): max_v = max(v_speeds) min_v = min(v_speeds) - # Identify frames with stop signs visible in model (high stop confidence or tracked distance exists) + # Identify frames with a stop visible to vision (horizon stop or exp stop flag) active_frames = [] for idx, r in enumerate(results): - if (r.get("stop_confidence", 0.0) > 0.1) or (r.get("tracked_stop_dist") is not None): + if r.get("horizon_stopping", False) or r.get("should_stop_exp", False) or r.get("w_vision", 0.0) > 0.1: active_frames.append(idx) if not active_frames: @@ -181,22 +182,22 @@ def analyze_failures(route_str, segment, label, results): r = results[idx] # 1. Check for premature latch decay (decaying while still moving fast) - if r.get("w_vision", 0.0) < 0.2 and r.get("v_ego", 0.0) > 2.0 and r.get("stop_confidence", 0.0) > 0.4: + if r.get("w_vision", 0.0) < 0.2 and r.get("v_ego", 0.0) > 2.0 and r.get("horizon_stopping", False): latch_decays += 1 - # 2. Check for tracked stop distance wiping out/clearing while speed is high + # 2. Check for stop detection being cleared while speed is still high if idx > start_idx: prev_r = results[idx - 1] - if prev_r.get("tracked_stop_dist") is not None and r.get("tracked_stop_dist") is None: - if r.get("v_ego", 0.0) > 1.0 and not r.get("vision_departing", False): + if prev_r.get("horizon_stopping", False) and not r.get("horizon_stopping", False): + if r.get("v_ego", 0.0) > 1.0 and not r.get("is_departing", False): tracking_resets += 1 - # 3. Check for early departure trigger causing positive creep acceleration override - if r.get("departing", False) and r.get("v_ego", 0.0) > 1.5 and r.get("d_stop_calc", 999) > 0.5: + # 3. Check for departure lockout firing while still approaching a predicted stop + if r.get("is_departing", False) and r.get("v_ego", 0.0) > 1.5 and r.get("horizon_stopping", False): early_departures += 1 - # 4. Check if the kinematic decel collapsed near the stop line - if r.get("a_kinematic_stop", 0.0) > -0.1 and r.get("v_ego", 0.0) > 1.0 and 0.5 < r.get("tracked_stop_dist", 999) < 8.0: + # 4. Check if the brake floor collapsed near the stop line + if r.get("a_brake_fused", 0.0) > -0.1 and r.get("v_ego", 0.0) > 1.0 and r.get("horizon_stopping", False): kinematic_collapses += 1 # Determine primary failure mode @@ -204,11 +205,11 @@ def analyze_failures(route_str, segment, label, results): if latch_decays > 5: failure_modes.append("Premature Latch Decay (w_vision collapsed)") if tracking_resets > 0: - failure_modes.append("Stop Distance Tracker Cleared Early") + failure_modes.append("Stop Detection Cleared Early") if early_departures > 5: - failure_modes.append("Early Departure Lockout Bypass (departing=True while approaching)") + failure_modes.append("Early Departure Lockout Bypass (is_departing while approaching)") if kinematic_collapses > 5: - failure_modes.append("Kinematic Stop Floor Collapse near stop line") + failure_modes.append("Kinematic Brake Floor Collapse near stop line") failure_mode = " / ".join(failure_modes) if failure_modes else "Weak general deceleration tracking" outcome = f"Blew past stop line. Min speed reached: {min_v:.2f} m/s." if min_v > 0.5 else "Stopped but late/harsh." @@ -255,17 +256,13 @@ def run_suite(): # General findings f.write("COMMON STRUCTURAL ROOT CAUSES IDENTIFIED:\n") f.write("------------------------------------------\n") - f.write("1. Stop Tracker Resetting on Model Re-acceleration:\n") - f.write(" When stop lines approach index 0, the model's trajectory velocity endpoint\n") - f.write(" flip positive (a_exp > 0.1, v_horizon > 0.5). Because 'model_stop_predicted'\n") - f.write(" evaluates to False, the 'vision_departing' or 'departing' signal fires TRUE.\n") - f.write(" This instantly triggers 'self.tracked_stop_dist = None', deleting the\n") - f.write(" kinematic decel floor while the car is still traveling at speed close to the line.\n\n") + f.write("1. Stop Detection Cleared on Model Re-acceleration:\n") + f.write(" When the trajectory velocity endpoint flips positive (a_exp > 0.1, v_horizon > 1.2)\n") + f.write(" the 'vision_departing' / 'is_departing' signal fires TRUE and clears 'horizon_stopping'\n") + f.write(" while the car is still traveling at speed close to the line, dropping the brake floor.\n\n") f.write("2. Insufficient Latch Sustainability (w_vision decays):\n") - f.write(" If the model stops outputting a highly confident slow-down endpoint, even briefly,\n") - f.write(" the soft latch 'w_vision_filtered' is multiplied by 0.90 or 0.97. If it decays\n") - f.write(" below 0.25, the system unlocks the throttle override lockouts, reverting to CCM/chill\n") - f.write(" creep commands.\n\n") + f.write(" If the model stops outputting a low velocity endpoint, even briefly, the vision weight\n") + f.write(" decays toward zero and unlocks cruise throttle while still closing on the stop.\n\n") f.write("DETAILED SEGMENT TELEMETRY BREAKDOWN:\n") f.write("-------------------------------------\n") @@ -294,7 +291,7 @@ def run_suite(): v_ego = [r["v_ego"] for r in results] w_vis = [r["w_vision"] for r in results] a_out = [r["a_out"] for r in results] - a_kin = [r["a_kinematic_stop"] for r in results] + a_kin = [r["a_brake_fused"] for r in results] # Left column: Speeds and Latch activations ax_l = axes[idx, 0] diff --git a/tools/replay/hem_stop_analyzer.py.py b/tools/replay/hem_stop_analyzer.py.py index 469bace27..edf41642e 100644 --- a/tools/replay/hem_stop_analyzer.py.py +++ b/tools/replay/hem_stop_analyzer.py.py @@ -128,7 +128,7 @@ def simulate_hem(data_frames): lead = MockLead(status=False) # Execute state update - a_out = controller.update( + a_out, should_stop_fused = controller.update( v_ego=frame["v_ego"], v_cruise=frame["v_cruise"], lead_one=lead, @@ -139,6 +139,7 @@ def simulate_hem(data_frames): diag = dict(controller.diag) diag["a_out"] = float(a_out) + diag["should_stop_fused"] = bool(should_stop_fused) diag["t_rel"] = frame["t"] - data_frames[0]["t"] sim_results.append(diag) @@ -155,10 +156,10 @@ def analyze_failures(route_str, segment, label, results): max_v = max(v_speeds) min_v = min(v_speeds) - # Identify frames with stop signs visible in model (high stop confidence or tracked distance exists) + # Identify frames with a stop visible to vision (horizon stop or exp stop flag) active_frames = [] for idx, r in enumerate(results): - if (r.get("stop_confidence", 0.0) > 0.1) or (r.get("tracked_stop_dist") is not None): + if r.get("horizon_stopping", False) or r.get("should_stop_exp", False) or r.get("w_vision", 0.0) > 0.1: active_frames.append(idx) if not active_frames: @@ -181,22 +182,22 @@ def analyze_failures(route_str, segment, label, results): r = results[idx] # 1. Check for premature latch decay (decaying while still moving fast) - if r.get("w_vision", 0.0) < 0.2 and r.get("v_ego", 0.0) > 2.0 and r.get("stop_confidence", 0.0) > 0.4: + if r.get("w_vision", 0.0) < 0.2 and r.get("v_ego", 0.0) > 2.0 and r.get("horizon_stopping", False): latch_decays += 1 - # 2. Check for tracked stop distance wiping out/clearing while speed is high + # 2. Check for stop detection being cleared while speed is still high if idx > start_idx: prev_r = results[idx - 1] - if prev_r.get("tracked_stop_dist") is not None and r.get("tracked_stop_dist") is None: - if r.get("v_ego", 0.0) > 1.0 and not r.get("vision_departing", False): + if prev_r.get("horizon_stopping", False) and not r.get("horizon_stopping", False): + if r.get("v_ego", 0.0) > 1.0 and not r.get("is_departing", False): tracking_resets += 1 - # 3. Check for early departure trigger causing positive creep acceleration override - if r.get("departing", False) and r.get("v_ego", 0.0) > 1.5 and r.get("d_stop_calc", 999) > 0.5: + # 3. Check for departure lockout firing while still approaching a predicted stop + if r.get("is_departing", False) and r.get("v_ego", 0.0) > 1.5 and r.get("horizon_stopping", False): early_departures += 1 - # 4. Check if the kinematic decel collapsed near the stop line - if r.get("a_kinematic_stop", 0.0) > -0.1 and r.get("v_ego", 0.0) > 1.0 and 0.5 < r.get("tracked_stop_dist", 999) < 8.0: + # 4. Check if the brake floor collapsed near the stop line + if r.get("a_brake_fused", 0.0) > -0.1 and r.get("v_ego", 0.0) > 1.0 and r.get("horizon_stopping", False): kinematic_collapses += 1 # Determine primary failure mode @@ -204,11 +205,11 @@ def analyze_failures(route_str, segment, label, results): if latch_decays > 5: failure_modes.append("Premature Latch Decay (w_vision collapsed)") if tracking_resets > 0: - failure_modes.append("Stop Distance Tracker Cleared Early") + failure_modes.append("Stop Detection Cleared Early") if early_departures > 5: - failure_modes.append("Early Departure Lockout Bypass (departing=True while approaching)") + failure_modes.append("Early Departure Lockout Bypass (is_departing while approaching)") if kinematic_collapses > 5: - failure_modes.append("Kinematic Stop Floor Collapse near stop line") + failure_modes.append("Kinematic Brake Floor Collapse near stop line") failure_mode = " / ".join(failure_modes) if failure_modes else "Weak general deceleration tracking" outcome = f"Blew past stop line. Min speed reached: {min_v:.2f} m/s." if min_v > 0.5 else "Stopped but late/harsh." @@ -255,17 +256,13 @@ def run_suite(): # General findings f.write("COMMON STRUCTURAL ROOT CAUSES IDENTIFIED:\n") f.write("------------------------------------------\n") - f.write("1. Stop Tracker Resetting on Model Re-acceleration:\n") - f.write(" When stop lines approach index 0, the model's trajectory velocity endpoint\n") - f.write(" flip positive (a_exp > 0.1, v_horizon > 0.5). Because 'model_stop_predicted'\n") - f.write(" evaluates to False, the 'vision_departing' or 'departing' signal fires TRUE.\n") - f.write(" This instantly triggers 'self.tracked_stop_dist = None', deleting the\n") - f.write(" kinematic decel floor while the car is still traveling at speed close to the line.\n\n") + f.write("1. Stop Detection Cleared on Model Re-acceleration:\n") + f.write(" When the trajectory velocity endpoint flips positive (a_exp > 0.1, v_horizon > 1.2)\n") + f.write(" the 'vision_departing' / 'is_departing' signal fires TRUE and clears 'horizon_stopping'\n") + f.write(" while the car is still traveling at speed close to the line, dropping the brake floor.\n\n") f.write("2. Insufficient Latch Sustainability (w_vision decays):\n") - f.write(" If the model stops outputting a highly confident slow-down endpoint, even briefly,\n") - f.write(" the soft latch 'w_vision_filtered' is multiplied by 0.90 or 0.97. If it decays\n") - f.write(" below 0.25, the system unlocks the throttle override lockouts, reverting to CCM/chill\n") - f.write(" creep commands.\n\n") + f.write(" If the model stops outputting a low velocity endpoint, even briefly, the vision weight\n") + f.write(" decays toward zero and unlocks cruise throttle while still closing on the stop.\n\n") f.write("DETAILED SEGMENT TELEMETRY BREAKDOWN:\n") f.write("-------------------------------------\n") @@ -294,7 +291,7 @@ def run_suite(): v_ego = [r["v_ego"] for r in results] w_vis = [r["w_vision"] for r in results] a_out = [r["a_out"] for r in results] - a_kin = [r["a_kinematic_stop"] for r in results] + a_kin = [r["a_brake_fused"] for r in results] # Left column: Speeds and Latch activations ax_l = axes[idx, 0] diff --git a/tools/replay/mode_sim.py b/tools/replay/mode_sim.py index 671fcb446..02e815bfb 100644 --- a/tools/replay/mode_sim.py +++ b/tools/replay/mode_sim.py @@ -532,8 +532,11 @@ def run_simulation(grid, bufs, toggles): a_exp = 0.0 # Hybrid Experimental Mode continuous fusion - a_hem = hybrid.update(v_ego, v_cruise, lead, model_v2, a_chill, a_exp, t_follow=t_follow) - authority = hybrid.exp_authority + a_hem, _should_stop_fused = hybrid.update( + v_ego=v_ego, v_cruise=v_cruise, lead_one=lead, model_v2=model_v2, + a_chill=a_chill, a_exp=a_exp, + ) + authority = hybrid.w_vision model_v0 = v_ego if model_v2 is not None: