diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 3d9111bbf9..100a5b9a6c 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -2997,6 +2997,7 @@ class LongitudinalPlanner: 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 diff --git a/starpilot/controls/lib/hybrid_experimental_mode.py b/starpilot/controls/lib/hybrid_experimental_mode.py index 161b4bfa06..c307ca6bc2 100644 --- a/starpilot/controls/lib/hybrid_experimental_mode.py +++ b/starpilot/controls/lib/hybrid_experimental_mode.py @@ -8,16 +8,6 @@ def lerp(a: float, b: float, t: float) -> float: return float((1.0 - t) * a + t * b) -def sigmoid(x: float, k: float = 4.0, x0: float = 0.0) -> float: - """Smooth 0-to-1 activation curve.""" - z = np.clip(-k * (x - x0), -30.0, 30.0) - return float(1.0 / (1.0 + np.exp(z))) - - -def smooth_min(a: float, b: float, k: float = 6.0) -> float: - return lerp(b, a, sigmoid(b - a, k=k)) - - class HybridExperimentalMode: """ Final Arbitrator between Chill (MPC Cruise/Radar) and Exp (Vision E2E): @@ -52,9 +42,9 @@ class HybridExperimentalMode: self.VISION_BRAKE_SENSITIVITY = float(np.clip(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): + should_stop_exp=False, should_stop_chill=False, gas_pressed=False): - # Robustness: never let non-finite or corrupt inputs propagate into the blend. + #never let non-finite or corrupt inputs propagate into the fusion. if not np.isfinite(a_chill): a_chill = float(self.prev_a_target) if not np.isfinite(a_exp): @@ -79,13 +69,13 @@ class HybridExperimentalMode: lead_d = float(getattr(lead_one, "dRel", 150.0)) # 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) + 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 - # Do not latch horizon_stopping if the lead is actively pulling away or driver commands takeoff if is_departing: horizon_stopping = False else: @@ -115,12 +105,9 @@ class HybridExperimentalMode: else: self.w_vision = max(0.0, self.w_vision - 0.04) - # 5. Dual-Regime blend + # 5. Dual-Regime Fusion # Braking Regime: Pure vision braking when model demands it - if a_exp < 0.0: - a_brake_fused = min(a_chill, a_exp) - else: - a_brake_fused = min(a_chill, a_exp) + a_brake_fused = min(a_chill, a_exp) # 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) @@ -129,8 +116,6 @@ class HybridExperimentalMode: 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_out = lerp(a_throttle_fused, a_brake_fused, self.w_vision) @@ -138,7 +123,7 @@ class HybridExperimentalMode: # 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) + should_stop_fused = bool(should_stop_chill or ((should_stop_exp or standstill_intent) and not is_departing)) if self.record_diag: self.diag = { @@ -176,4 +161,4 @@ class HybridExperimentalMode: } self.prev_a_target = a_out - return a_out, should_stop_fused + return a_out, should_stop_fused \ No newline at end of file diff --git a/starpilot/controls/tests/test_hybrid_experimental_mode.py b/starpilot/controls/tests/test_hybrid_experimental_mode.py index d636cdb74b..8a90d6187e 100644 --- a/starpilot/controls/tests/test_hybrid_experimental_mode.py +++ b/starpilot/controls/tests/test_hybrid_experimental_mode.py @@ -5,8 +5,6 @@ import pytest from openpilot.starpilot.controls.lib.hybrid_experimental_mode import ( HybridExperimentalMode, lerp, - sigmoid, - smooth_min, ) @@ -40,14 +38,6 @@ def run(controller, *, v_ego=20.0, v_cruise=30.0, lead=None, model=None, a_chill 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_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_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) @@ -228,7 +218,7 @@ def test_lead_departure_releases_vision_latch(): 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) + 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 diff --git a/tools/replay/export_hem_telemetry.py b/tools/replay/export_hem_telemetry.py index 06f7a16cce..9bf704913f 100644 --- a/tools/replay/export_hem_telemetry.py +++ b/tools/replay/export_hem_telemetry.py @@ -2,7 +2,7 @@ """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). +capturing the raw model action intents, radar lead states, and planner controls. """ import os import sys @@ -23,21 +23,39 @@ ROUTES_TO_EXPORT = [ ("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 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} + """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 = {} @@ -69,6 +87,7 @@ def export_routes(): continue car_state_msgs = [] + radar_state_msgs = [] model_msgs = [] splan_msgs = [] lplan_msgs = [] @@ -82,6 +101,8 @@ def export_routes(): 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": @@ -99,6 +120,7 @@ def export_routes(): 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]) @@ -109,6 +131,7 @@ def export_routes(): 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) @@ -118,7 +141,6 @@ def export_routes(): if cs is None: continue - # Engagement & State flags enabled = False state = 0 active = False @@ -136,7 +158,6 @@ def export_routes(): 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) @@ -150,19 +171,36 @@ def export_routes(): 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 - # 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) + # 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)), - "a_exp": float(getattr(model_msg.action, "desiredAcceleration", 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, @@ -171,9 +209,6 @@ def export_routes(): "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), }) @@ -194,5 +229,6 @@ def export_routes(): 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