mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-30 20:53:42 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add HEM telemetry exporter and update forensic analysis tools

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