Enhance Hybrid Experimental Mode and HEM Telemetry Exporter with new features and optimizations

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

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

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

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()