From 5f95574f02edc8e04d73fc946de52baf61964762 Mon Sep 17 00:00:00 2001 From: Prabhaav Pillai Date: Thu, 27 Aug 2026 14:36:33 -0400 Subject: [PATCH] Add HybridExperimental mode --- common/params_keys.h | 4 + .../controls/lib/longitudinal_planner.py | 137 ++- .../tests/test_longitudinal_planner.py | 42 + .../tests/test_longitudinal_planner_hem.py | 284 ++++++ .../controls/tests/test_starpilot_planner.py | 15 + selfdrive/ui/lib/starpilot_status.py | 32 + selfdrive/ui/tests/test_starpilot_status.py | 37 +- .../common/assets/device_settings_layout.json | 38 + starpilot/common/safe_mode.py | 1 + starpilot/common/starpilot_variables.py | 25 +- .../common/tests/test_starpilot_variables.py | 12 + .../controls/lib/hybrid_experimental_mode.py | 153 ++++ starpilot/controls/starpilot_card.py | 3 + starpilot/controls/starpilot_planner.py | 6 +- .../tests/test_hybrid_experimental_mode.py | 336 +++++++ .../assets/components/tools/tmux.js | 66 +- starpilot/system/the_galaxy/the_galaxy.py | 70 +- tools/replay/export_hem_telemetry.py | 234 +++++ tools/replay/hem_forensic.py | 783 ++++++++++++++++ tools/replay/hem_stop_analyzer.py | 325 +++++++ tools/replay/hem_stop_analyzer.py.py | 325 +++++++ tools/replay/mode_sim.py | 840 ++++++++++++++++++ 22 files changed, 3732 insertions(+), 36 deletions(-) create mode 100644 selfdrive/controls/tests/test_longitudinal_planner_hem.py create mode 100644 starpilot/controls/lib/hybrid_experimental_mode.py create mode 100644 starpilot/controls/tests/test_hybrid_experimental_mode.py create mode 100644 tools/replay/export_hem_telemetry.py create mode 100644 tools/replay/hem_forensic.py create mode 100644 tools/replay/hem_stop_analyzer.py create mode 100644 tools/replay/hem_stop_analyzer.py.py create mode 100644 tools/replay/mode_sim.py diff --git a/common/params_keys.h b/common/params_keys.h index 4c5bb81d8..02e013294 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -242,6 +242,10 @@ inline static std::unordered_map keys = { {"CommunityFavorites", {PERSISTENT, STRING, "", "", 1}}, {"ConditionalChill", {PERSISTENT, BOOL, "0", "0", 1}}, {"ConditionalExperimental", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}}, + {"HybridExpBias", {PERSISTENT, FLOAT, "0", "0", 1}}, + {"HybridExperimental", {PERSISTENT, BOOL, "0", "0", 1}}, + {"HybridVisionBrakeSensitivity", {PERSISTENT, FLOAT, "1", "1", 1}}, + {"HEMExpDominant", {CLEAR_ON_MANAGER_START, BOOL, "0", "0", 2}}, {"CurvatureData", {PERSISTENT | DONT_LOG, JSON, "{}", "{}"}}, {"CurveSpeedController", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}}, {"CurveSpeedControllerNoLead", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}}, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index b9dedda37..5c04e8825 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -7,8 +7,10 @@ from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX from openpilot.common.constants import CV from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL +from openpilot.common.params import Params from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.starpilot.common.model_versions import is_tinygrad_model_version +from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode from openpilot.starpilot.controls.lib.starpilot_vcruise import FT_TO_M, OFFSET_FT_MAX, OFFSET_FT_MIN from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc @@ -142,6 +144,26 @@ VISION_LEAD_APPROACH_BRAKING_DEFICIT_MIN = 0.75 VISION_LEAD_APPROACH_BRAKING_MIN_LEAD_BRAKE = 0.45 VISION_LEAD_APPROACH_BRAKING_FULL_LEAD_BRAKE = 1.20 PLANNER_SAFETY_WARNING_INTERVAL = 5.0 +HEM_STATUS_LOG_INTERVAL = 10.0 +HEM_AUTH_PUB_INTERVAL = 0.5 + + +def _hem_log_timestamp() -> str: + """Wall-clock timestamp with millisecond precision for the [HEM] live log.""" + from datetime import datetime + now = datetime.now() + return now.strftime("%H:%M:%S") + f".{now.microsecond // 1000:03d}" + + +def _hem_format_diag_value(value): + """Compact, deterministic formatting for the per-frame HEM diagnostic dump.""" + if isinstance(value, (bool, np.bool_)): + return "1" if value else "0" + if isinstance(value, float): + return f"{value:.4f}" + if isinstance(value, (int, np.integer)): + return str(int(value)) + return str(value) VISION_LEAD_APPROACH_BRAKING_FLOOR_MIN_DECEL = 1.30 VISION_LEAD_APPROACH_BRAKING_FLOOR_MAX_DECEL = 1.75 VISION_LEAD_APPROACH_CONFIRM_TIME = 0.25 @@ -611,6 +633,11 @@ class LongitudinalPlanner: self.duplicate_vision_comfort_lead_source = None self.prev_experimental_mode = None self.experimental_release_accel_until = 0.0 + self.hybrid_controller = HybridExperimentalMode() + self._hem_status_log_t = 0.0 + self._hem_logged_active = False + self._hem_auth_pub_t = 0.0 + self._hem_params_memory = None if self.is_preap: try: @@ -1910,6 +1937,36 @@ class LongitudinalPlanner: floor = min(LC_MERGE_ACCEL_BIAS, cruise_cap) return floor + def _log_hem_status(self, now_t, active, v_ego, a_chill, a_exp, a_fused): + hc = self.hybrid_controller + hc.record_diag = True + ts = _hem_log_timestamp() + if active != self._hem_logged_active: + self._hem_logged_active = active + self._hem_status_log_t = 0.0 + print(f"[HEM] {ts} mode {'ON' if active else 'OFF'}") + if not active: + return + # Rich per-frame dump of every HEM decision variable so a missed stop can be + # diagnosed to the exact frame and signal (see hybrid_experimental_mode diag). + d = hc.diag + if d: + print(f"[HEM] {ts} " + " ".join(f"{k}={_hem_format_diag_value(v)}" for k, v in d.items())) + + def _publish_hem_status(self, now_t): + if self._hem_params_memory is None: + try: + self._hem_params_memory = Params(memory=True) + except Exception: + return + if now_t - self._hem_auth_pub_t < HEM_AUTH_PUB_INTERVAL: + return + self._hem_auth_pub_t = now_t + try: + self._hem_params_memory.put_bool("HEMExpDominant", bool(self.hybrid_controller.last_exp_dominant)) + except Exception: + pass + def update(self, sm, starpilot_toggles): if self.is_preap: self._preap_param_frame += 1 @@ -1962,6 +2019,7 @@ class LongitudinalPlanner: self.a_desired = np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1]) self.model_allow_throttle = True self.model_allow_throttle_transition_t = 0.0 + self.hybrid_controller.reset(float(self.a_desired)) # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) @@ -2323,26 +2381,31 @@ class LongitudinalPlanner: model_launch_accel = self.get_model_launch_accel(model_launch_v, model_launch_a, action_t, scene_v_ego) if classic_model: - output_a_target, output_should_stop = get_accel_from_plan_classic( + output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan_classic( self.CP, self.v_desired_trajectory, self.a_desired_trajectory, starpilot_toggles.vEgoStopping) elif tinygrad_model: output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan( self.v_desired_trajectory, self.a_desired_trajectory, action_t=action_t, vEgoStopping=starpilot_toggles.vEgoStopping) - output_a_target_e2e = sm['modelV2'].action.desiredAcceleration - output_should_stop_e2e = sm['modelV2'].action.shouldStop - - if self.mode == 'acc' or self.generation == 'v9': - output_a_target = output_a_target_mpc - output_should_stop = output_should_stop_mpc - else: - output_a_target = min(output_a_target_mpc, output_a_target_e2e) - output_should_stop = output_should_stop_e2e or output_should_stop_mpc else: - output_a_target, output_should_stop = get_accel_from_plan( + output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan( self.v_desired_trajectory, self.a_desired_trajectory, action_t=action_t, vEgoStopping=starpilot_toggles.vEgoStopping) + if bool(getattr(starpilot_toggles, "hybrid_experimental_mode", False)): + 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 + output_a_target = min(output_a_target_mpc, output_a_target_e2e) + output_should_stop = output_should_stop_e2e or output_should_stop_mpc + self._log_hem_status(now_t, False, scene_v_ego, output_a_target_mpc, output_a_target_e2e, output_a_target) + else: + output_a_target = output_a_target_mpc + output_should_stop = output_should_stop_mpc + self._log_hem_status(now_t, False, scene_v_ego, output_a_target_mpc, float('nan'), output_a_target) + comfort_output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0] vision_cap_accel_min = min(comfort_output_accel_min, get_vehicle_min_accel(self.CP, v_ego)) output_accel_min = comfort_output_accel_min @@ -2975,8 +3038,56 @@ 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, + gas_pressed=bool(getattr(sm['carState'], 'gasPressed', False)), + ) + + # 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 619b4cac3..861d7208d 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -607,6 +607,48 @@ def test_experimental_mlsim_uses_vehicle_min_accel_floor(model_version): assert planner.output_a_target < comfort_min_accel +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) + + # 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.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 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")) + assert planner_exp.output_a_target <= -1.5 + + +def test_hybrid_mode_arbitrates_vision_braking_with_lead(): + v_ego = 20.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + hybrid_toggles = SimpleNamespace(**vars(make_toggles("v11")), hybrid_experimental_mode=True) + + # With a tracked lead, the hybrid grants full E2E braking authority. + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm( + v_ego, + desired_accel=-2.0, + min_accel=-2.0, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=30.0, v_lead=10.0), + ) + planner.update(sm, hybrid_toggles) + assert planner.output_a_target < 0.0 + + def test_gm_pedal_vehicle_min_accel_uses_brand_when_car_name_is_missing(): CP = SimpleNamespace( carName=None, 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/controls/tests/test_starpilot_planner.py b/selfdrive/controls/tests/test_starpilot_planner.py index 8625ba6b6..ece7086d7 100644 --- a/selfdrive/controls/tests/test_starpilot_planner.py +++ b/selfdrive/controls/tests/test_starpilot_planner.py @@ -189,6 +189,21 @@ def test_lateral_resume_delay_ignores_signal_cycles_that_never_slow_enough(monke planner.shutdown() +def test_hybrid_mode_keeps_cem_detector_warm_with_modes_off(monkeypatch): + planner = make_planner(monkeypatch) + monkeypatch.setattr(planner.starpilot_cem, "update", lambda *args, **kwargs: None) + + try: + toggles = make_toggles(hybrid_experimental_mode=True) + + planner.update(0.0, False, make_sm(planner, frame=1, v_ego=20.0, left_blinker=False), toggles) + + assert planner.starpilot_ccm.experimental_mode is False + assert planner.starpilot_cem.experimental_mode is False + finally: + planner.shutdown() + + def test_radarless_follow_hold_applies_to_tracked_vision_lead(monkeypatch): planner = StarPilotPlanner(Path("/tmp/nonexistent"), DummyThemeManager()) diff --git a/selfdrive/ui/lib/starpilot_status.py b/selfdrive/ui/lib/starpilot_status.py index 161c23457..210ab7fad 100644 --- a/selfdrive/ui/lib/starpilot_status.py +++ b/selfdrive/ui/lib/starpilot_status.py @@ -11,6 +11,7 @@ AOL_COLOR = rl.Color(10, 186, 181, 255) ENGAGED_COLOR = rl.Color(22, 127, 64, 255) OVERRIDE_COLOR = rl.Color(137, 146, 141, 255) EXPERIMENTAL_COLOR = rl.Color(218, 111, 37, 255) +HYBRID_EXPERIMENTAL_COLOR = rl.Color(0, 153, 255, 255) CEM_OVERRIDE_COLOR = rl.Color(255, 214, 0, 255) SWITCHBACK_COLOR = rl.Color(139, 108, 197, 255) TRAFFIC_COLOR = rl.Color(201, 34, 49, 255) @@ -27,6 +28,31 @@ def is_longitudinal_only_active(state: UIState) -> bool: return bool(state.sm["selfdriveState"].enabled and not car_control.latActive) +def _is_hybrid_experimental_mode(state: UIState) -> bool: + """True when HEM (hybrid experimental) is the active longitudinal mode.""" + return bool(state.starpilot_toggles.get("hybrid_experimental_mode", False)) + + +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), which reflects the + active fusion regime rather than a raw vision weight. + """ + params_memory = getattr(state, "params_memory", None) + if params_memory is None: + return False + try: + return bool(int(params_memory.get("HEMExpDominant") or 0)) + except (TypeError, ValueError): + return False + + +def _hem_border_color(state: UIState) -> rl.Color: + """Blue when chill dominates the fusion, orange when experimental/vision dominates (like CEM).""" + return EXPERIMENTAL_COLOR if _hem_exp_dominant(state) else HYBRID_EXPERIMENTAL_COLOR + + def _override_color_applies(state: UIState) -> bool: """Only gray the status when the active control mode is being overridden.""" if state.status != UIStatus.OVERRIDE: @@ -57,6 +83,8 @@ def get_border_color(state: UIState): if state.always_on_lateral_active: return AOL_COLOR # Only color the border for CEM/experimental while actually enabled. + if enabled and _is_hybrid_experimental_mode(state): + return _hem_border_color(state) if enabled and state.conditional_status in CEM_DISABLED_OVERRIDE_STATUSES: return CEM_OVERRIDE_COLOR if enabled and state.sm["selfdriveState"].experimentalMode: @@ -67,6 +95,8 @@ def get_border_color(state: UIState): def get_path_edge_color(state: UIState): + if state.sm["selfdriveState"].enabled and _is_hybrid_experimental_mode(state): + return _hem_border_color(state) if state.conditional_status in CEM_ACTIVE_STATUSES: return EXPERIMENTAL_COLOR return get_border_color(state) @@ -85,6 +115,8 @@ def get_screen_edge_color(state: UIState): return AOL_COLOR # Keep the screen edge disengaged-blue when experimental mode is only the # requested longitudinal mode, not the active driving state. + if enabled and _is_hybrid_experimental_mode(state): + return _hem_border_color(state) if enabled and state.conditional_status in CEM_DISABLED_OVERRIDE_STATUSES: return CEM_OVERRIDE_COLOR if enabled and state.sm["selfdriveState"].experimentalMode: diff --git a/selfdrive/ui/tests/test_starpilot_status.py b/selfdrive/ui/tests/test_starpilot_status.py index e2c4dc3c9..287b73a8f 100644 --- a/selfdrive/ui/tests/test_starpilot_status.py +++ b/selfdrive/ui/tests/test_starpilot_status.py @@ -3,16 +3,20 @@ from types import SimpleNamespace from openpilot.selfdrive.ui.lib.starpilot_status import ( DISENGAGED_COLOR, ENGAGED_COLOR, + EXPERIMENTAL_COLOR, + HYBRID_EXPERIMENTAL_COLOR, LONGITUDINAL_ONLY_COLOR, AOL_COLOR, OVERRIDE_COLOR, get_border_color, + get_path_edge_color, get_screen_edge_color, ) from openpilot.selfdrive.ui.ui_state import UIStatus -def _state(*, enabled=False, lat_active=False, aol=False, status=None, events=()): +def _state(*, enabled=False, lat_active=False, aol=False, status=None, events=(), hybrid=False, hem_exp_dominant=None): + params_memory = {"HEMExpDominant": b"1"} if hem_exp_dominant else {} return SimpleNamespace( sm={ "selfdriveState": SimpleNamespace(enabled=enabled, experimentalMode=False), @@ -24,6 +28,8 @@ def _state(*, enabled=False, lat_active=False, aol=False, status=None, events=() switchback_mode_enabled=False, traffic_mode_enabled=False, conditional_status=0, + starpilot_toggles={"hybrid_experimental_mode": hybrid}, + params_memory=SimpleNamespace(get=lambda key, default=None: params_memory.get(key, default)), ) @@ -44,6 +50,35 @@ def test_lateral_active_colors_remain_unchanged(): assert _rgb(get_border_color(_state())) == _rgb(DISENGAGED_COLOR) +def test_hybrid_experimental_mode_uses_blue_border(): + state = _state(enabled=True, lat_active=True, hybrid=True) + + assert _rgb(get_border_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR) + assert _rgb(get_screen_edge_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR) + assert _rgb(get_path_edge_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR) + + +def test_hybrid_experimental_mode_uses_orange_when_exp_dominates(): + state = _state(enabled=True, lat_active=True, hybrid=True, hem_exp_dominant=True) + + assert _rgb(get_border_color(state)) == _rgb(EXPERIMENTAL_COLOR) + assert _rgb(get_screen_edge_color(state)) == _rgb(EXPERIMENTAL_COLOR) + assert _rgb(get_path_edge_color(state)) == _rgb(EXPERIMENTAL_COLOR) + + +def test_hybrid_experimental_mode_keeps_blue_when_chill_dominates(): + state = _state(enabled=True, lat_active=True, hybrid=True, hem_exp_dominant=False) + + assert _rgb(get_border_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR) + assert _rgb(get_screen_edge_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR) + assert _rgb(get_path_edge_color(state)) == _rgb(HYBRID_EXPERIMENTAL_COLOR) + + +def test_hybrid_experimental_mode_requires_enabled(): + assert _rgb(get_border_color(_state(hybrid=True))) == _rgb(DISENGAGED_COLOR) + assert _rgb(get_border_color(_state(enabled=True, lat_active=True))) == _rgb(ENGAGED_COLOR) + + def test_override_color_matches_active_control_mode(): lateral_override = SimpleNamespace(overrideLateral=True, overrideLongitudinal=False) longitudinal_override = SimpleNamespace(overrideLateral=False, overrideLongitudinal=True) diff --git a/starpilot/common/assets/device_settings_layout.json b/starpilot/common/assets/device_settings_layout.json index 61d28f3ec..750b8fee1 100644 --- a/starpilot/common/assets/device_settings_layout.json +++ b/starpilot/common/assets/device_settings_layout.json @@ -2261,6 +2261,44 @@ "parent_key": "ConditionalChill", "settings_tier": "advanced" }, + { + "key": "HybridExperimental", + "label": "Hybrid Experimental Mode", + "description": "Fuse the throttle response of chill ACC with the model's early, natural vision braking using a single continuous controller. Chill ACC always provides the hard safety distance, while the E2E trajectory is blended in for early stops. Unconfirmed vision braking is capped to a gentle coast, and vision is never allowed to under-brake.", + "picker_description": "Continuously blends Chill ACC with the model's vision braking.", + "data_type": "bool", + "ui_type": "toggle", + "is_parent_toggle": true, + "settings_tier": "advanced" + }, + { + "key": "HybridExpBias", + "label": "E2E Authority Bias", + "description": "Biases the continuous hybrid between Chill and E2E. Negative favors Chill throttle and braking, positive favors E2E vision behavior. Range -1.0 to 1.0, default 0.0.", + "picker_description": "Biases authority between Chill and E2E vision.", + "data_type": "float", + "ui_type": "numeric", + "min": -1.0, + "max": 1.0, + "step": 0.05, + "precision": 2, + "parent_key": "HybridExperimental", + "settings_tier": "advanced" + }, + { + "key": "HybridVisionBrakeSensitivity", + "label": "Vision Brake Sensitivity", + "description": "Scales how strongly the model's predicted stop horizon contributes to early braking. Below 1.0 reduces unprompted slowing; above 1.0 starts red-light and queue braking earlier. Range 0.0 to 2.0, default 1.0.", + "picker_description": "Scales early vision braking authority.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 2.0, + "step": 0.05, + "precision": 2, + "parent_key": "HybridExperimental", + "settings_tier": "advanced" + }, { "key": "SLCAbbreviatedSources", "label": "Show Abbreviated Icon Sources", diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index ab9680b53..aab25b893 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -111,6 +111,7 @@ SAFE_MODE_MANAGED_KEYS = ( "ReduceLateralAccelerationSnow", "ConditionalExperimental", "ConditionalChill", + "HybridExperimental", "CECurves", "CECurvesLead", "CELead", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index eea603ee6..b70dd5ee8 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -376,6 +376,19 @@ def speed_limit_controller_available(openpilot_longitudinal: bool, redneck_cruis return openpilot_longitudinal or redneck_cruise +def get_longitudinal_modes(openpilot_longitudinal: bool, cem: bool, ccm: bool, hybrid: bool) -> tuple[bool, bool, bool]: + """Resolve the mutually exclusive longitudinal control modes (CEM/CCM/Hybrid).""" + conditional_experimental_mode = bool(openpilot_longitudinal) and bool(cem) + conditional_chill_mode = bool(openpilot_longitudinal) and not conditional_experimental_mode and bool(ccm) + hybrid_experimental_mode = ( + bool(openpilot_longitudinal) and + not conditional_experimental_mode and + not conditional_chill_mode and + bool(hybrid) + ) + return conditional_experimental_mode, conditional_chill_mode, hybrid_experimental_mode + + def migrate_cancel_button_controls(params: Params | None = None) -> bool: params = params or Params(return_defaults=True) if params.get_bool(CANCEL_BUTTON_MIGRATION_KEY) or not params.get_bool("RemapCancelToDistance"): @@ -834,8 +847,16 @@ class StarPilotVariables: self.migrate_prius_cluster_offset(str(toggle.car_model)) toggle.cluster_offset = self.get_value("ClusterOffset", cast=float, condition=toggle.car_make == "toyota") - toggle.conditional_experimental_mode = toggle.openpilot_longitudinal and self.get_value("ConditionalExperimental") - toggle.conditional_chill_mode = toggle.openpilot_longitudinal and not toggle.conditional_experimental_mode and self.get_value("ConditionalChill") + toggle.conditional_experimental_mode, toggle.conditional_chill_mode, toggle.hybrid_experimental_mode = get_longitudinal_modes( + toggle.openpilot_longitudinal, + self.get_value("ConditionalExperimental"), + self.get_value("ConditionalChill"), + self.get_value("HybridExperimental"), + ) + toggle.hybrid_exp_bias = self.get_value( + "HybridExpBias", cast=float, condition=toggle.hybrid_experimental_mode, default=0.0, min=-1.0, max=1.0) + toggle.hybrid_vision_brake_sensitivity = self.get_value( + "HybridVisionBrakeSensitivity", cast=float, condition=toggle.hybrid_experimental_mode, default=1.0, min=0.0, max=2.0) toggle.conditional_curves = self.get_value("CECurves", condition=toggle.conditional_experimental_mode) toggle.conditional_curves_lead = self.get_value("CECurvesLead", condition=toggle.conditional_curves) toggle.conditional_lead = self.get_value("CELead", condition=toggle.conditional_experimental_mode) diff --git a/starpilot/common/tests/test_starpilot_variables.py b/starpilot/common/tests/test_starpilot_variables.py index 09db9240d..0c7a78655 100644 --- a/starpilot/common/tests/test_starpilot_variables.py +++ b/starpilot/common/tests/test_starpilot_variables.py @@ -258,6 +258,18 @@ def test_missing_bounded_value_uses_explicit_default(): assert value == 1.0 +def test_get_longitudinal_modes_is_mutually_exclusive(): + # Hybrid wins when selected and CEM/CCM params are off. + assert spv.get_longitudinal_modes(True, False, False, True) == (False, False, True) + # CEM takes priority over the other modes when enabled. + assert spv.get_longitudinal_modes(True, True, True, True) == (True, False, False) + # CCM engages only when CEM is off. + assert spv.get_longitudinal_modes(True, False, True, False) == (False, True, False) + assert spv.get_longitudinal_modes(True, False, True, True) == (False, True, False) + # Nothing engages without openpilot longitudinal. + assert spv.get_longitudinal_modes(False, True, True, True) == (False, False, False) + + def test_disabled_conditional_experimental_toggles_are_off(monkeypatch, tmp_path): params_cls = spv.Params diff --git a/starpilot/controls/lib/hybrid_experimental_mode.py b/starpilot/controls/lib/hybrid_experimental_mode.py new file mode 100644 index 000000000..ca915ff4f --- /dev/null +++ b/starpilot/controls/lib/hybrid_experimental_mode.py @@ -0,0 +1,153 @@ +#!/usr/bin/env python3 +import math +from openpilot.common.realtime import DT_MDL + + +def lerp(a: float, b: float, t: float) -> float: + """Linear interpolation / blend between a and b by weight t (0.0 to 1.0).""" + return (1.0 - t) * a + t * b + + +def clamp(val: float, low: float, high: float) -> float: + return max(low, min(high, val)) + + +class HybridExperimentalMode: + """ + 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. + """ + + def __init__(self): + self.DT = DT_MDL + self.w_vision = 0.0 + self.prev_a_target = 0.0 + self.last_exp_dominant = False + self.diag = {} + self.record_diag = False + + # 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 math.isfinite(a_ego) else 0.0 + self.w_vision = 0.0 + self.last_exp_dominant = False + self.diag = {} + + def set_tuning(self, exp_bias: float, vision_brake_sensitivity: float): + self.HYBRID_EXP_BIAS = clamp(exp_bias, -1.0, 1.0) + self.VISION_BRAKE_SENSITIVITY = clamp(vision_brake_sensitivity, 0.0, 2.0) + + def update(self, v_ego, v_cruise, lead_one, model_v2, a_chill, a_exp, + should_stop_exp=False, should_stop_chill=False, gas_pressed=False): + + # 0. Sanitize inputs + if not math.isfinite(a_chill): + a_chill = self.prev_a_target + if not math.isfinite(a_exp): + a_exp = a_chill + + # 1. Trajectory Analysis + traj_v = getattr(getattr(model_v2, "velocity", None), "x", None) + has_full_trajectory = bool(traj_v and len(traj_v) >= 24 and all(math.isfinite(v) for v in traj_v)) + + if has_full_trajectory: + v_horizon = float(traj_v[-1]) + v_short = float(traj_v[23]) # ~4.0s lookahead + v_min = float(min(traj_v)) + else: + v_horizon = v_short = v_min = float(v_ego) + + lead_status = bool(getattr(lead_one, "status", False)) + lead_v = float(getattr(lead_one, "vLead", 0.0)) + lead_d = float(getattr(lead_one, "dRel", 150.0)) + + # 2. Vision Departure / Driver Override Detection (Priority Check) + at_standstill = v_ego < 0.8 + lead_departing = at_standstill and lead_status and (lead_v > 0.6) and ((lead_v - v_ego) > 0.4) + vision_departing = at_standstill and (v_horizon > 1.5) and (a_exp > 0.15) + driver_override = bool(gas_pressed) + is_departing = lead_departing or vision_departing or driver_override + + # 3. Vision Stop & Decel Detection + horizon_stopping = not is_departing and ( + (v_horizon < 0.8) or (has_full_trajectory and v_short < 1.5) or should_stop_exp + ) + + speed_drop_ratio = max(0.0, (v_ego - v_min) / max(v_ego, 2.0)) + model_decel_strength = max(0.0, -a_exp * 0.5) + + if is_departing: + raw_vision_metric = 0.0 + w_target = 0.0 + self.w_vision = 0.0 + else: + if horizon_stopping: + raw_vision_metric = 1.0 + elif lead_status and lead_d < 40.0: + raw_vision_metric = max(speed_drop_ratio, model_decel_strength) + else: + raw_vision_metric = max(speed_drop_ratio, model_decel_strength) * 0.5 + + w_target = clamp(raw_vision_metric * self.VISION_BRAKE_SENSITIVITY, 0.0, 1.0) + + # 4. Dynamic Filter (Fast Attack, Smooth Decay) + if w_target > self.w_vision: + self.w_vision = min(1.0, self.w_vision + 0.15) + else: + self.w_vision = max(0.0, self.w_vision - 0.04) + + # 5. Dual-Regime Fusion + a_brake_fused = min(a_chill, a_exp) + a_throttle_fused = a_chill + max(0.0, a_exp - a_chill) * self.HYBRID_EXP_BIAS + + # Output Arbitration + is_stopping_event = (self.w_vision > 0.3) or horizon_stopping + self.last_exp_dominant = bool(is_stopping_event and not is_departing) + + if self.last_exp_dominant: + a_out = a_brake_fused + else: + a_out = lerp(a_throttle_fused, a_brake_fused, self.w_vision) + + # 6. Authoritative Standstill Handshake + standstill_intent = not is_departing and (v_ego < 0.5 and (v_horizon < 0.4 or should_stop_exp)) + should_stop_fused = bool(should_stop_chill or (not is_departing and (should_stop_exp or standstill_intent))) + + if self.record_diag: + self.diag = { + "v_ego": v_ego, "v_cruise": v_cruise, + "a_chill": a_chill, "a_exp": a_exp, + "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, + "has_full_trajectory": 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_target": w_target, + "w_vision": self.w_vision, + "lead_departing": lead_departing, + "vision_departing": vision_departing, + "driver_override": driver_override, + "is_departing": is_departing, + "horizon_stopping": horizon_stopping, + "a_brake_fused": a_brake_fused, + "a_throttle_fused": a_throttle_fused, + "is_stopping_event": is_stopping_event, + "exp_dominant": self.last_exp_dominant, + "standstill_intent": standstill_intent, + "should_stop_fused": should_stop_fused, + "a_out": a_out, + "regime": "brake" if self.last_exp_dominant else "throttle", + "standstill": standstill_intent, + } + + self.prev_a_target = a_out + return a_out, should_stop_fused \ No newline at end of file diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index d647f24f9..5a9ca9c50 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -120,6 +120,9 @@ class StarPilotCard: if getattr(starpilot_toggles, "safe_mode", False): return + if getattr(starpilot_toggles, "hybrid_experimental_mode", False): + return + if starpilot_toggles.conditional_experimental_mode: current_status = self.params_memory.get_int("CEStatus", default=CEStatus["OFF"]) override_value = next_manual_ce_status(current_status, sm["selfdriveState"].experimentalMode) diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index 4cc96b91f..eaf2cc60d 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -223,7 +223,11 @@ class StarPilotPlanner: self.starpilot_following.update(controls_enabled, v_ego, sm, starpilot_toggles) conditional_tracking_active = controls_enabled or sm["starpilotCarState"].alwaysOnLateralEnabled - if conditional_tracking_active and bool(getattr(starpilot_toggles, "conditional_experimental_mode", False)): + if conditional_tracking_active and bool(getattr(starpilot_toggles, "hybrid_experimental_mode", False)): + self.starpilot_cem.update(v_ego, sm, starpilot_toggles, v_cruise) + self.starpilot_ccm.experimental_mode = False + self.starpilot_cem.experimental_mode = False + elif conditional_tracking_active and bool(getattr(starpilot_toggles, "conditional_experimental_mode", False)): # Keep CEM's filters warm in AOL so engagement can inherit the current scene. self.starpilot_cem.update(v_ego, sm, starpilot_toggles, v_cruise) self.starpilot_ccm.experimental_mode = True diff --git a/starpilot/controls/tests/test_hybrid_experimental_mode.py b/starpilot/controls/tests/test_hybrid_experimental_mode.py new file mode 100644 index 000000000..8a90d6187 --- /dev/null +++ b/starpilot/controls/tests/test_hybrid_experimental_mode.py @@ -0,0 +1,336 @@ +#!/usr/bin/env python3 +import numpy as np +import pytest + +from openpilot.starpilot.controls.lib.hybrid_experimental_mode import ( + HybridExperimentalMode, + lerp, +) + + +class FakeLead: + def __init__(self, status=False, d_rel=150.0, v_lead=0.0): + self.status = status + self.dRel = float(d_rel) + self.vLead = float(v_lead) + + +class FakeModel: + def __init__(self, velocity=None, position=None): + self.velocity = type("Velocity", (), {"x": list(velocity)})() if velocity is not None else None + self.position = type("Position", (), {"x": list(position)})() if position is not None else None + + +def make_controller(prev=0.0): + controller = HybridExperimentalMode() + controller.reset(prev) + return controller + + +def run(controller, *, v_ego=20.0, v_cruise=30.0, lead=None, model=None, a_chill=0.5, a_exp=0.8, frames=80): + lead = lead if lead is not None else FakeLead() + 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, should_stop = controller.update(v_ego, v_cruise, lead, model, a_chill, a_exp) + return result, should_stop + + +def test_update_returns_accel_and_should_stop_tuple(): + controller = make_controller() + 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_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=0.5, a_exp=0.8) + assert a == pytest.approx(0.5, abs=1e-3) + + +def test_throttle_uses_exp_bias(): + controller = make_controller() + 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, 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(): + controller = make_controller(prev=0.0) + traj_v = np.zeros(33) + traj_x = np.zeros(33) + model = FakeModel(velocity=traj_v, position=traj_x) + + 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) + traj_v = np.linspace(25.0, 0.0, 33) + 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) + assert a <= -2.0, f"Stopping deceleration should be fully honored without dilution, got {a}" + + +def test_red_light_low_speed_roll_prevent_dilution(): + controller = make_controller(prev=-0.5) + traj_v = np.linspace(0.8, 0.0, 33) + 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"Vision stop clamp must enforce stopping bite at low speeds, got {a}" + + +def test_vision_stop_does_not_dilute_brake_with_cruise_throttle(): + controller = make_controller(prev=0.0) + 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_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) + 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_inf_a_exp_does_not_propagate(): + 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_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() + controller.set_tuning(5.0, 9.0) + assert controller.HYBRID_EXP_BIAS == 1.0 + assert controller.VISION_BRAKE_SENSITIVITY == 2.0 + controller.set_tuning(-5.0, -1.0) + assert controller.HYBRID_EXP_BIAS == -1.0 + assert controller.VISION_BRAKE_SENSITIVITY == 0.0 + + +def test_slower_lead_closing_blends_vision_decel(): + controller = make_controller(prev=0.0) + 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_cut_in_emergency_braking_preserves_chill_floor(): + 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 preserve the Chill brake floor, got {a}" + + +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) + 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 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(0.3, 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=[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(): + 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))) + run(controller, v_ego=20.0, v_cruise=25.0, lead=FakeLead(status=False), + model=model, a_chill=0.0, a_exp=-3.0, frames=60) + assert controller.last_exp_dominant + + +def test_last_exp_dominant_false_when_chill_brakes_for_lead(): + controller = make_controller(prev=0.0) + 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))) + 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 not controller.last_exp_dominant + + +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 diff --git a/starpilot/system/the_galaxy/assets/components/tools/tmux.js b/starpilot/system/the_galaxy/assets/components/tools/tmux.js index 45863d679..318667953 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/tmux.js +++ b/starpilot/system/the_galaxy/assets/components/tools/tmux.js @@ -193,6 +193,7 @@ export function TmuxLog() { log: '', selectorAction: null, transport: "connecting", + manualStop: false, }); const lifecycleVersion = ++tmuxLifecycleVersion; let lifecycleTimer = null; @@ -289,6 +290,12 @@ export function TmuxLog() { return; } + if (state.manualStop) { + stopLiveTransport(); + state.transport = "stopped"; + return; + } + if (!isTmuxRouteActive()) { stopLiveTransport(); state.transport = "inactive"; @@ -328,11 +335,55 @@ export function TmuxLog() { return "Tmux Live Log"; } - function togglePause() { - state.paused = !state.paused; - if (!state.paused) { - state.log = state.latest; + function clearLog() { + state.manualStop = false; + state.paused = false; + fetch("/api/tmux_log/clear", { method: "POST" }) + .then(res => { + if (!res.ok) return res.text().then(msg => { throw new Error(msg); }); + state.log = ""; + state.latest = ""; + showSnackbar("Log cleared — ready for a fresh run!", "success"); + }) + .catch(err => showSnackbar(`Clear failed: ${err.message}`, "error")); + } + + function stopLog() { + // Freeze the displayed data and tear down the live transport so nothing more + // can overwrite the captured [HEM] log before the user copies it. + state.manualStop = true; + state.paused = true; + stopLiveTransport(); + state.transport = "stopped"; + showSnackbar("Log stopped — data frozen for copying.", "success"); + } + + function resumeLog() { + state.manualStop = false; + state.paused = false; + state.transport = "connecting"; + updateTransportForLifecycle(); + } + + function copyLog() { + const text = state.log || ""; + if (!text) { + showSnackbar("Nothing to copy yet.", "error"); + return; } + const done = ok => showSnackbar(ok ? `Copied ${text.length} chars to clipboard!` : "Copy failed.", ok ? "success" : "error"); + if (navigator.clipboard && navigator.clipboard.writeText) { + navigator.clipboard.writeText(text).then(() => done(true)).catch(() => done(false)); + return; + } + const ta = document.createElement("textarea"); + ta.value = text; + ta.style.position = "fixed"; + ta.style.left = "-9999px"; + document.body.appendChild(ta); + ta.select(); + try { done(document.execCommand("copy")); } catch (e) { done(false); } + document.body.removeChild(ta); } function captureLog() { @@ -384,11 +435,12 @@ export function TmuxLog() {
- + + + + - -
diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index 276acc71d..2584e1924 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -1208,6 +1208,37 @@ NAVIGATION_PERSISTED_LOCATION_MAX_AGE_SECONDS = 24 * 60 * 60 TMUX_LOGS_PATH = Path("/data/tmux_logs") + +def _filter_hem_log_lines(output): + """Reduce a tmux pane capture to only lines emitted by the [HEM] logger. + + The Galaxy terminal is used to debug Hybrid Experimental Mode stopping, so only + [HEM] lines (which now carry a millisecond timestamp and full per-frame diag) + are surfaced; all other process output is dropped. + """ + if not output: + return "" + lines = [line for line in output.splitlines() if "[HEM]" in line] + return "\n".join(lines) + ("\n" if lines else "") + + +# Generous but bounded scrollback/capture depth for the [HEM] diagnostics. This +# is far beyond any stop-signal reproduction run (~20k lines ~= 16 min of HEM +# logging at 20 Hz) while keeping capture cost, tunnel bandwidth, and browser +# render work bounded so it cannot grow without limit or burden the device. +TMUX_HISTORY_LIMIT = 20000 +TMUX_CAPTURE_LINES = 20000 + + +def _ensure_tmux_session(): + """Create the comma session if needed, size it, and raise its scrollback limit + so the captured [HEM] window can hold the full run without overflow.""" + if subprocess.run(["tmux", "has-session", "-t", "comma"], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL).returncode != 0: + run_cmd(["tmux", "new-session", "-d", "-s", "comma", "-x", "240", "-y", "70", "bash"], "Started tmux session", "Failed to start tmux session") + else: + run_cmd(["tmux", "resize-window", "-t", "comma:0", "-x", "240", "-y", "70"], "Resized tmux window", "Failed to resize tmux window") + run_cmd(["tmux", "set-option", "-t", "comma:0", "history-limit", str(TMUX_HISTORY_LIMIT)], "Set tmux scrollback limit.", "Failed to set tmux scrollback limit.") + MODEL_DOWNLOAD_PARAM = "ModelToDownload" MODEL_DOWNLOAD_ALL_PARAM = "DownloadAllModels" ALLOW_GPU_DOWNLOAD_WITHOUT_GPU_PARAM = "AllowGpuModelDownloadWithoutGpu" @@ -1524,6 +1555,7 @@ _TROUBLESHOOT_PERSONALITY_KEYS = [ _TROUBLESHOOT_CEM_KEYS = [ "ConditionalExperimental", + "HybridExperimental", "CESpeed", "CESpeedLead", "CECurves", @@ -5202,15 +5234,16 @@ def setup(app): "updated": updated, }), 200 - if key in {"ConditionalExperimental", "ConditionalChill"}: + if key in {"ConditionalExperimental", "ConditionalChill", "HybridExperimental"}: enabled = str_val.strip() in ("1", "true", "True") params.put_bool(key, enabled) updated = {key: enabled} if enabled: - other_key = "ConditionalChill" if key == "ConditionalExperimental" else "ConditionalExperimental" - params.put_bool(other_key, False) - updated[other_key] = False + for other_key in ("ConditionalExperimental", "ConditionalChill", "HybridExperimental"): + if other_key != key: + params.put_bool(other_key, False) + updated[other_key] = False update_starpilot_toggles() return jsonify({ @@ -8336,16 +8369,15 @@ def setup(app): @app.route("/api/tmux_log/live", methods=["GET"]) def stream_tmux_log(): - if subprocess.run(["tmux", "has-session", "-t", "comma"], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL).returncode != 0: - run_cmd(["tmux", "new-session", "-d", "-s", "comma", "-x", "240", "-y", "70", "bash"], "Started tmux session", "Failed to start tmux session") - else: - run_cmd(["tmux", "resize-window", "-t", "comma:0", "-x", "240", "-y", "70"], "Resized tmux window", "Failed to resize tmux window") + _ensure_tmux_session() def generate(): last_output = "" last_keepalive = 0.0 while True: - output = subprocess.check_output(["tmux", "capture-pane", "-t", "comma:0", "-p", "-S", "-1000"], text=True) + output = _filter_hem_log_lines( + subprocess.check_output(["tmux", "capture-pane", "-t", "comma:0", "-p", "-S", f"-{TMUX_CAPTURE_LINES}"], text=True) + ) if output != last_output: yield "data: " + "\n".join(reversed(output.splitlines())).replace("\n", "\ndata: ") + "\n\n" @@ -8365,19 +8397,33 @@ def setup(app): @app.route("/api/tmux_log/snapshot", methods=["GET"]) def snapshot_tmux_log(): try: - output = subprocess.check_output(["tmux", "capture-pane", "-t", "comma:0", "-p", "-S", "-1000"], text=True) + output = subprocess.check_output(["tmux", "capture-pane", "-t", "comma:0", "-p", "-S", f"-{TMUX_CAPTURE_LINES}"], text=True) except subprocess.CalledProcessError: - run_cmd(["tmux", "new-session", "-d", "-s", "comma", "-x", "240", "-y", "70", "bash"], "Started tmux session", "Failed to start tmux session") - output = subprocess.check_output(["tmux", "capture-pane", "-t", "comma:0", "-p", "-S", "-1000"], text=True) + _ensure_tmux_session() + output = subprocess.check_output(["tmux", "capture-pane", "-t", "comma:0", "-p", "-S", f"-{TMUX_CAPTURE_LINES}"], text=True) except Exception as e: return jsonify({"error": str(e)}), 500 + output = _filter_hem_log_lines(output) + try: live_text = "\n".join(reversed(output.splitlines())) return jsonify({"data": live_text}), 200 except Exception as e: return jsonify({"error": str(e)}), 500 + @app.route("/api/tmux_log/clear", methods=["POST"]) + def clear_tmux_log(): + """Start a clean slate for a fresh HEM test run: wipe tmux scrollback and the + visible pane so only newly-emitted [HEM] lines are captured.""" + try: + _ensure_tmux_session() + subprocess.run(["tmux", "clear-history", "-t", "comma:0"], check=False) + subprocess.run(["tmux", "send-keys", "-t", "comma:0", "clear", "Enter"], check=False) + return jsonify({"message": "Log cleared!"}), 200 + except Exception as e: + return jsonify({"error": str(e)}), 500 + @app.route("/api/tmux_log/rename//", methods=["PUT"]) def rename_tmux_log_path_params(old, new): old_path = TMUX_LOGS_PATH / old diff --git a/tools/replay/export_hem_telemetry.py b/tools/replay/export_hem_telemetry.py new file mode 100644 index 000000000..9bf704913 --- /dev/null +++ b/tools/replay/export_hem_telemetry.py @@ -0,0 +1,234 @@ +#!/usr/bin/env python3 +"""HEM Telemetry Exporter. + +Extracts real telemetry from target segments and saves them to a portable JSON file, +capturing the raw model action intents, radar lead states, and planner controls. +""" +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_lead(lead_msg): + if lead_msg is None: + return {"status": False, "dRel": 150.0, "vLead": 0.0, "aLeadK": 0.0, "yRel": 0.0, "radar": False} + return { + "status": bool(getattr(lead_msg, "status", False)), + "dRel": float(getattr(lead_msg, "dRel", 150.0)), + "vLead": float(getattr(lead_msg, "vLead", 0.0)), + "aLeadK": float(getattr(lead_msg, "aLeadK", 0.0)), + "yRel": float(getattr(lead_msg, "yRel", 0.0)), + "radar": bool(getattr(lead_msg, "radar", False)), + "modelProb": float(getattr(lead_msg, "modelProb", 0.0)), + } + + +def serialize_model(model_msg): + """Extracts velocity, position, acceleration lists and raw action intents from modelV2.""" + v_list = list(getattr(getattr(model_msg, "velocity", None), "x", [])) + x_list = list(getattr(getattr(model_msg, "position", None), "x", [])) + a_list = list(getattr(getattr(model_msg, "acceleration", None), "x", [])) + + action = getattr(model_msg, "action", None) + a_exp_raw = float(getattr(action, "desiredAcceleration", 0.0)) if action is not None else 0.0 + should_stop_exp_raw = bool(getattr(action, "shouldStop", False)) if action is not None else False + + return { + "velocity": v_list, + "position": x_list, + "acceleration": a_list, + "desiredAcceleration": a_exp_raw, + "shouldStop": should_stop_exp_raw, + } + + +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 = [] + radar_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 == "radarState": + radar_state_msgs.append((t, msg.radarState)) + 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]) + radar_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) + rs = next((m for t, m in reversed(radar_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 + + 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 + + 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_brake = bool(cs.brakePressed) and not op_braking + + # Lead tracks + lead_one_data = serialize_lead(getattr(rs, "leadOne", None)) + lead_two_data = serialize_lead(getattr(rs, "leadTwo", None)) + mpc_source = str(getattr(lplan, "longitudinalPlanSource", "lead0")) + + # Raw model outputs + action = getattr(model_msg, "action", None) + a_exp_raw = float(getattr(action, "desiredAcceleration", 0.0)) if action is not None else 0.0 + should_stop_exp_raw = bool(getattr(action, "shouldStop", False)) if action is not None else False + + frames.append({ + "t": t_model, + "v_ego": float(cs.vEgo), + "a_ego": float(getattr(cs, "aEgo", 0.0)), + "standstill": bool(getattr(cs, "standstill", False)), + "v_cruise": float(getattr(splan, "vCruise", getattr(cs, "vCruise", 0.0))), + # a_chill is the real planner-side output target from longitudinalPlan + "a_chill": float(getattr(lplan, "aTarget", 0.0)), + "should_stop_chill": bool(getattr(lplan, "shouldStop", False)), + # Raw E2E vision neural net output (not modified by MPC) + "a_exp_raw": a_exp_raw, + "should_stop_exp_raw": should_stop_exp_raw, + "accel_jerk": float(getattr(splan, "accelerationJerk", 1.0)), + "min_accel": float(getattr(splan, "minAcceleration", -3.5)), + "max_accel": float(getattr(splan, "maxAcceleration", 1.5)), + "mpc_source": mpc_source, + "lead_one": lead_one_data, + "lead_two": lead_two_data, + "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), + "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 new file mode 100644 index 000000000..44067f075 --- /dev/null +++ b/tools/replay/hem_forensic.py @@ -0,0 +1,783 @@ +#!/usr/bin/env python3 +"""HEM forensic analyzer: figure out exactly what HybridExperimentalMode decided +and why, on a logged route, down to the per-frame input params and every internal +gate. Designed to answer "why did HEM roll through that stop sign / red light?" + +It replays a route segment(s) through the *real* LongitudinalPlanner (Chill / ACC) +and the *real* HybridExperimentalMode (using the logged modelV2 action for Exp), +and records HybridExperimentalMode's full internal decision state each frame +(vision weight, stop detection, throttle/brake fusion, standstill anchor, safety +barrier, slew filter). It then: + + * auto-detects "stop roll-through" incidents (vehicle creeping/coasting through + a detected stop with no braking authority), + * prints a per-frame forensic log of the decision chain around the incident, + * prints a root-cause narrative naming the exact gate that failed, and + * writes a graph of the Exp input, Chill input, fused output, authority, vision + weight and regime. + +Usage: + ./dev python tools/replay/hem_forensic.py / [--segments 0,1] + [--start 30] [--end 120] [--data_dir /path/to/routes] [--out hem_forensic.png] + [--show] [--csv out.csv] [--window 12] + [--set hybrid_exp_bias=0.2 --set hybrid_vision_brake_sensitivity=1.2] + +Examples: + ./dev python tools/replay/hem_forensic.py afb7ef2ed593d651/000000af--414a758637 --segments 0,1 + ./dev python tools/replay/hem_forensic.py afb7ef2ed593d651/000000af--414a758637 --segments 0,1 --window 20 --set hybrid_exp_bias=0.2 +""" +from __future__ import annotations + +import argparse +import bisect +import sys +import time +from pathlib import Path +from types import SimpleNamespace +from typing import Any + +import numpy as np + +ROOT_DIR = Path(__file__).resolve().parents[2] +if str(ROOT_DIR) not in sys.path: + sys.path.insert(0, str(ROOT_DIR)) + +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET +from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner +from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode +from openpilot.tools.replay.mode_sim import ( + FakeParams, + TOGGLE_DEFAULTS, + as_lead, + load_buffers, + parse_toggle_overrides, + resolve_segment_identifier, +) + +# Services we need to reconstruct the Chill (ACC) planner + Exp input + HEM. +SERVICES = { + "carState", "radarState", "starpilotRadarState", "modelV2", + "longitudinalPlan", "selfdriveState", "starpilotPlan", "starpilotCarState", + "controlsState", "carParams", "liveParameters", "carControl", +} + +# Fields in HybridExperimentalMode.diag that are part of the decision chain. +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 + "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"] + + +def merge_buffers(buffers_list): + """Merge per-segment {service: [(t, msg), ...]} into one dict sorted by time.""" + merged = {s: [] for s in SERVICES} + for bufs in buffers_list: + for s in SERVICES: + merged[s].extend(bufs.get(s, [])) + for s in SERVICES: + merged[s].sort(key=lambda x: x[0]) + return merged + + +def run_forensic(grid, bufs, toggles): + """Replay through the real planner + HEM, recording every HEM internal gate.""" + ts = {s: [t for t, _ in m] for s, m in bufs.items()} + ms = {s: [m for _, m in bufs[s]] for s in bufs} + + def latest(service, t): + times = ts.get(service) + if not times: + return None + idx = bisect.bisect_right(times, t) - 1 + return ms[service][idx] if idx >= 0 else None + + cp_candidate = None + for _, cp in bufs.get("carParams", []): + if cp is not None: + cp_candidate = cp + break + if cp_candidate is None: + cp_candidate = SimpleNamespace( + brand="toyota", carFingerprint="TOYOTA_RAV4", openpilotLongitudinalControl=True, + pcmCruise=False, steerRatio=15.0, wheelbase=2.7, longitudinalActuatorDelay=0.2, flags=0, + ) + + planner_state = SimpleNamespace( + params=FakeParams(), + params_memory=FakeParams(), + starpilot_following=SimpleNamespace(following_lead=False, slower_lead=False), + starpilot_vcruise=SimpleNamespace( + slc=SimpleNamespace(experimental_mode=False), + stop_sign_confirmed=False, + forcing_stop=False, + ), + ) + + hybrid = HybridExperimentalMode() + hybrid.record_diag = True # capture full per-frame decision state for the trace + hybrid.set_tuning(toggles.hybrid_exp_bias, toggles.hybrid_vision_brake_sensitivity) + chill_long_planner = LongitudinalPlanner(cp_candidate, dt=DT_MDL) + + n = len(grid) + out = {k: np.zeros(n) for k in [ + "v_ego", "v_cruise", "model_v0", "a_chill", "a_exp", "hem_a", "hem_authority", + "lead_d_rel", "lead_v_lead", "logged_aTarget", "a_ego", + ]} + out["lead_status"] = np.zeros(n, dtype=bool) + out["brake_pressed"] = np.zeros(n, dtype=bool) + out["gas_pressed"] = np.zeros(n, dtype=bool) + for k in DIAG_KEYS: + 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] + time.monotonic = lambda: fake_clock[0] + + last_model_v2 = None + for _, m in bufs.get("modelV2", []): + if m is not None: + last_model_v2 = m + break + + try: + for i, t in enumerate(grid): + fake_clock[0] = float(i) * DT_MDL + + car_raw = latest("carState", t) + v_ego_raw = max(float(getattr(car_raw, "vEgo", 0.0)), 0.0) + car = SimpleNamespace( + vEgo=v_ego_raw, + vEgoCluster=max(float(getattr(car_raw, "vEgoCluster", v_ego_raw)), 0.0), + vCruise=float(getattr(car_raw, "vCruise", 0.0)), + standstill=bool(getattr(car_raw, "standstill", False)), + leftBlinker=bool(getattr(car_raw, "leftBlinker", False)), + rightBlinker=bool(getattr(car_raw, "rightBlinker", False)), + gasPressed=bool(getattr(car_raw, "gasPressed", False)), + brakePressed=bool(getattr(car_raw, "brakePressed", False)), + steeringAngleDeg=float(getattr(car_raw, "steeringAngleDeg", 0.0)), + aEgo=float(getattr(car_raw, "aEgo", 0.0)), + leftBlindspot=bool(getattr(car_raw, "leftBlindspot", False)), + rightBlindspot=bool(getattr(car_raw, "rightBlindspot", False)), + ) + v_ego = car.vEgo + + radar = latest("radarState", t) + lead = as_lead(getattr(radar, "leadOne", None) if radar is not None else None) + lead2 = as_lead(getattr(radar, "leadTwo", None) if radar is not None else None) + sradar = latest("starpilotRadarState", t) + lead_left = as_lead(getattr(sradar, "leadLeft", None) if sradar is not None else None) + lead_right = as_lead(getattr(sradar, "leadRight", None) if sradar is not None else None) + + lplan = latest("longitudinalPlan", t) + sds = latest("selfdriveState", t) + splan = latest("starpilotPlan", t) + scs = latest("starpilotCarState", t) + model_v2 = latest("modelV2", t) + if model_v2 is not None: + last_model_v2 = model_v2 + else: + model_v2 = last_model_v2 # carry last vision frame across a replay gap + live_params = latest("liveParameters", t) + car_control = latest("carControl", t) + controls_state = latest("controlsState", t) + car_params = latest("carParams", t) or cp_candidate + + v_cruise = float(getattr(splan, "vCruise", 0.0) or 0.0) + if not (v_cruise > 0): + v_cruise_kph = float(getattr(car_raw, "vCruise", 0.0) or 0.0) + if 0 < v_cruise_kph < V_CRUISE_UNSET: + v_cruise = min(v_cruise_kph, V_CRUISE_MAX) * CV.KPH_TO_MS + else: + v_cruise = v_ego + + tracking_lead = bool(getattr(splan, "trackingLead", False)) + if not tracking_lead: + tracking_lead = bool(getattr(lplan, "hasLead", False)) + + t_follow = float(getattr(splan, "tFollow", 1.45)) + following_lead = tracking_lead and lead.dRel < (t_follow * 2) * v_ego + planner_state.lead_one = lead + planner_state.tracking_lead = tracking_lead + + sm_dict = { + "carState": car, + "radarState": SimpleNamespace(leadOne=lead, leadTwo=lead2), + "starpilotRadarState": SimpleNamespace(leadLeft=lead_left, leadRight=lead_right), + "starpilotCarState": SimpleNamespace( + trafficModeEnabled=bool(getattr(scs, "trafficModeEnabled", False)), + alwaysOnLateralEnabled=bool(getattr(scs, "alwaysOnLateralEnabled", False)), + dashboardStopSign=int(getattr(scs, "dashboardStopSign", 0)), + accelPressed=bool(getattr(scs, "accelPressed", False)), + ), + "selfdriveState": SimpleNamespace( + enabled=bool(getattr(sds, "enabled", False)), + experimentalMode=False, # force ACC/Chill evaluation in MPC + personality=0, + ), + "longitudinalPlan": SimpleNamespace( + hasLead=bool(getattr(lplan, "hasLead", False)), + allowThrottle=bool(getattr(lplan, "allowThrottle", True)), + shouldStop=bool(getattr(lplan, "shouldStop", False)), + aTarget=float(getattr(lplan, "aTarget", 0.0)), + ), + "starpilotPlan": SimpleNamespace( + vCruise=v_cruise, + tFollow=t_follow, + trackingLead=tracking_lead, + redLight=bool(getattr(splan, "redLight", False)), + forcingStop=bool(getattr(splan, "forcingStop", False)), + forcingStopLength=float(getattr(splan, "forcingStopLength", 100.0)), + minAcceleration=float(getattr(splan, "minAcceleration", -3.5)), + maxAcceleration=float(getattr(splan, "maxAcceleration", 1.5)), + accelerationJerk=float(getattr(splan, "accelerationJerk", 1.0)), + dangerJerk=float(getattr(splan, "dangerJerk", 1.0)), + speedJerk=float(getattr(splan, "speedJerk", 1.0)), + dangerFactor=float(getattr(splan, "dangerFactor", 1.0)), + disableThrottle=bool(getattr(splan, "disableThrottle", False)), + ), + "controlsState": SimpleNamespace( + longControlState=LongCtrlState.pid, + forceDecel=bool(getattr(controls_state, "forceDecel", False)), + curvature=0.0, + ), + "liveParameters": SimpleNamespace( + angleOffsetDeg=float(getattr(live_params, "angleOffsetDeg", 0.0)) if live_params else 0.0, + ), + "carControl": SimpleNamespace( + orientationNED=getattr(car_control, "orientationNED", [0.0, 0.0, 0.0]) if car_control else [0.0, 0.0, 0.0], + ), + "modelV2": model_v2, + "carParams": car_params, + } + + # Chill (ACC) acceleration target via the real planner. + chill_long_planner.update(sm_dict, toggles) + a_chill = float(chill_long_planner.output_a_target) + + # Exp (vision) acceleration target from the logged model action. + a_exp = 0.0 + if model_v2 is not None: + try: + action_obj = getattr(model_v2, "action", None) + if action_obj is not None and hasattr(action_obj, "desiredAcceleration"): + a_exp = float(action_obj.desiredAcceleration) + else: + accel_x = getattr(model_v2, "acceleration", None) + if accel_x is not None and len(accel_x.x): + a_exp = float(accel_x.x[0]) + except Exception: + a_exp = 0.0 + + 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: + try: + vel_x = getattr(model_v2, "velocity", None) + if vel_x is not None and len(vel_x.x): + model_v0 = float(vel_x.x[0]) + except Exception: + model_v0 = v_ego + + out["v_ego"][i] = v_ego + out["v_cruise"][i] = v_cruise + out["model_v0"][i] = model_v0 + out["a_chill"][i] = a_chill + out["a_exp"][i] = a_exp + out["hem_a"][i] = a_hem + 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 + out["logged_aTarget"][i] = float(getattr(lplan, "aTarget", 0.0)) + out["brake_pressed"][i] = car.brakePressed + out["gas_pressed"][i] = car.gasPressed + out["a_ego"][i] = car.aEgo + + for k in DIAG_KEYS: + val = hybrid.diag.get(k) + if isinstance(val, bool): + out["hem_" + k][i] = bool(val) + else: + try: + out["hem_" + k][i] = float(val) + except Exception: + 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 + + return out + + +def detect_incidents(out, t): + """Find frames where a detected stop was rolled/crept through for lack of braking. + + A roll-through frame: the car is actually moving (v_ego > 0.5 m/s), the vision + side has flagged a stop (w_vision > 0.3), yet the fused output is essentially + coasting because BOTH chill and exp inputs failed to request a stop: + a_chill (ACC) is not braking -> a_chill > -0.3 + exp effective accel is non-negative -> a_exp_effective >= 0 + Under those conditions a_brake_fused = min(a_chill, a_exp) lands at ~0 and HEM + coasts through the stop it claims to see. + """ + idxs = [] + for i in range(len(t)): + v = out["v_ego"][i] + w = out["hem_w_vision"][i] + ac = out["hem_a_chill"][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) + return idxs + + +def hard_brake_clusters(out, gap_sec=1.5): + """Clusters of frames where the DRIVER slammed the brakes (a_ego < -2.0). + + The driver's own hard brake is the ground-truth marker that HEM failed to stop + the car. Each cluster is a discrete human-intervention event.""" + hb = hard_brake_idxs(out) + return cluster_indices(hb, gap_sec=gap_sec) + + +def classify_failure(out, sl, i_brake): + """Classify WHY the driver had to brake, from the frames just before the brake.""" + lead = int(3.0 / DT_MDL) + pre = slice(max(sl.start, i_brake - lead), i_brake) + if pre.stop <= pre.start: + return "UNKNOWN", {} + v = out["v_ego"][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] + # out["hem_regime"] is True for the BRAKE regime. + brake_share = float(np.mean(out["hem_regime"][pre])) + throttle_share = 1.0 - brake_share + + stats = { + "v_before": float(v[-1]), "v_max": float(v.max()), + "exp_eff_mean": float(exp_eff.mean()), "exp_eff_min": float(exp_eff.min()), + "w_vis_max": float(wv.max()), "w_vis_mean": float(wv.mean()), + "hem_min": float(hem.min()), "hem_mean": float(hem.mean()), + "chill_min": float(chill.min()), + "throttle_share": throttle_share, + } + + if stats["hem_min"] > -0.35 and stats["w_vis_max"] < 0.5 and stats["exp_eff_mean"] >= 0.0: + mode = "NO_STOP_REQUEST" # HEM never braked; neither input requested a stop + elif stats["throttle_share"] > 0.6 and stats["exp_eff_mean"] < -0.2: + mode = "THROTTLE_OVERRIDE" # vision wanted to brake but HEM output positive throttle + elif stats["w_vis_max"] > 0.4 and stats["exp_eff_mean"] < -0.2 and stats["hem_min"] > -1.2: + mode = "WEAK_BRAKE" # vision saw the stop but braking authority was too weak + elif stats["w_vis_max"] > 0.3 and stats["exp_eff_mean"] < -0.1: + mode = "LATE_BRAKE" # HEM braked but too late / not enough distance + else: + mode = "UNKNOWN" + return mode, stats + + +def cluster_indices(idxs, gap_sec=2.0): + """Split incident indices into clusters separated by > gap_sec of quiet time.""" + if not idxs: + return [] + gap = int(gap_sec / DT_MDL) + clusters = [[idxs[0]]] + for idx in idxs[1:]: + if idx - clusters[-1][-1] > gap: + clusters.append([idx]) + else: + clusters[-1].append(idx) + return clusters + + +def hard_brake_idxs(out): + """Frames where the DRIVER slammed the brakes (a_ego strongly negative) — a + human-intervention marker proving HEM was about to run the stop.""" + return [i for i in range(len(out["a_ego"])) if out["a_ego"][i] < -2.0] + + +def select_main_cluster(clusters, hb): + """Pick the incident cluster the driver actually intervened on (nearest hard + brake); fall back to the densest cluster when no driver braking occurred.""" + if not clusters: + return [] + if not hb: + return max(clusters, key=len) + best, best_gap = None, float("inf") + for cl in clusters: + gap = min(min(abs(i - j) for j in hb) for i in cl) + if gap < best_gap: + best_gap, best = gap, cl + return best + + +def window_around(idxs, t, out, span_sec=6.0, max_frames=400): + """Select the incident cluster the driver braked on, return (slice, cluster).""" + if not idxs: + return None, [] + clusters = cluster_indices(idxs) + main = select_main_cluster(clusters, hard_brake_idxs(out)) + start = max(0, main[0] - int(span_sec / DT_MDL)) + end = min(len(t) - 1, main[-1] + int(span_sec / DT_MDL)) + if end - start > max_frames: + mid = (start + end) // 2 + start = max(0, mid - max_frames // 2) + end = min(len(t) - 1, start + max_frames) + main = [i for i in main if start <= i <= end] + return slice(start, end + 1), main + + +def print_forensic_log(out, sl, t0): + print("\n" + "-" * 122) + print(" PER-FRAME HEM DECISION CHAIN") + print("-" * 122) + hdr = (f" {'t':>5} {'vEgo':>5} {'aEgo':>6} {'dvrBrk':>6} {'chill':>6} {'exp':>6} {'expEff':>6} " + f"{'wVis':>5} {'auth':>5} {'regime':>8} {'stand':>5} {'brakeF':>6} {'throtF':>6} {'hem_a':>6}") + print(hdr) + print(" " + "-" * 122) + for i in range(sl.start, sl.stop): + dt = t0[i] + row = [ + f"{dt:5.1f}", + f"{out['v_ego'][i]:5.2f}", + f"{out['a_ego'][i]:6.2f}", + 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'][i]:6.2f}", + f"{out['hem_w_vision'][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}", + f"{out['hem_a_throttle_fused'][i]:6.2f}", + f"{out['hem_a'][i]:6.2f}", + ] + print(" " + " ".join(row)) + + +def print_root_cause(out, t0, brake_cluster, sl, mode, stats): + print("\n" + "=" * 78) + print(" ROOT-CAUSE DIAGNOSIS (driver-brake anchored)") + print("=" * 78) + if not brake_cluster: + print(" No hard driver brake (a_ego < -2.0) found; no human intervention to explain.") + print(" If a stop was still rolled through, pass --start/--end around it.") + return + + i = brake_cluster[0] + dt = t0[i] + v = out["v_ego"][i] + print(f" Driver slammed the brakes at t = {dt:.1f}s (segment-relative), v_ego = {v:.1f} m/s " + f"({v * CV.MS_TO_MPH:.1f} mph), a_ego = {out['a_ego'][i]:.2f} m/s².") + print(f" HEM fused output at that instant: {out['hem_a'][i]:+.2f} m/s².") + + labels = { + "NO_STOP_REQUEST": "HEM NEVER REQUESTED A STOP", + "THROTTLE_OVERRIDE": "HEM ACCELERATED DESPITE VISION STOP", + "WEAK_BRAKE": "HEM BRAKED BUT TOO WEAKLY", + "LATE_BRAKE": "HEM BRAKED BUT TOO LATE", + "UNKNOWN": "NO CLEAR FAILURE MODE (see trace)", + } + print(f"\n FAILURE MODE: {labels.get(mode, mode)}") + + print("\n [HEM STATE IN THE ~3s BEFORE THE DRIVER BRAKE]") + print(f" Speed just before brake : {stats.get('v_before', 0):.1f} m/s (max {stats.get('v_max', 0):.1f})") + print(f" Vision weight w_vision : mean {stats.get('w_vis_mean', 0):.2f} | max {stats.get('w_vis_max', 0):.2f} (0.3+ = stop detected)") + print(f" Exp effective a : mean {stats.get('exp_eff_mean', 0):+.2f} | min {stats.get('exp_eff_min', 0):+.2f} m/s²") + print(f" Chill (ACC) a_chill : min {stats.get('chill_min', 0):+.2f} m/s²") + print(f" HEM output a_hem : mean {stats.get('hem_mean', 0):+.2f} | min {stats.get('hem_min', 0):+.2f} m/s²") + print(f" Throttle regime share : {100 * stats.get('throttle_share', 0):.0f}%") + + print("\n [INTERPRETATION]") + if mode == "NO_STOP_REQUEST": + print(" • Vision never flagged a stop (w_vision stayed < 0.5) and Exp requested no brake") + print(" (a_exp_eff >= 0). Chill/ACC also requested no brake. HEM coasted, so the driver") + print(" had to brake. The stop/light was missed at the perception level (model predicted go).") + elif mode == "THROTTLE_OVERRIDE": + print(" • Vision DID see the stop (exp_eff < -0.2) but HEM spent >60% of the approach in the") + print(" THROTTLE regime and output positive accel. The throttle path (smooth_max of chill/exp)") + print(" or a departing flag won over the brake path, so HEM drove toward the light.") + elif mode == "WEAK_BRAKE": + print(" • Vision saw the stop (w_vision > 0.4) and Exp requested a brake, but HEM's output") + print(f" never went harder than {stats.get('hem_min', 0):.2f} m/s². That decel was not enough to") + print(" stop in time at the approach speed, so the driver had to brake hard.") + elif mode == "LATE_BRAKE": + print(" • HEM did brake but only started hard late in the approach (or the needed stop distance") + print(" exceeded what its braking could cover), forcing the driver to intervene.") + else: + print(" • Review the per-frame trace above; the inputs/state were mixed.") + + print("\n [FIX DIRECTIONS]") + if mode in ("NO_STOP_REQUEST", "THROTTLE_OVERRIDE"): + print(" • When w_vision or exp_eff indicates a stop but neither input produces a brake, HEM") + print(" should synthesize its own kinematic brake (a_kinematic_stop) instead of blending 0's.") + print(" • Lock the braking regime (w_accel=0) harder when a stop is detected, so the throttle") + print(" path cannot override a vision brake.") + elif mode in ("WEAK_BRAKE", "LATE_BRAKE"): + print(" • HEM currently mirrors the model's Exp brake (a_exp_eff) and the ACC brake; when those") + print(" are too gentle for the required stop distance, HEM should apply a stronger floor based") + print(" on the kinematic stop calculation (-v²/2d) and vision brake sensitivity.") + print(" • Raise VISION_BRAKE_SENSITIVITY or lower the kinematic-stop distance threshold so the") + print(" vision stop requests decel earlier and harder.") + + +def plot_results(out, t0, args, incident_idxs): + import matplotlib + if not args.show: + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + n = len(t0) + fig, axs = plt.subplots(5, 1, figsize=(15, 17), sharex=True, + gridspec_kw={"height_ratios": [2, 2, 2, 2, 1.4]}) + fig.suptitle(f"HEM forensic {args.route} segs {args.segments} " + f"(bias={args_set(args.set, 'hybrid_exp_bias', 0.0):+.2f}, " + f"vis_brake={args_set(args.set, 'hybrid_vision_brake_sensitivity', 1.0):.2f})", + fontsize=11) + + # shade incident region + if incident_idxs: + for ax in axs: + x0 = t0[max(0, incident_idxs[0] - int(2 / DT_MDL))] + x1 = t0[min(n - 1, incident_idxs[-1] + int(2 / DT_MDL))] + ax.axvspan(x0, x1, color="red", alpha=0.08) + + # speed + axs[0].plot(t0, out["v_ego"] * CV.MS_TO_MPH, color="black", lw=1.5, label="v_ego") + axs[0].plot(t0, out["v_cruise"] * CV.MS_TO_MPH, color="tab:blue", lw=1.0, ls="--", label="v_cruise") + axs[0].plot(t0, out["model_v0"] * CV.MS_TO_MPH, color="tab:green", lw=1.0, alpha=0.7, label="model v[0]") + axs[0].set_ylabel("mph") + axs[0].set_title("1) Speed") + axs[0].legend(loc="upper right", fontsize=8) + axs[0].grid(alpha=0.3) + + # accel inputs vs output + axs[1].axhline(0, color="gray", lw=0.6) + hard_brake = out["a_ego"] < -2.0 + if np.any(hard_brake): + axs[1].fill_between(t0, -3.5, 2.0, where=hard_brake, step="post", + color="crimson", alpha=0.15, label="driver hard brake") + axs[1].plot(t0, out["a_chill"], color="tab:blue", lw=1.1, ls="--", label="Chill (ACC) input") + axs[1].plot(t0, out["a_exp"], color="tab:orange", lw=1.1, ls=":", label="Exp (vision) input") + axs[1].plot(t0, out["hem_a"], color="tab:cyan", lw=1.6, label="HEM fused output") + axs[1].plot(t0, out["a_ego"], color="crimson", lw=1.0, alpha=0.8, label="a_ego (actual)") + axs[1].plot(t0, out["logged_aTarget"], color="gray", lw=0.9, alpha=0.6, label="logged aTarget") + axs[1].set_ylim(-3.5, 2.0) + axs[1].set_ylabel("accel (m/s²)") + axs[1].set_title("2) HEM inputs vs output vs actual (crimson = driver brake)") + axs[1].legend(loc="upper right", fontsize=7, ncol=2) + axs[1].legend(loc="upper right", fontsize=8, ncol=2) + axs[1].grid(alpha=0.3) + + # 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_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") + axs[2].set_title("3) Vision stop detection vs Exp effective accel") + axs[2].legend(loc="upper right", fontsize=8) + axs[2].grid(alpha=0.3) + + # decision gates + axs[3].plot(t0, out["hem_a_brake_fused"], color="tab:blue", lw=1.2, label="brake-fused") + axs[3].plot(t0, out["hem_a_throttle_fused"], color="tab:orange", lw=1.2, label="throttle-fused") + axs[3].plot(t0, out["hem_a_anchored"], color="tab:green", lw=1.2, ls="--", label="after standstill") + axs[3].plot(t0, out["hem_a_safe"], color="tab:red", lw=1.2, ls="--", label="after safety") + axs[3].plot(t0, out["hem_a"], color="black", lw=1.6, label="final output") + axs[3].axhline(0, color="gray", lw=0.6) + axs[3].set_ylabel("accel (m/s²)") + axs[3].set_title("4) Fusion pipeline: which gate produced the output") + axs[3].legend(loc="upper right", fontsize=8, ncol=2) + axs[3].grid(alpha=0.3) + + # regime + standstill + axs[4].fill_between(t0, 0.0, 1.0, where=out["hem_regime"], step="post", + color="tab:red", alpha=0.5, label="brake regime") + axs[4].fill_between(t0, 1.0, 2.0, where=out["hem_standstill"], step="post", + color="tab:green", alpha=0.5, label="standstill anchor") + axs[4].set_yticks([0.5, 1.5]) + axs[4].set_yticklabels(["brake", "standstill"]) + axs[4].set_ylim(-0.3, 2.3) + axs[4].set_xlabel("time (s, segment-relative)") + axs[4].set_title("5) Regime") + axs[4].legend(loc="upper right", fontsize=8) + axs[4].grid(axis="y", alpha=0.3) + + fig.tight_layout() + if args.out: + p = Path(args.out) + p.parent.mkdir(parents=True, exist_ok=True) + fig.savefig(p, dpi=150) + print(f"Saved graph to {p}") + if args.show: + plt.show() + plt.close(fig) + + +def args_set(overrides, key, default): + for item in overrides: + k, sep, raw = item.partition("=") + if sep and k.strip() == key: + try: + return float(raw.strip()) + except ValueError: + return default + return default + + +def main(argv=None): + parser = argparse.ArgumentParser( + description="Forensic analysis of what HybridExperimentalMode decided, and why, on a route.") + parser.add_argument("route", help="route: / (the part after connect.comma.ai/), " + "e.g. afb7ef2ed593d651/000000af--414a758637") + parser.add_argument("--segments", default="0", help="comma-separated segment indices, e.g. 0,1") + parser.add_argument("--data_dir", help="local directory containing route files") + parser.add_argument("--start", type=float, default=None, help="start seconds (segment-relative)") + parser.add_argument("--end", type=float, default=None, help="end seconds (segment-relative)") + parser.add_argument("--window", type=float, default=8.0, + help="seconds of context to print around each detected incident (default 8)") + parser.add_argument("--out", default="hem_forensic.png", help="output PNG path") + parser.add_argument("--show", action="store_true", help="show plot window") + parser.add_argument("--csv", default=None, help="optional CSV output of the time series") + parser.add_argument("--set", action="append", default=[], metavar="KEY=value", + help="override a toggle, e.g. --set hybrid_exp_bias=0.2 --set hybrid_vision_brake_sensitivity=1.2") + args = parser.parse_args(argv) + + overrides = parse_toggle_overrides(args.set) + defaults = dict(TOGGLE_DEFAULTS) + defaults.update(overrides) + toggles = SimpleNamespace(**defaults) + for key, value in overrides.items(): + print(f"toggle override: {key} = {value}") + + segments = [int(s) for s in args.segments.split(",") if s.strip() != ""] + buffers_list = [] + for seg in segments: + identifiers = resolve_segment_identifier(args.route, seg, args.data_dir) + if not identifiers: + print(f"No data found for {args.route} seg {seg}", file=sys.stderr) + return 1 + print(f"Loading seg {seg}: {identifiers}") + bufs = load_buffers(identifiers, SERVICES) + if not any(bufs[s] for s in SERVICES): + print(f"No messages loaded for seg {seg}.", file=sys.stderr) + return 1 + buffers_list.append(bufs) + + bufs = merge_buffers(buffers_list) + all_min = min(t for s in SERVICES for t, _ in bufs[s]) + all_max = max(t for s in SERVICES for t, _ in bufs[s]) + start = all_min if args.start is None else all_min + args.start + end = all_max if args.end is None else min(all_max, all_min + args.end) + if end - start < DT_MDL: + print(f"Empty time window [{start:.1f}, {end:.1f}].", file=sys.stderr) + return 1 + grid = np.arange(start, end, DT_MDL) + print(f"Window: {start - all_min:.1f}s -> {end - all_min:.1f}s ({len(grid)} frames at {1 / DT_MDL:.0f} Hz)") + t0 = grid - start + + out = run_forensic(grid, bufs, toggles) + + if args.csv: + import csv + csv_path = Path(args.csv) + csv_path.parent.mkdir(parents=True, exist_ok=True) + top_cols = ["v_ego", "v_cruise", "a_chill", "a_exp", "hem_a", "hem_authority", + "lead_status", "lead_d_rel", "lead_v_lead", "a_ego", + "brake_pressed", "gas_pressed", "logged_aTarget"] + diag_cols = ["hem_" + k for k in DIAG_KEYS] + ["hem_regime", "hem_standstill"] + seen = set() + unique_cols = [] + for c in ["t"] + top_cols + diag_cols: + if c not in seen: + seen.add(c) + unique_cols.append(c) + with open(csv_path, "w", newline="") as f: + writer = csv.writer(f) + writer.writerow(unique_cols) + for i in range(len(t0)): + row = [f"{t0[i]:.3f}"] + for c in unique_cols[1:]: + row.append(f"{out[c][i]:.4f}") + writer.writerow(row) + print(f"Saved CSV to {csv_path}") + + brake_clusters = hard_brake_clusters(out) + if brake_clusters: + main_brake = max(brake_clusters, key=len) + lead_sec = int(args.window / DT_MDL) + sl = slice(max(0, main_brake[0] - lead_sec), + min(len(t0), main_brake[-1] + int(3 / DT_MDL))) + if sl.stop - sl.start > 600: + mid = (sl.start + sl.stop) // 2 + sl = slice(max(0, mid - 300), min(len(t0), mid + 300)) + print(f"\nDetected {len(brake_clusters)} driver hard-brake interventions.") + print(f"Focusing on the main one at t={t0[main_brake[0]]:.1f}s..{t0[main_brake[-1]]:.1f}s; " + "use --start/--end to zoom elsewhere.") + mode, stats = classify_failure(out, sl, main_brake[0]) + focus_incidents = main_brake + else: + print("\nNo hard driver brake (a_ego < -2.0) found in the window.") + print("Falling back to coast-through detection (vision saw a stop but HEM coasted).") + incident_idxs = detect_incidents(out, t0) + sl, focus_incidents = window_around(incident_idxs, t0, out, span_sec=args.window) + if sl is None: + sl = slice(0, len(t0)) + mode, stats = None, {} + + print_forensic_log(out, sl, t0) + print_root_cause(out, t0, focus_incidents, sl, mode, stats) + plot_results(out, t0, args, focus_incidents if focus_incidents else []) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/replay/hem_stop_analyzer.py b/tools/replay/hem_stop_analyzer.py new file mode 100644 index 000000000..edf41642e --- /dev/null +++ b/tools/replay/hem_stop_analyzer.py @@ -0,0 +1,325 @@ +#!/usr/bin/env python3 +import os +import sys +import numpy as np +import matplotlib.pyplot as plt +from pathlib import Path + +# Add openpilot paths if running from tools/ or other subdirectories +sys.path.insert(0, str(Path(__file__).resolve().parents[2])) + +from openpilot.common.realtime import DT_MDL +from openpilot.tools.lib.logreader import LogReader +from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode + +# Define the specific routes and segments where stop-sign or red-light failures occurred +ROUTES_TO_ANALYZE = [ + ("afb7ef2ed593d651/000000b3--9c3d58d585", 3, "Stop Sign"), + ("afb7ef2ed593d651/000000b3--9c3d58d585", 8, "Stop Sign"), + ("afb7ef2ed593d651/000000b3--9c3d58d585", 10, "Stop Sign"), + ("afb7ef2ed593d651/000000b3--9c3d58d585", 11, "Stop Sign"), + ("afb7ef2ed593d651/000000b4--1a67659212", 1, "Stop Sign"), + ("afb7ef2ed593d651/000000b5--8ee86fef97", 0, "Stop Sign"), + ("afb7ef2ed593d651/000000b6--711631f3fd", 0, "Stop Sign"), + ("afb7ef2ed593d651/000000b7--9bfbaf247a", 1, "Stop Sign"), + ("afb7ef2ed593d651/000000b8--e3147dbfc3", 0, "Stop Sign"), + ("afb7ef2ed593d651/000000b9--976a3b50cb", 0, "Stop Sign"), + ("afb7ef2ed593d651/000000b9--976a3b50cb", 1, "Stop Sign"), + ("afb7ef2ed593d651/000000b9--976a3b50cb", 2, "Stop Sign"), + ("afb7ef2ed593d651/000000b9--976a3b50cb", 3, "Stop Sign"), + ("afb7ef2ed593d651/000000b9--976a3b50cb", 6, "Red Light"), +] + +class MockLead: + def __init__(self, status=False, d_rel=150.0, v_lead=0.0): + self.status = status + self.dRel = float(d_rel) + self.vLead = float(v_lead) + +def fetch_telemetry(route_str, segment): + """Loads specified segment logs. Attempts local lookups first, then falls back to public API.""" + route_clean = route_str.replace("|", "/") + segment_str = f"{segment:02d}" if isinstance(segment, int) else str(segment) + + # Search local paths first + local_paths = [ + Path(f"/data/media/0/realdata/{route_clean}--{segment}"), + Path(os.path.expanduser(f"~/.comma/media/0/realdata/{route_clean}--{segment}")), + Path(f"./{route_clean}--{segment}"), + ] + + lr = None + for path in local_paths: + rlog_file = path / "rlog" + if rlog_file.exists(): + print(f"Loading local data: {rlog_file}") + lr = LogReader(str(rlog_file)) + break + + if lr is None: + # Construct comma public database URL fallback + dongle_id, route_sig = route_clean.split("/", 1) + remote_url = f"https://commadata2.blob.core.windows.net/commadata2/{dongle_id}/{route_sig}/{segment}/rlog" + print(f"Loading remote data: {remote_url}") + try: + lr = LogReader(remote_url) + except Exception as e: + print(f"Failed to fetch {remote_url}: {e}") + return [] + + print("Parsing messages...") + car_state_msgs = [] + model_msgs = [] + splan_msgs = [] + lplan_msgs = [] + + 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)) + + 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]) + + # Synchronize messages on model 20Hz time-grid + data_frames = [] + for t_model, model_msg in model_msgs: + # Find latest corresponding messages + 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) + + if cs is None: + continue + + v_ego = cs.vEgo + v_cruise = getattr(splan, "vCruise", getattr(cs, "vCruise", 0.0)) + a_chill = getattr(lplan, "aTarget", 0.0) + a_exp = getattr(model_msg.action, "desiredAcceleration", 0.0) + + data_frames.append({ + "t": t_model, + "v_ego": v_ego, + "v_cruise": v_cruise, + "a_chill": a_chill, + "a_exp": a_exp, + "model": model_msg, + }) + + return data_frames + +def simulate_hem(data_frames): + """Simulates the controller over the synchronized messages.""" + controller = HybridExperimentalMode() + controller.record_diag = True + + sim_results = [] + for frame in data_frames: + # Lead initialization + lead = MockLead(status=False) + + # Execute state update + a_out, should_stop_fused = controller.update( + v_ego=frame["v_ego"], + v_cruise=frame["v_cruise"], + lead_one=lead, + model_v2=frame["model"], + a_chill=frame["a_chill"], + a_exp=frame["a_exp"], + ) + + 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) + + return sim_results + +def analyze_failures(route_str, segment, label, results): + """Analyzes decisions inside the critical deceleration window.""" + total_frames = len(results) + if total_frames == 0: + return None + + # Find the primary deceleration zone (where v_ego drops, or should have dropped) + v_speeds = [r["v_ego"] for r in results] + max_v = max(v_speeds) + min_v = min(v_speeds) + + # 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("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: + return { + "route": route_str, "segment": segment, "label": label, + "outcome": "No stop event detected by model.", + "failure_mode": "Model did not predict stop point.", + } + + start_idx = min(active_frames) + end_idx = max(active_frames) + + # Analyze tracking variables inside the event window + latch_decays = 0 + tracking_resets = 0 + early_departures = 0 + kinematic_collapses = 0 + + for idx in range(start_idx, end_idx + 1): + 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("horizon_stopping", False): + latch_decays += 1 + + # 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("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 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 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 + failure_modes = [] + if latch_decays > 5: + failure_modes.append("Premature Latch Decay (w_vision collapsed)") + if tracking_resets > 0: + failure_modes.append("Stop Detection Cleared Early") + if early_departures > 5: + failure_modes.append("Early Departure Lockout Bypass (is_departing while approaching)") + if kinematic_collapses > 5: + 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." + + return { + "route": route_str, + "segment": segment, + "label": label, + "max_v": max_v, + "min_v": min_v, + "latch_decays": latch_decays, + "tracking_resets": tracking_resets, + "early_departures": early_departures, + "kinematic_collapses": kinematic_collapses, + "outcome": outcome, + "failure_mode": failure_mode, + } + +def run_suite(): + all_analyses = [] + plot_data = {} + + for route, seg, label in ROUTES_TO_ANALYZE: + print(f"\nAnalyzing {route} segment {seg} ({label})...") + frames = fetch_telemetry(route, seg) + if not frames: + print("No frames retrieved. Skipping.") + continue + + results = simulate_hem(frames) + analysis = analyze_failures(route, seg, label, results) + if analysis: + all_analyses.append(analysis) + plot_data[f"{route.split('/')[-1][:8]}_s{seg}"] = (frames, results) + + # 1. Export forensic summary text report + report_path = "hem_forensic_report.txt" + print(f"\nWriting forensic report to {report_path}...") + with open(report_path, "w") as f: + f.write("=== HYBRID EXPERIMENTAL MODE (HEM) FAILURE CASE STUDY SUMMARY ===\n") + f.write(f"Analyzed {len(all_analyses)} routes where vehicle rolled stop-lines\n") + f.write("==================================================================\n\n") + + # General findings + f.write("COMMON STRUCTURAL ROOT CAUSES IDENTIFIED:\n") + f.write("------------------------------------------\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 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") + for a in all_analyses: + f.write(f"Route: {a['route']} | Seg: {a['segment']} ({a['label']})\n") + f.write(f" • Velocity Profile : {a['max_v']:.1f} m/s -> {a['min_v']:.1f} m/s\n") + f.write(f" • Tracker Resets : {a['tracking_resets']} frames\n") + f.write(f" • Early Departures : {a['early_departures']} frames\n") + f.write(f" • Outcome : {a['outcome']}\n") + f.write(f" • Primary Fault : {a['failure_mode']}\n") + f.write("------------------------------------------------------------------\n") + + # 2. Generate tiled plot of critical variables for a subset of failures + print("Generating diagnostic plots...") + plot_keys = list(plot_data.keys())[:4] # Plot up to 4 significant failures for space + if not plot_keys: + return + + fig, axes = plt.subplots(len(plot_keys), 2, figsize=(15, 3 * len(plot_keys)), sharex="col") + if len(plot_keys) == 1: + axes = np.expand_dims(axes, axis=0) + + for idx, key in enumerate(plot_keys): + frames, results = plot_data[key] + t = [r["t_rel"] for r in results] + 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_brake_fused"] for r in results] + + # Left column: Speeds and Latch activations + ax_l = axes[idx, 0] + ax_l.plot(t, v_ego, color="black", lw=1.5, label="v_ego (m/s)") + ax_l_twin = ax_l.twinx() + ax_l_twin.plot(t, w_vis, color="orange", alpha=0.7, ls="--", label="w_vision (latch)") + ax_l.set_title(f"Run: {key} - Speeds", fontsize=10) + ax_l.set_ylabel("Speed (m/s)") + ax_l_twin.set_ylabel("Latch Active", color="orange") + ax_l.grid(alpha=0.3) + if idx == 0: + ax_l.legend(loc="upper left") + ax_l_twin.legend(loc="upper right") + + # Right column: Acceleration commands + ax_r = axes[idx, 1] + ax_r.plot(t, a_out, color="blue", lw=1.5, label="a_out (fused)") + ax_r.plot(t, a_kin, color="red", alpha=0.6, ls=":", label="a_kinematic_stop") + ax_r.set_title(f"Run: {key} - Acceleration Commands", fontsize=10) + ax_r.set_ylabel("Accel (m/s²)") + ax_r.grid(alpha=0.3) + if idx == 0: + ax_r.legend(loc="upper right") + + plt.tight_layout() + plot_out_path = "hem_failures_analysis.png" + plt.savefig(plot_out_path, dpi=150) + print(f"Diagnostic graph saved to {plot_out_path}") + +if __name__ == "__main__": + run_suite() \ No newline at end of file diff --git a/tools/replay/hem_stop_analyzer.py.py b/tools/replay/hem_stop_analyzer.py.py new file mode 100644 index 000000000..edf41642e --- /dev/null +++ b/tools/replay/hem_stop_analyzer.py.py @@ -0,0 +1,325 @@ +#!/usr/bin/env python3 +import os +import sys +import numpy as np +import matplotlib.pyplot as plt +from pathlib import Path + +# Add openpilot paths if running from tools/ or other subdirectories +sys.path.insert(0, str(Path(__file__).resolve().parents[2])) + +from openpilot.common.realtime import DT_MDL +from openpilot.tools.lib.logreader import LogReader +from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode + +# Define the specific routes and segments where stop-sign or red-light failures occurred +ROUTES_TO_ANALYZE = [ + ("afb7ef2ed593d651/000000b3--9c3d58d585", 3, "Stop Sign"), + ("afb7ef2ed593d651/000000b3--9c3d58d585", 8, "Stop Sign"), + ("afb7ef2ed593d651/000000b3--9c3d58d585", 10, "Stop Sign"), + ("afb7ef2ed593d651/000000b3--9c3d58d585", 11, "Stop Sign"), + ("afb7ef2ed593d651/000000b4--1a67659212", 1, "Stop Sign"), + ("afb7ef2ed593d651/000000b5--8ee86fef97", 0, "Stop Sign"), + ("afb7ef2ed593d651/000000b6--711631f3fd", 0, "Stop Sign"), + ("afb7ef2ed593d651/000000b7--9bfbaf247a", 1, "Stop Sign"), + ("afb7ef2ed593d651/000000b8--e3147dbfc3", 0, "Stop Sign"), + ("afb7ef2ed593d651/000000b9--976a3b50cb", 0, "Stop Sign"), + ("afb7ef2ed593d651/000000b9--976a3b50cb", 1, "Stop Sign"), + ("afb7ef2ed593d651/000000b9--976a3b50cb", 2, "Stop Sign"), + ("afb7ef2ed593d651/000000b9--976a3b50cb", 3, "Stop Sign"), + ("afb7ef2ed593d651/000000b9--976a3b50cb", 6, "Red Light"), +] + +class MockLead: + def __init__(self, status=False, d_rel=150.0, v_lead=0.0): + self.status = status + self.dRel = float(d_rel) + self.vLead = float(v_lead) + +def fetch_telemetry(route_str, segment): + """Loads specified segment logs. Attempts local lookups first, then falls back to public API.""" + route_clean = route_str.replace("|", "/") + segment_str = f"{segment:02d}" if isinstance(segment, int) else str(segment) + + # Search local paths first + local_paths = [ + Path(f"/data/media/0/realdata/{route_clean}--{segment}"), + Path(os.path.expanduser(f"~/.comma/media/0/realdata/{route_clean}--{segment}")), + Path(f"./{route_clean}--{segment}"), + ] + + lr = None + for path in local_paths: + rlog_file = path / "rlog" + if rlog_file.exists(): + print(f"Loading local data: {rlog_file}") + lr = LogReader(str(rlog_file)) + break + + if lr is None: + # Construct comma public database URL fallback + dongle_id, route_sig = route_clean.split("/", 1) + remote_url = f"https://commadata2.blob.core.windows.net/commadata2/{dongle_id}/{route_sig}/{segment}/rlog" + print(f"Loading remote data: {remote_url}") + try: + lr = LogReader(remote_url) + except Exception as e: + print(f"Failed to fetch {remote_url}: {e}") + return [] + + print("Parsing messages...") + car_state_msgs = [] + model_msgs = [] + splan_msgs = [] + lplan_msgs = [] + + 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)) + + 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]) + + # Synchronize messages on model 20Hz time-grid + data_frames = [] + for t_model, model_msg in model_msgs: + # Find latest corresponding messages + 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) + + if cs is None: + continue + + v_ego = cs.vEgo + v_cruise = getattr(splan, "vCruise", getattr(cs, "vCruise", 0.0)) + a_chill = getattr(lplan, "aTarget", 0.0) + a_exp = getattr(model_msg.action, "desiredAcceleration", 0.0) + + data_frames.append({ + "t": t_model, + "v_ego": v_ego, + "v_cruise": v_cruise, + "a_chill": a_chill, + "a_exp": a_exp, + "model": model_msg, + }) + + return data_frames + +def simulate_hem(data_frames): + """Simulates the controller over the synchronized messages.""" + controller = HybridExperimentalMode() + controller.record_diag = True + + sim_results = [] + for frame in data_frames: + # Lead initialization + lead = MockLead(status=False) + + # Execute state update + a_out, should_stop_fused = controller.update( + v_ego=frame["v_ego"], + v_cruise=frame["v_cruise"], + lead_one=lead, + model_v2=frame["model"], + a_chill=frame["a_chill"], + a_exp=frame["a_exp"], + ) + + 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) + + return sim_results + +def analyze_failures(route_str, segment, label, results): + """Analyzes decisions inside the critical deceleration window.""" + total_frames = len(results) + if total_frames == 0: + return None + + # Find the primary deceleration zone (where v_ego drops, or should have dropped) + v_speeds = [r["v_ego"] for r in results] + max_v = max(v_speeds) + min_v = min(v_speeds) + + # 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("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: + return { + "route": route_str, "segment": segment, "label": label, + "outcome": "No stop event detected by model.", + "failure_mode": "Model did not predict stop point.", + } + + start_idx = min(active_frames) + end_idx = max(active_frames) + + # Analyze tracking variables inside the event window + latch_decays = 0 + tracking_resets = 0 + early_departures = 0 + kinematic_collapses = 0 + + for idx in range(start_idx, end_idx + 1): + 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("horizon_stopping", False): + latch_decays += 1 + + # 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("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 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 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 + failure_modes = [] + if latch_decays > 5: + failure_modes.append("Premature Latch Decay (w_vision collapsed)") + if tracking_resets > 0: + failure_modes.append("Stop Detection Cleared Early") + if early_departures > 5: + failure_modes.append("Early Departure Lockout Bypass (is_departing while approaching)") + if kinematic_collapses > 5: + 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." + + return { + "route": route_str, + "segment": segment, + "label": label, + "max_v": max_v, + "min_v": min_v, + "latch_decays": latch_decays, + "tracking_resets": tracking_resets, + "early_departures": early_departures, + "kinematic_collapses": kinematic_collapses, + "outcome": outcome, + "failure_mode": failure_mode, + } + +def run_suite(): + all_analyses = [] + plot_data = {} + + for route, seg, label in ROUTES_TO_ANALYZE: + print(f"\nAnalyzing {route} segment {seg} ({label})...") + frames = fetch_telemetry(route, seg) + if not frames: + print("No frames retrieved. Skipping.") + continue + + results = simulate_hem(frames) + analysis = analyze_failures(route, seg, label, results) + if analysis: + all_analyses.append(analysis) + plot_data[f"{route.split('/')[-1][:8]}_s{seg}"] = (frames, results) + + # 1. Export forensic summary text report + report_path = "hem_forensic_report.txt" + print(f"\nWriting forensic report to {report_path}...") + with open(report_path, "w") as f: + f.write("=== HYBRID EXPERIMENTAL MODE (HEM) FAILURE CASE STUDY SUMMARY ===\n") + f.write(f"Analyzed {len(all_analyses)} routes where vehicle rolled stop-lines\n") + f.write("==================================================================\n\n") + + # General findings + f.write("COMMON STRUCTURAL ROOT CAUSES IDENTIFIED:\n") + f.write("------------------------------------------\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 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") + for a in all_analyses: + f.write(f"Route: {a['route']} | Seg: {a['segment']} ({a['label']})\n") + f.write(f" • Velocity Profile : {a['max_v']:.1f} m/s -> {a['min_v']:.1f} m/s\n") + f.write(f" • Tracker Resets : {a['tracking_resets']} frames\n") + f.write(f" • Early Departures : {a['early_departures']} frames\n") + f.write(f" • Outcome : {a['outcome']}\n") + f.write(f" • Primary Fault : {a['failure_mode']}\n") + f.write("------------------------------------------------------------------\n") + + # 2. Generate tiled plot of critical variables for a subset of failures + print("Generating diagnostic plots...") + plot_keys = list(plot_data.keys())[:4] # Plot up to 4 significant failures for space + if not plot_keys: + return + + fig, axes = plt.subplots(len(plot_keys), 2, figsize=(15, 3 * len(plot_keys)), sharex="col") + if len(plot_keys) == 1: + axes = np.expand_dims(axes, axis=0) + + for idx, key in enumerate(plot_keys): + frames, results = plot_data[key] + t = [r["t_rel"] for r in results] + 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_brake_fused"] for r in results] + + # Left column: Speeds and Latch activations + ax_l = axes[idx, 0] + ax_l.plot(t, v_ego, color="black", lw=1.5, label="v_ego (m/s)") + ax_l_twin = ax_l.twinx() + ax_l_twin.plot(t, w_vis, color="orange", alpha=0.7, ls="--", label="w_vision (latch)") + ax_l.set_title(f"Run: {key} - Speeds", fontsize=10) + ax_l.set_ylabel("Speed (m/s)") + ax_l_twin.set_ylabel("Latch Active", color="orange") + ax_l.grid(alpha=0.3) + if idx == 0: + ax_l.legend(loc="upper left") + ax_l_twin.legend(loc="upper right") + + # Right column: Acceleration commands + ax_r = axes[idx, 1] + ax_r.plot(t, a_out, color="blue", lw=1.5, label="a_out (fused)") + ax_r.plot(t, a_kin, color="red", alpha=0.6, ls=":", label="a_kinematic_stop") + ax_r.set_title(f"Run: {key} - Acceleration Commands", fontsize=10) + ax_r.set_ylabel("Accel (m/s²)") + ax_r.grid(alpha=0.3) + if idx == 0: + ax_r.legend(loc="upper right") + + plt.tight_layout() + plot_out_path = "hem_failures_analysis.png" + plt.savefig(plot_out_path, dpi=150) + print(f"Diagnostic graph saved to {plot_out_path}") + +if __name__ == "__main__": + run_suite() \ No newline at end of file diff --git a/tools/replay/mode_sim.py b/tools/replay/mode_sim.py new file mode 100644 index 000000000..02e815bfb --- /dev/null +++ b/tools/replay/mode_sim.py @@ -0,0 +1,840 @@ +#!/usr/bin/env python3 +"""Offline mode-scenario estimator for CEM / Chill (CCM) / HEM / Exp. + +Replays a logged route segment through the real StarPilot mode state machines +(ConditionalExperimentalMode, ConditionalChillMode, HybridExperimentalMode) +and openpilot's LongitudinalPlanner, then plots and prints what each mode's +experimental intent and acceleration target would have been at every 20 Hz frame. + +Usage: + ./dev python tools/replay/mode_sim.py [--segment 0] [--start 30] [--end 120] + [--data_dir /path/to/routes] [--out graph.png] [--show] + [--set CESpeed=20 --set CEModelStopTime=3.0 --set CCMLead=true] [--csv out.csv] + +Examples: + ./dev python tools/replay/mode_sim.py afb7ef2ed593d651/00000095--9dcb90357c --segment 9 +""" +from __future__ import annotations + +import argparse +import bisect +import sys +import time +from pathlib import Path +from types import SimpleNamespace +from typing import Any + +import numpy as np + +ROOT_DIR = Path(__file__).resolve().parents[2] +if str(ROOT_DIR) not in sys.path: + sys.path.insert(0, str(ROOT_DIR)) + +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET +from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_accel_from_plan +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE +from openpilot.starpilot.common.experimental_state import CCStatus, CEStatus +from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, calculate_road_curvature +from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME +from openpilot.starpilot.controls.lib.conditional_chill_mode import ConditionalChillMode +from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode +from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode +from openpilot.tools.lib.logreader import LogReader, ReadMode, parse_direct, parse_indirect +from openpilot.tools.lib.route import SegmentRange + +SERVICES = { + "carState", "radarState", "starpilotRadarState", "modelV2", + "longitudinalPlan", "selfdriveState", "starpilotPlan", "starpilotCarState", + "controlsState", "carParams", "liveParameters", "carControl", +} + +CE_LABELS = { + CEStatus["CURVATURE"]: "CURVE", + CEStatus["LEAD"]: "LEAD", + CEStatus["SIGNAL"]: "SIGNAL", + CEStatus["SPEED"]: "SPEED", + CEStatus["SPEED_LIMIT"]: "SPEED_LIMIT", + CEStatus["STOP_LIGHT"]: "STOP_LIGHT", +} +CC_LABELS = { + CCStatus["LEAD"]: "LEAD", + CCStatus["SPEED"]: "SPEED", +} +CC_AUTO_STATUSES = {CCStatus["SPEED"], CCStatus["LEAD"]} + +# CEM/CCM toggle defaults (units in m/s where appropriate). +TOGGLE_DEFAULTS: dict[str, Any] = { + # CEM + "conditional_curves": True, + "conditional_curves_lead": True, + "conditional_lead": True, + "conditional_slower_lead": True, + "conditional_stopped_lead": True, + "conditional_open_road": False, + "conditional_limit": 0.0, # CESpeed, m/s (0 = disabled) + "conditional_limit_lead": 0.0, # CESpeedLead, m/s (0 = disabled) + "conditional_signal": 0.0, # CESignalSpeed, m/s (0 = disabled) + "conditional_model_stop_time": 0.0, # CEModelStopTime, s (0 = disabled) + "conditional_signal_lane_detection": False, + "lane_detection_width": 0.0, + # Chill (CCM) + "conditional_chill_speed": 30 * CV.MPH_TO_MS, # CCMSpeed + "conditional_chill_speed_lead": 25 * CV.MPH_TO_MS, # CCMSpeedLead + "conditional_chill_speed_margin": 3 * CV.MPH_TO_MS, # CCMSetSpeedMargin + "conditional_chill_lead": True, + "conditional_chill_launch_assist": False, + # HEM + "hybrid_exp_bias": 0.0, + "hybrid_vision_brake_sensitivity": 1.0, + # StarPilot / Planner toggles + "taco_tune": False, + "classic_model": False, + "tinygrad_model": False, + "vEgoStopping": 0.05, + "hybrid_experimental_mode": False, + "radar_takeoffs": False, + "lane_change_close_gap": False, + "minimum_lane_change_speed": 0.0, + "model_version": None, +} +BOOL_KEYS = { + "conditional_curves", "conditional_curves_lead", "conditional_lead", + "conditional_slower_lead", "conditional_stopped_lead", "conditional_open_road", + "conditional_signal_lane_detection", "conditional_chill_lead", + "conditional_chill_launch_assist", "taco_tune", "classic_model", "tinygrad_model", + "hybrid_experimental_mode", "radar_takeoffs", "lane_change_close_gap", +} + + +class FakeParams: + def __init__(self): + self.bools = {} + self.ints = {} + + def get_bool(self, key): + return bool(self.bools.get(key, False)) + + def put_bool(self, key, value): + self.bools[key] = bool(value) + + def get_int(self, key, default=0): + return int(self.ints.get(key, default)) + + def put_int(self, key, value): + self.ints[key] = int(value) + + +def default_lead(status=False): + return SimpleNamespace( + status=status, + dRel=float("inf"), + yRel=0.0, + vRel=0.0, + aRel=0.0, + vLead=0.0, + vLeadK=0.0, + dPath=0.0, + vLat=0.0, + aLeadK=0.0, + aLeadTau=1.5, + fcw=False, + modelProb=0.0, + radar=False, + ) + + +def as_lead(raw): + if raw is None: + return default_lead() + try: + status = bool(getattr(raw, "status", False)) + except Exception: + status = False + return SimpleNamespace( + status=status, + dRel=float(getattr(raw, "dRel", float("inf"))), + yRel=float(getattr(raw, "yRel", 0.0)), + vRel=float(getattr(raw, "vRel", 0.0)), + aRel=float(getattr(raw, "aRel", 0.0)), + vLead=float(getattr(raw, "vLead", 0.0)), + vLeadK=float(getattr(raw, "vLeadK", getattr(raw, "vLead", 0.0))), + dPath=float(getattr(raw, "dPath", 0.0)), + vLat=float(getattr(raw, "vLat", 0.0)), + aLeadK=float(getattr(raw, "aLeadK", 0.0)), + aLeadTau=float(getattr(raw, "aLeadTau", 1.5)), + fcw=bool(getattr(raw, "fcw", False)), + modelProb=float(getattr(raw, "modelProb", 0.0)), + radar=bool(getattr(raw, "radar", False)), + ) + + +def local_segment_files(data_dir, route_name, segment): + data_root = Path(data_dir) + segment_names = (f"{route_name}--{segment}", f"{route_name.replace('|', '/')}/{segment}") + filenames = ("rlog.zst", "rlog.bz2", "qlog.zst", "qlog.bz2") + identifiers = [] + for segment_name in segment_names: + for filename in filenames: + candidate = data_root / segment_name / filename + if candidate.exists(): + identifiers.append(str(candidate)) + for filename in filenames: + explorer = data_root / f"{route_name}--{segment}--{filename}" + if explorer.exists(): + identifiers.append(str(explorer)) + return identifiers + + +def resolve_segment_identifier(route, segment, data_dir): + direct = parse_direct(route) + if direct is not None: + return [str(direct)] + parsed = parse_indirect(route) + sr = SegmentRange(parsed) + route_name = sr.route_name.replace("/", "|") + if data_dir: + identifiers = local_segment_files(data_dir, route_name, segment) + if identifiers: + return identifiers + return [f"{route_name}--{segment}"] + + +def load_buffers(identifiers, services): + bufs = {s: [] for s in services} + for identifier in identifiers: + try: + logreader = LogReader(identifier, default_mode=ReadMode.RLOG) + except Exception as exc: + print(f"Unable to open {identifier}: {exc}", file=sys.stderr) + continue + for evt in logreader: + which = evt.which() + if which not in services: + continue + try: + t = evt.logMonoTime * 1e-9 + except Exception: + continue + bufs[which].append((t, getattr(evt, which))) + for service in services: + bufs[service].sort(key=lambda x: x[0]) + return bufs + + +def parse_toggle_overrides(args_set): + overrides = {} + for item in args_set or []: + key, sep, raw = item.partition("=") + key = key.strip() + if not sep or key not in TOGGLE_DEFAULTS: + print(f"warning: ignoring unknown toggle override '{item}'", file=sys.stderr) + continue + raw = raw.strip() + if key in BOOL_KEYS: + overrides[key] = raw.lower() in {"1", "true", "yes", "on"} + else: + try: + overrides[key] = float(raw) + except ValueError: + print(f"warning: ignoring non-numeric toggle override '{item}'", file=sys.stderr) + return overrides + + +def run_simulation(grid, bufs, toggles): + ts = {s: [t for t, _ in m] for s, m in bufs.items()} + ms = {s: [m for _, m in bufs[s]] for s in bufs} + + def latest(service, t): + times = ts.get(service) + if not times: + return None + idx = bisect.bisect_right(times, t) - 1 + return ms[service][idx] if idx >= 0 else None + + cp_candidate = None + for _, cp in bufs.get("carParams", []): + if cp is not None: + cp_candidate = cp + break + + if cp_candidate is None: + cp_candidate = SimpleNamespace( + brand="toyota", + carFingerprint="TOYOTA_RAV4", + openpilotLongitudinalControl=True, + pcmCruise=False, + steerRatio=15.0, + wheelbase=2.7, + longitudinalActuatorDelay=0.2, + flags=0, + ) + + planner_state = SimpleNamespace( + params=FakeParams(), + params_memory=FakeParams(), + starpilot_following=SimpleNamespace(following_lead=False, slower_lead=False), + starpilot_vcruise=SimpleNamespace( + slc=SimpleNamespace(experimental_mode=False), + stop_sign_confirmed=False, + forcing_stop=False, + ), + ) + + cem = ConditionalExperimentalMode(planner_state) + cem_ccm_detector = ConditionalExperimentalMode(planner_state) + ccm = ConditionalChillMode(planner_state, cem_ccm_detector) + hybrid = HybridExperimentalMode() + hybrid.set_tuning(toggles.hybrid_exp_bias, toggles.hybrid_vision_brake_sensitivity) + + # Full openpilot Longitudinal Planner for true Chill (ACC) MPC execution + chill_long_planner = LongitudinalPlanner(cp_candidate, dt=DT_MDL) + + n = len(grid) + out = { + "t": grid, + "v_ego": np.zeros(n), + "v_cruise": np.zeros(n), + "model_v0": np.zeros(n), + "lead_status": np.zeros(n, dtype=bool), + "lead_dRel": np.zeros(n), + "lead_vLead": np.zeros(n), + "cem_exp": np.zeros(n, dtype=bool), + "cem_status": np.zeros(n, dtype=int), + "ccm_exp": np.zeros(n, dtype=bool), + "ccm_status": np.zeros(n, dtype=int), + "chill_active": np.zeros(n, dtype=bool), + "logged_exp": np.zeros(n, dtype=bool), + "logged_aTarget": np.zeros(n), + "a_chill": np.zeros(n), + "a_exp": np.zeros(n), + "hem_a": np.zeros(n), + "hem_authority": np.zeros(n), + } + + real_monotonic = time.monotonic + fake_clock = [0.0] + time.monotonic = lambda: fake_clock[0] + + try: + for i, t in enumerate(grid): + fake_clock[0] = float(i) * DT_MDL + + car_raw = latest("carState", t) + v_ego_raw = max(float(getattr(car_raw, "vEgo", 0.0)), 0.0) + car = SimpleNamespace( + vEgo=v_ego_raw, + vEgoCluster=max(float(getattr(car_raw, "vEgoCluster", v_ego_raw)), 0.0), + vCruise=float(getattr(car_raw, "vCruise", 0.0)), + standstill=bool(getattr(car_raw, "standstill", False)), + leftBlinker=bool(getattr(car_raw, "leftBlinker", False)), + rightBlinker=bool(getattr(car_raw, "rightBlinker", False)), + gasPressed=bool(getattr(car_raw, "gasPressed", False)), + brakePressed=bool(getattr(car_raw, "brakePressed", False)), + steeringAngleDeg=float(getattr(car_raw, "steeringAngleDeg", 0.0)), + aEgo=float(getattr(car_raw, "aEgo", 0.0)), + leftBlindspot=bool(getattr(car_raw, "leftBlindspot", False)), + rightBlindspot=bool(getattr(car_raw, "rightBlindspot", False)), + ) + v_ego = car.vEgo + + radar = latest("radarState", t) + lead = as_lead(getattr(radar, "leadOne", None) if radar is not None else None) + lead2 = as_lead(getattr(radar, "leadTwo", None) if radar is not None else None) + + sradar = latest("starpilotRadarState", t) + lead_left = as_lead(getattr(sradar, "leadLeft", None) if sradar is not None else None) + lead_right = as_lead(getattr(sradar, "leadRight", None) if sradar is not None else None) + + lplan = latest("longitudinalPlan", t) + sds = latest("selfdriveState", t) + splan = latest("starpilotPlan", t) + scs = latest("starpilotCarState", t) + model_v2 = latest("modelV2", t) + live_params = latest("liveParameters", t) + car_control = latest("carControl", t) + controls_state = latest("controlsState", t) + car_params = latest("carParams", t) or cp_candidate + + controls_enabled = bool(getattr(sds, "enabled", False)) + conditional_tracking_active = controls_enabled or bool(getattr(scs, "alwaysOnLateralEnabled", False)) + + v_cruise = float(getattr(splan, "vCruise", 0.0) or 0.0) + if not (v_cruise > 0): + v_cruise_kph = float(getattr(car_raw, "vCruise", 0.0) or 0.0) + if 0 < v_cruise_kph < V_CRUISE_UNSET: + v_cruise = min(v_cruise_kph, V_CRUISE_MAX) * CV.KPH_TO_MS + else: + v_cruise = v_ego + + tracking_lead = bool(getattr(splan, "trackingLead", False)) + if not tracking_lead: + tracking_lead = bool(getattr(lplan, "hasLead", False)) + + model_length = 0.0 + if model_v2 is not None: + try: + pos_x = getattr(model_v2, "position", None) + if pos_x is not None and len(pos_x.x): + model_length = float(pos_x.x[-1]) + except Exception: + model_length = 0.0 + + forcing_stop = bool(getattr(splan, "forcingStop", False)) + raw_model_stopped = model_length < CRUISING_SPEED * PLANNER_TIME + model_stopped = raw_model_stopped or forcing_stop + + road_curvature_detected = False + if model_v2 is not None: + try: + road_curvature, _time_to_curve = calculate_road_curvature(model_v2, v_ego) + road_curvature_detected = ( + (1 / abs(road_curvature)) ** 0.5 < v_ego > CRUISING_SPEED + and not (car.leftBlinker or car.rightBlinker) + ) + except Exception: + road_curvature_detected = False + + try: + curvature = float(getattr(controls_state, "curvature", 0.0)) + except Exception: + curvature = 0.0 + lateral_accel = v_ego ** 2 * curvature + driving_in_curve = abs(lateral_accel) >= MINIMUM_LATERAL_ACCELERATION + + lane_width_left = 0.0 + lane_width_right = 0.0 + if model_v2 is not None: + try: + lane_lines = getattr(model_v2, "laneLines", None) + road_edges = getattr(model_v2, "roadEdges", None) + if lane_lines is not None and len(lane_lines) >= 4: + edge_left = road_edges[0] if road_edges is not None and len(road_edges) else None + edge_right = road_edges[1] if road_edges is not None and len(road_edges) > 1 else None + lane_width_left = calculate_lane_width(lane_lines[0], lane_lines[1], edge_left) + lane_width_right = calculate_lane_width(lane_lines[3], lane_lines[2], edge_right) + except Exception: + pass + + t_follow = float(getattr(splan, "tFollow", 1.45)) + following_lead = tracking_lead and lead.dRel < (t_follow * 2) * v_ego + slower_lead = False + if (conditional_tracking_active and tracking_lead and toggles.conditional_slower_lead + and lead.status and lead.vLead < v_ego): + distance_factor = max(lead.dRel - (lead.vLead * t_follow), 1) + braking_offset = float(np.clip(min(v_ego - lead.vLead, lead.vLead) - COMFORT_BRAKE, 1, distance_factor)) + slower_lead = braking_offset > 1 + + planner_state.lead_one = lead + planner_state.tracking_lead = tracking_lead + planner_state.model_length = model_length + planner_state.raw_model_stopped = raw_model_stopped + planner_state.model_stopped = model_stopped + planner_state.road_curvature_detected = road_curvature_detected + planner_state.driving_in_curve = driving_in_curve + planner_state.lane_width_left = lane_width_left + planner_state.lane_width_right = lane_width_right + planner_state.starpilot_following.following_lead = following_lead + planner_state.starpilot_following.slower_lead = slower_lead + planner_state.starpilot_vcruise.forcing_stop = forcing_stop + + raw_personality = getattr(sds, "personality", None) + if hasattr(raw_personality, "raw"): + personality_val = int(raw_personality.raw) + elif isinstance(raw_personality, (int, float)): + personality_val = int(raw_personality) + else: + personality_val = 0 + + long_ctrl_state = getattr(controls_state, "longControlState", LongCtrlState.pid) + + sm_dict = { + "carState": car, + "radarState": SimpleNamespace(leadOne=lead, leadTwo=lead2), + "starpilotRadarState": SimpleNamespace(leadLeft=lead_left, leadRight=lead_right), + "starpilotCarState": SimpleNamespace( + trafficModeEnabled=bool(getattr(scs, "trafficModeEnabled", False)), + alwaysOnLateralEnabled=bool(getattr(scs, "alwaysOnLateralEnabled", False)), + dashboardStopSign=int(getattr(scs, "dashboardStopSign", 0)), + accelPressed=bool(getattr(scs, "accelPressed", False)), + ), + "selfdriveState": SimpleNamespace( + enabled=controls_enabled, + experimentalMode=False, # Enforce ACC/Chill mode evaluation in MPC + personality=personality_val, + ), + "longitudinalPlan": SimpleNamespace( + hasLead=bool(getattr(lplan, "hasLead", False)), + allowThrottle=bool(getattr(lplan, "allowThrottle", True)), + shouldStop=bool(getattr(lplan, "shouldStop", False)), + aTarget=float(getattr(lplan, "aTarget", 0.0)), + ), + "starpilotPlan": SimpleNamespace( + vCruise=v_cruise, + tFollow=t_follow, + trackingLead=tracking_lead, + redLight=bool(getattr(splan, "redLight", False)), + forcingStop=forcing_stop, + forcingStopLength=float(getattr(splan, "forcingStopLength", 100.0)), + minAcceleration=float(getattr(splan, "minAcceleration", -3.5)), + maxAcceleration=float(getattr(splan, "maxAcceleration", 1.5)), + accelerationJerk=float(getattr(splan, "accelerationJerk", 1.0)), + dangerJerk=float(getattr(splan, "dangerJerk", 1.0)), + speedJerk=float(getattr(splan, "speedJerk", 1.0)), + dangerFactor=float(getattr(splan, "dangerFactor", 1.0)), + disableThrottle=bool(getattr(splan, "disableThrottle", False)), + ), + "controlsState": SimpleNamespace( + longControlState=long_ctrl_state, + forceDecel=bool(getattr(controls_state, "forceDecel", False)), + curvature=curvature, + ), + "liveParameters": SimpleNamespace( + angleOffsetDeg=float(getattr(live_params, "angleOffsetDeg", 0.0)) if live_params else 0.0, + ), + "carControl": SimpleNamespace( + orientationNED=getattr(car_control, "orientationNED", [0.0, 0.0, 0.0]) if car_control else [0.0, 0.0, 0.0], + ), + "modelV2": model_v2, + "carParams": car_params, + } + + # Run CEM / CCM State Machines + if conditional_tracking_active: + cem.update(v_ego, sm_dict, toggles, v_cruise) + ccm.update(v_ego, v_cruise, sm_dict, toggles) + else: + cem.experimental_mode = False + cem.status_value = CEStatus["OFF"] + ccm.experimental_mode = True + ccm.status_value = CCStatus["OFF"] + + chill_active = (not ccm.experimental_mode) and ccm.status_value in CC_AUTO_STATUSES + + # Compute accurate MPC Chill (ACC) Acceleration + chill_long_planner.update(sm_dict, toggles) + a_chill = float(chill_long_planner.output_a_target) + + # Determine realistic Exp acceleration target + a_exp = 0.0 + if model_v2 is not None: + try: + action_obj = getattr(model_v2, "action", None) + if action_obj is not None and hasattr(action_obj, "desiredAcceleration"): + a_exp = float(action_obj.desiredAcceleration) + else: + accel_x = getattr(model_v2, "acceleration", None) + if accel_x is not None and len(accel_x.x): + a_exp = float(accel_x.x[0]) + except Exception: + a_exp = 0.0 + + # Hybrid Experimental Mode continuous fusion + 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: + try: + vel_x = getattr(model_v2, "velocity", None) + if vel_x is not None and len(vel_x.x): + model_v0 = float(vel_x.x[0]) + except Exception: + model_v0 = v_ego + + out["v_ego"][i] = v_ego + out["v_cruise"][i] = v_cruise + out["model_v0"][i] = model_v0 + out["lead_status"][i] = lead.status + out["lead_dRel"][i] = lead.dRel + out["lead_vLead"][i] = lead.vLead + out["cem_exp"][i] = bool(cem.experimental_mode) + out["cem_status"][i] = int(cem.status_value) + out["ccm_exp"][i] = bool(ccm.experimental_mode) + out["ccm_status"][i] = int(ccm.status_value) + out["chill_active"][i] = chill_active + out["logged_exp"][i] = bool(getattr(sds, "experimentalMode", False)) + out["logged_aTarget"][i] = float(getattr(lplan, "aTarget", 0.0)) + out["a_chill"][i] = a_chill + out["a_exp"][i] = a_exp + out["hem_a"][i] = a_hem + out["hem_authority"][i] = authority + finally: + time.monotonic = real_monotonic + + return out + + +def intervals_for(t, keys): + """Group consecutive identical non-zero status keys into [start, end, key] spans.""" + spans = [] + cur = 0 + for i, k in enumerate(keys): + if k and k == cur: + spans[-1][1] = t[i] + elif k: + spans.append([t[i], t[i], k]) + cur = k + else: + cur = 0 + return spans + + +def print_summary(out): + t = out["t"] + n = len(t) + if n == 0: + print("No simulation frames to summarize.") + return + + v_ego_mph = out["v_ego"] * CV.MS_TO_MPH + v_cruise_mph = out["v_cruise"] * CV.MS_TO_MPH + standstill_mask = out["v_ego"] < 0.2 + standstill_pct = 100.0 * np.mean(standstill_mask) + + stops = 0 + for i in range(1, n): + if standstill_mask[i] and not standstill_mask[i - 1]: + stops += 1 + + cem_switches = int(np.sum(np.diff(out["cem_exp"].astype(int)) != 0)) + ccm_switches = int(np.sum(np.diff(out["chill_active"].astype(int)) != 0)) + + print("\n" + "=" * 78) + print(f" MODE SIMULATION SUMMARY & TELEMETRY LOG ({t[0]:.1f}s -> {t[-1]:.1f}s | {n} frames)") + print("=" * 78) + + print("\n[1. DRIVE PROFILE]") + print(f" • Speed (v_ego) : Min {v_ego_mph.min():.1f} mph | Avg {v_ego_mph.mean():.1f} mph | Max {v_ego_mph.max():.1f} mph") + print(f" • Cruise Set Speed : Min {v_cruise_mph.min():.1f} mph | Avg {v_cruise_mph.mean():.1f} mph | Max {v_cruise_mph.max():.1f} mph") + print(f" • Standstill Time : {standstill_pct:.1f}% ({stops} full stop events)") + print(f" • Lead Presence : {100.0 * np.mean(out['lead_status']):.1f}% of route") + + print("\n[2. MODE INTENT & TRIGGER BREAKDOWN]") + print(f" • Logged (Actual) : Exp {100.0 * out['logged_exp'].mean():.1f}%") + print(f" • CEM (Conditional) : Exp {100.0 * out['cem_exp'].mean():.1f}% | {cem_switches} mode toggles") + cem_reasons = [] + for status_code, label in CE_LABELS.items(): + pct = 100.0 * np.mean((out["cem_status"] == status_code) & out["cem_exp"]) + if pct > 0.1: + cem_reasons.append(f"{label}: {pct:.1f}%") + if cem_reasons: + print(f" └─ CEM Active Triggers -> {', '.join(cem_reasons)}") + + print(f" • Chill Mode (CCM) : Chill {100.0 * out['chill_active'].mean():.1f}% | Exp {100.0 * out['ccm_exp'].mean():.1f}% | {ccm_switches} toggles") + ccm_reasons = [] + for status_code, label in CC_LABELS.items(): + pct = 100.0 * np.mean((out["ccm_status"] == status_code) & out["chill_active"]) + if pct > 0.1: + ccm_reasons.append(f"{label}: {pct:.1f}%") + if ccm_reasons: + print(f" └─ Chill Active Reasons -> {', '.join(ccm_reasons)}") + + auth = out["hem_authority"] + chill_dom = 100.0 * np.mean(auth < 0.35) + blended = 100.0 * np.mean((auth >= 0.35) & (auth <= 0.65)) + exp_dom = 100.0 * np.mean(auth > 0.65) + print(f" • HEM (Hybrid) : Mean Exp Authority {auth.mean():.2f}") + print(f" └─ Distribution -> Chill-dominant (<0.35): {chill_dom:.1f}% | Blended (0.35-0.65): {blended:.1f}% | Exp-dominant (>0.65): {exp_dom:.1f}%") + + print("\n[3. ACCELERATION ENVELOPE (m/s²)]") + print(f" {'Signal':<16} | {'Min (Max Brake)':<15} | {'Mean':<10} | {'Max (Max Throttle)':<18}") + print(" " + "-" * 66) + print(f" {'Logged aTarget':<16} | {out['logged_aTarget'].min():<15.2f} | {out['logged_aTarget'].mean():<10.2f} | {out['logged_aTarget'].max():<18.2f}") + print(f" {'Chill a':<16} | {out['a_chill'].min():<15.2f} | {out['a_chill'].mean():<10.2f} | {out['a_chill'].max():<18.2f}") + print(f" {'Exp a':<16} | {out['a_exp'].min():<15.2f} | {out['a_exp'].mean():<10.2f} | {out['a_exp'].max():<18.2f}") + print(f" {'HEM Fused a':<16} | {out['hem_a'].min():<15.2f} | {out['hem_a'].mean():<10.2f} | {out['hem_a'].max():<18.2f}") + + print("\n[4. KEY EVENT CHRONOLOGY (Sampled Significant Transitions)]") + print(f" {'Time (s)':<9} | {'vEgo':<8} | {'CEM Status':<12} | {'CCM':<7} | {'a_chill':<8} | {'a_exp':<8} | {'HEM a':<8} | {'Auth':<5} | Event Context") + print(" " + "-" * 95) + + step = max(1, n // 18) + sample_indices = set(range(0, n, step)) + + hard_brakes = np.where(out["hem_a"] < -1.2)[0] + if len(hard_brakes) > 0: + sample_indices.update(hard_brakes[::max(1, len(hard_brakes)//4)]) + + standstills = np.where(standstill_mask)[0] + if len(standstills) > 0: + sample_indices.update(standstills[::max(1, len(standstills)//3)]) + + sorted_indices = sorted(list(sample_indices))[:22] + + for idx in sorted_indices: + cur_t = t[idx] - t[0] + ego_mph = v_ego_mph[idx] + cem_st = CE_LABELS.get(out["cem_status"][idx], "OFF") if out["cem_exp"][idx] else "CHILL" + ccm_st = "CHILL" if out["chill_active"][idx] else "EXP" + ac = out["a_chill"][idx] + ae = out["a_exp"][idx] + ah = out["hem_a"][idx] + au = out["hem_authority"][idx] + + ctx = [] + if out["lead_status"][idx]: + ctx.append(f"Lead {out['lead_dRel'][idx]:.0f}m") + if ego_mph < 1.0: + ctx.append("Standstill") + elif ah < -1.0: + ctx.append("Braking") + elif ah > 0.8: + ctx.append("Accelerating") + else: + ctx.append("Cruising") + + if abs(ac - ae) > 1.0: + ctx.append("Disagreement") + + context_str = ", ".join(ctx) + print(f" {cur_t:<9.1f} | {ego_mph:<5.1f}mph | {cem_st:<12} | {ccm_st:<7} | {ac:<8.2f} | {ae:<8.2f} | {ah:<8.2f} | {au:<5.2f} | {context_str}") + + print("=" * 78 + "\n") + + +def plot_results(out, toggles, args): + import matplotlib + if not args.show: + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + t = out["t"] + n = len(t) + + fig, (ax_speed, ax_mode, ax_accel) = plt.subplots( + 3, 1, figsize=(14, 10), sharex=True, + gridspec_kw={"height_ratios": [2, 3, 2]}, + ) + fig.suptitle(f"Mode scenario estimate {args.route} seg {args.segment}", fontsize=11) + + ax_speed.plot(t, out["v_ego"] * CV.MS_TO_MPH, color="black", lw=1.4, label="v_ego") + ax_speed.plot(t, out["v_cruise"] * CV.MS_TO_MPH, color="tab:blue", lw=1.0, ls="--", label="v_cruise") + ax_speed.plot(t, out["model_v0"] * CV.MS_TO_MPH, color="tab:green", lw=1.0, alpha=0.7, label="model v[0]") + ax_speed.set_ylabel("mph") + ax_speed.legend(loc="upper right", fontsize=8, ncol=3) + ax_speed.grid(alpha=0.3) + + rows = [ + ("Exp (always)", out["t"], np.ones(n, dtype=bool), "tab:blue", 3.0), + ("CEM", out["t"], out["cem_exp"], "tab:orange", 2.0), + ("Chill", out["t"], out["chill_active"], "tab:green", 1.0), + ("Logged", out["t"], out["logged_exp"], "tab:gray", 0.0), + ] + for label, tt, active, color, ycenter in rows: + ax_mode.fill_between(tt, ycenter - 0.28, ycenter + 0.28, where=active, + step="post", color=color, alpha=0.55, edgecolor="none") + + cem_spans = intervals_for(t, [out["cem_status"][i] if out["cem_exp"][i] else 0 for i in range(n)]) + for a, b, k in cem_spans: + if b - a > 0.8 and k in CE_LABELS: + ax_mode.text((a + b) / 2, 2.0 + 0.34, CE_LABELS[k], ha="center", fontsize=7, color="tab:orange") + cc_spans = intervals_for(t, [out["ccm_status"][i] if out["chill_active"][i] else 0 for i in range(n)]) + for a, b, k in cc_spans: + if b - a > 0.8 and k in CC_LABELS: + ax_mode.text((a + b) / 2, 1.0 + 0.34, CC_LABELS[k], ha="center", fontsize=7, color="tab:green") + + ax_hem = ax_mode.twinx() + ax_hem.plot(t, out["hem_authority"], color="tab:cyan", lw=1.2, label="HEM exp-authority") + ax_hem.set_ylim(0, 1) + ax_hem.set_ylabel("HEM exp authority", color="tab:cyan", fontsize=8) + ax_hem.tick_params(axis="y", labelcolor="tab:cyan", labelsize=7) + + ax_mode.set_yticks([3.0, 2.0, 1.0, 0.0]) + ax_mode.set_yticklabels(["Exp", "CEM", "Chill", "Logged"]) + ax_mode.set_ylim(-0.6, 3.6) + ax_mode.grid(axis="y", alpha=0.3) + + ax_accel.plot(t, out["logged_aTarget"], color="tab:gray", lw=1.2, label="logged aTarget") + ax_accel.plot(t, out["a_chill"], color="tab:blue", lw=1.0, ls="--", label="chill (ACC) a") + ax_accel.plot(t, out["a_exp"], color="tab:orange", lw=1.0, ls=":", label="exp (model) a") + ax_accel.plot(t, out["hem_a"], color="tab:cyan", lw=1.4, label="HEM fused a") + ax_accel.set_ylabel("accel (m/s²)") + ax_accel.set_xlabel("time (s, segment-relative)") + ax_accel.legend(loc="upper right", fontsize=8, ncol=2) + ax_accel.grid(alpha=0.3) + + fig.tight_layout() + if args.out: + out_path = Path(args.out) + out_path.parent.mkdir(parents=True, exist_ok=True) + fig.savefig(out_path, dpi=150) + print(f"Saved graph to {out_path}") + if args.show: + plt.show() + plt.close(fig) + + +def main(argv=None): + parser = argparse.ArgumentParser( + description="Estimate CEM/Chill/HEM/Exp mode intent from a logged route and plot a comparison graph.") + parser.add_argument("route", help="route (e.g. dongle|2023-07-27--13-01-19 or dongle/2023-07-27--13-01-19/0)") + parser.add_argument("--segment", type=int, default=0, help="segment index (default 0)") + parser.add_argument("--data_dir", help="local directory containing route files") + parser.add_argument("--start", type=float, default=None, help="start seconds (segment-relative)") + parser.add_argument("--end", type=float, default=None, help="end seconds (segment-relative)") + parser.add_argument("--out", default="mode_sim.png", help="output PNG path") + parser.add_argument("--show", action="store_true", help="show the plot window instead of saving only") + parser.add_argument("--csv", default=None, help="optional CSV output of the time series") + parser.add_argument("--set", action="append", default=[], metavar="KEY=value", + help="override a mode toggle, e.g. --set CESpeed=20 --set CEModelStopTime=3.0 --set CCMLead=false") + args = parser.parse_args(argv) + + overrides = parse_toggle_overrides(args.set) + defaults = dict(TOGGLE_DEFAULTS) + defaults.update(overrides) + toggles = SimpleNamespace(**defaults) + for key, value in overrides.items(): + print(f"toggle override: {key} = {value}") + + identifiers = resolve_segment_identifier(args.route, args.segment, args.data_dir) + if not identifiers: + print(f"No segment data found for {args.route} seg {args.segment}", file=sys.stderr) + return 1 + print(f"Loading: {identifiers}") + bufs = load_buffers(identifiers, SERVICES) + if not any(bufs[s] for s in SERVICES): + print("No messages loaded.", file=sys.stderr) + return 1 + + all_min = min(t for s in SERVICES for t, _ in bufs[s]) + all_max = max(t for s in SERVICES for t, _ in bufs[s]) + start = all_min if args.start is None else all_min + args.start + end = all_max if args.end is None else min(all_max, all_min + args.end) + if end - start < DT_MDL: + print(f"Empty time window [{start:.1f}, {end:.1f}]. Check --start/--end.", file=sys.stderr) + return 1 + grid = np.arange(start, end, DT_MDL) + print(f"Window: {start - all_min:.1f}s -> {end - all_min:.1f}s ({len(grid)} frames at {1 / DT_MDL:.0f} Hz)") + + out = run_simulation(grid, bufs, toggles) + + if args.csv: + import csv + csv_path = Path(args.csv) + csv_path.parent.mkdir(parents=True, exist_ok=True) + with open(csv_path, "w", newline="") as f: + writer = csv.writer(f) + writer.writerow(["t", "v_ego", "v_cruise", "model_v0", "cem_exp", "cem_status", + "ccm_exp", "ccm_status", "chill_active", "logged_exp", "logged_aTarget", + "a_chill", "a_exp", "hem_a", "hem_authority"]) + for i in range(len(out["t"])): + writer.writerow([out["t"][i], out["v_ego"][i], out["v_cruise"][i], out["model_v0"][i], + int(out["cem_exp"][i]), out["cem_status"][i], + int(out["ccm_exp"][i]), out["ccm_status"][i], int(out["chill_active"][i]), + int(out["logged_exp"][i]), out["logged_aTarget"][i], + out["a_chill"][i], out["a_exp"][i], out["hem_a"][i], out["hem_authority"][i]]) + print(f"Saved CSV to {csv_path}") + + print_summary(out) + plot_results(out, toggles, args) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) \ No newline at end of file