planner

selfdrive/controls/lib/longitudinal_planner.py

@@ -23,6 +23,9 @@ A_CRUISE_MAX_VALS = [1.125, 1.125, 1.125, 1.125, 1.25, 1.25, 1.5]
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 ALLOW_THROTTLE_THRESHOLD = 0.4
 MIN_ALLOW_THROTTLE_SPEED = 2.5
+RAW_LEAD_SAFETY_MIN_CLOSING_SPEED = 0.5
+RAW_LEAD_SAFETY_TTC = 7.0
+RAW_LEAD_SAFETY_DISTANCE = 40.0
 
 # Uncertainty-based filter disable thresholds
 UNCERT_SLOPE_TRIG = 0.12  # per second
@@ -237,6 +240,21 @@ class LongitudinalPlanner:
 
     return max(accel_min, -required_decel)
 
+  @staticmethod
+  def raw_close_lead_needs_control(lead, v_ego):
+    if lead is None or not lead.status:
+      return False
+
+    closing_speed = float(v_ego - lead.vLead)
+    lead_braking = float(lead.aLeadK) < -0.5
+    if closing_speed <= RAW_LEAD_SAFETY_MIN_CLOSING_SPEED and not lead_braking:
+      return False
+
+    d_rel = max(float(lead.dRel), 0.0)
+    dynamic_distance = max(RAW_LEAD_SAFETY_DISTANCE, 3.0 * float(v_ego))
+    ttc = d_rel / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf")
+    return d_rel < dynamic_distance and (ttc < RAW_LEAD_SAFETY_TTC or lead_braking)
+
   def update(self, sm, starpilot_toggles):
     self.generation = getattr(starpilot_toggles, "model_version", None)
     self.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
@@ -305,6 +323,10 @@ class LongitudinalPlanner:
     tracking_lead = bool(sm['starpilotPlan'].trackingLead)
     self.lead_one = sm['radarState'].leadOne
     self.lead_two = sm['radarState'].leadTwo
+    raw_close_lead_control = any(self.raw_close_lead_needs_control(lead, v_ego) for lead in (self.lead_one, self.lead_two))
+    # StarPilot trackingLead is debounce/model-length based. Keep a raw close-lead
+    # safety path so ACC/chill does not ignore a visible lead during that debounce.
+    lead_control_active = tracking_lead or raw_close_lead_control
 
     lead_dist = self.lead_one.dRel if self.lead_one.status else 50.0
 
@@ -423,7 +445,7 @@ class LongitudinalPlanner:
       self.mpc.mode = dec_mpc_mode
     self.mpc.update(sm['radarState'], v_cruise, x, v, a, j,
                     sm['starpilotPlan'].dangerFactor, sm['starpilotPlan'].tFollow,
-                    personality=personality, tracking_lead=tracking_lead)
+                    personality=personality, tracking_lead=lead_control_active)
 
     self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
@@ -494,7 +516,7 @@ class LongitudinalPlanner:
     output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0]
 
     close_lead_caps = []
-    if tracking_lead:
+    if lead_control_active:
       for lead in (self.lead_one, self.lead_two):
         cap = self.get_close_lead_brake_cap(lead, v_ego, output_accel_min)
         if cap is not None:
@@ -504,13 +526,13 @@ class LongitudinalPlanner:
       self.a_desired = min(self.a_desired, close_lead_brake_cap)
       output_a_target = min(output_a_target, close_lead_brake_cap)
 
-    if tracking_lead and sm['carState'].standstill:
+    if lead_control_active and sm['carState'].standstill:
       moving_leads = [lead for lead in (self.lead_one, self.lead_two)
                       if lead.status and lead.vLead > 0.0 and lead.dRel >= STOP_DISTANCE - 0.5]
       if moving_leads:
         output_a_target = max(output_a_target, 0.3)
 
-    if tracking_lead and np.isfinite(v_cruise) and any(lead.status for lead in (self.lead_one, self.lead_two)):
+    if lead_control_active and np.isfinite(v_cruise) and any(lead.status for lead in (self.lead_one, self.lead_two)):
       # Keep follow/catchup behavior from pulling past the cruise target. Using the
       # same action horizon as the planner preserves normal accel farther below set speed.
       cruise_accel_cap = (v_cruise - v_ego + 0.01) / max(action_t, self.dt)

selfdrive/controls/tests/test_longcontrol.py

@@ -135,7 +135,7 @@ def test_volt_testing_ground_handoff_freezes_integrator(monkeypatch):
   CP.longitudinalTuning.kiBP = [0.0]
   CP.longitudinalTuning.kiV = [0.03]
 
-  monkeypatch.setattr(longcontrol.testing_ground, "use_2", True, raising=False)
+  monkeypatch.setattr(longcontrol, "testing_ground", SimpleNamespace(use_2=True))
 
   lc = LongControl(CP)
   freeze = lc._get_pedal_long_freeze(a_target=0.7, error=0.7, v_ego=8.0, accel_limits=(-3.0, 2.0))
@@ -154,7 +154,7 @@ def test_non_interceptor_volt_testing_ground_handoff_freezes_integrator(monkeypa
   CP.longitudinalTuning.kiBP = [0.0]
   CP.longitudinalTuning.kiV = [0.03]
 
-  monkeypatch.setattr(longcontrol.testing_ground, "use_2", True, raising=False)
+  monkeypatch.setattr(longcontrol, "testing_ground", SimpleNamespace(use_2=True))
 
   lc = LongControl(CP)
   freeze = lc._get_pedal_long_freeze(a_target=0.7, error=0.7, v_ego=8.0, accel_limits=(-3.0, 2.0))

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -1,5 +1,8 @@
+import sys
+import types
 from types import SimpleNamespace
 
+import numpy as np
 import pytest
 
 from cereal import log
@@ -7,16 +10,16 @@ from opendbc.car.honda.interface import CarInterface
 from opendbc.car.honda.values import CAR
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_vehicle_min_accel
-from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 
 
-def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0):
+def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead: float = 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 = 0.0
+  lead.aLeadK = a_lead
   lead.vRel = 0.0
   lead.aRel = 0.0
   lead.modelProb = 0.0
@@ -48,7 +51,8 @@ def make_model(v_ego: float, desired_accel: float):
   return model
 
 
-def make_sm(v_ego: float, desired_accel: float, min_accel: float):
+def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimental_mode: bool = True,
+            tracking_lead: bool = False, lead_one=None, lead_two=None):
   return {
     "carControl": SimpleNamespace(orientationNED=[0.0, 0.0, 0.0]),
     "carState": SimpleNamespace(
@@ -66,16 +70,16 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float):
     "liveParameters": SimpleNamespace(angleOffsetDeg=0.0),
     "modelV2": make_model(v_ego, desired_accel),
     "radarState": SimpleNamespace(
-      leadOne=make_lead(status=False),
-      leadTwo=make_lead(status=False),
+      leadOne=lead_one if lead_one is not None else make_lead(status=False),
+      leadTwo=lead_two if lead_two is not None else make_lead(status=False),
     ),
-    "selfdriveState": SimpleNamespace(enabled=True, experimentalMode=True, personality=0),
+    "selfdriveState": SimpleNamespace(enabled=True, experimentalMode=experimental_mode, personality=0),
     "starpilotPlan": SimpleNamespace(
       vCruise=v_ego + 5.0,
       minAcceleration=min_accel,
       maxAcceleration=2.0,
       disableThrottle=False,
-      trackingLead=False,
+      trackingLead=tracking_lead,
       accelerationJerk=5.0,
       dangerJerk=5.0,
       speedJerk=5.0,
@@ -86,18 +90,19 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float):
   }
 
 
-def make_toggles():
+def make_toggles(model_version: str = "v11"):
   return SimpleNamespace(
     taco_tune=False,
     classic_model=False,
     tinygrad_model=True,
-    model_version="v11",
+    model_version=model_version,
     stop_distance=6.0,
     vEgoStopping=0.5,
   )
 
 
-def test_experimental_mlsim_uses_vehicle_min_accel_floor():
+@pytest.mark.parametrize("model_version", ["v11", "v12"])
+def test_experimental_mlsim_uses_vehicle_min_accel_floor(model_version):
   v_ego = 18.0
   desired_accel = -1.0
   comfort_min_accel = -0.5
@@ -109,9 +114,80 @@ def test_experimental_mlsim_uses_vehicle_min_accel_floor():
   vehicle_min_accel = get_vehicle_min_accel(CP, v_ego)
   assert vehicle_min_accel < comfort_min_accel
 
-  planner.update(sm, make_toggles())
+  planner.update(sm, make_toggles(model_version))
 
   assert planner.mode == "blended"
   assert planner.mlsim
   assert planner.output_a_target == pytest.approx(desired_accel, abs=1e-3)
   assert planner.output_a_target < comfort_min_accel
+
+
+@pytest.mark.parametrize("model_version", ["v11", "v12"])
+def test_acc_mode_uses_close_raw_lead_when_tracking_lead_is_debounced(model_version):
+  v_ego = 5.0
+
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  sm = make_sm(
+    v_ego,
+    desired_accel=-0.6,
+    min_accel=-1.0,
+    experimental_mode=False,
+    tracking_lead=False,
+    lead_one=make_lead(status=True, d_rel=24.0, v_lead=0.3),
+  )
+  sm["starpilotPlan"].vCruise = v_ego + 12.0
+
+  planner.update(sm, make_toggles(model_version))
+
+  assert planner.mode == "acc"
+  assert planner.raw_close_lead_needs_control(sm["radarState"].leadOne, v_ego)
+  assert planner.output_a_target == pytest.approx(
+    planner.get_close_lead_brake_cap(sm["radarState"].leadOne, v_ego, sm["starpilotPlan"].minAcceleration)
+  )
+
+
+def test_modeld_action_passes_tomb_raider_longitudinal_params(monkeypatch):
+  monkeypatch.setenv("DEBUG", "0")
+  fake_commonmodel = types.ModuleType("openpilot.selfdrive.modeld.models.commonmodel_pyx")
+  fake_commonmodel.DrivingModelFrame = object
+  fake_commonmodel.CLContext = object
+  monkeypatch.setitem(sys.modules, fake_commonmodel.__name__, fake_commonmodel)
+
+  from openpilot.selfdrive.modeld import modeld
+
+  captured = {}
+
+  def fake_get_accel_from_plan(speeds, accels, t_idxs, *, action_t, vEgoStopping):
+    captured["speeds"] = speeds
+    captured["accels"] = accels
+    captured["t_idxs"] = t_idxs
+    captured["action_t"] = action_t
+    captured["vEgoStopping"] = vEgoStopping
+    return 0.4, True
+
+  monkeypatch.setattr(modeld, "get_accel_from_plan_tomb_raider", fake_get_accel_from_plan)
+
+  plan = np.zeros((1, ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH), dtype=np.float32)
+  plan[0, :, Plan.VELOCITY] = 3.0
+  plan[0, :, Plan.ACCELERATION] = -0.1
+  prev_action = log.ModelDataV2.Action.new_message()
+  toggles = SimpleNamespace(vEgoStopping=0.42)
+
+  action = modeld.get_action_from_model(
+    {"plan": plan},
+    prev_action,
+    lat_action_t=0.2,
+    long_action_t=0.73,
+    v_ego=5.0,
+    mlsim=True,
+    is_v9=True,
+    starpilot_toggles=toggles,
+  )
+
+  assert captured["action_t"] == pytest.approx(0.73)
+  assert captured["vEgoStopping"] == pytest.approx(0.42)
+  assert list(captured["t_idxs"]) == ModelConstants.T_IDXS
+  np.testing.assert_allclose(captured["speeds"], 3.0)
+  np.testing.assert_allclose(captured["accels"], -0.1)
+  assert action.shouldStop

selfdrive/modeld/modeld.py

@@ -72,10 +72,12 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
       recovery_power = getattr(starpilot_toggles, "recovery_power", 1.0)
       plan = plan + recovery_power * model_output['planplus'][0]
       cloudlog.error(f"planplus applied: shape {model_output['planplus'].shape}, RECOVERY_POWER {recovery_power}")
+    v_ego_stopping = getattr(starpilot_toggles, "vEgoStopping", 0.3)
     desired_accel, should_stop = get_accel_from_plan_tomb_raider(plan[:,Plan.VELOCITY][:,0],
                                                                  plan[:,Plan.ACCELERATION][:,0],
                                                                  ModelConstants.T_IDXS,
-                                                                 action_t=long_action_t)
+                                                                 action_t=long_action_t,
+                                                                 vEgoStopping=v_ego_stopping)
     desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
 
     if is_v9:

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -1,4 +1,8 @@
 #!/usr/bin/env python3
+import contextlib
+import gc
+import os
+import sys
 import time
 import numpy as np
 from types import SimpleNamespace
@@ -15,18 +19,55 @@ from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
 class Plant:
   messaging_initialized = False
+  messaging_prefix = None
+
+  @staticmethod
+  @contextlib.contextmanager
+  def _messaging_socket_env():
+    prefix = os.environ.get("OPENPILOT_PREFIX")
+    if sys.platform != "darwin" or not prefix:
+      yield
+      return
+
+    old_namespace = os.environ.get("OPENPILOT_ZMQ_NAMESPACE")
+    del os.environ["OPENPILOT_PREFIX"]
+    os.environ["OPENPILOT_ZMQ_NAMESPACE"] = prefix
+    try:
+      yield
+    finally:
+      os.environ["OPENPILOT_PREFIX"] = prefix
+      if old_namespace is None:
+        os.environ.pop("OPENPILOT_ZMQ_NAMESPACE", None)
+      else:
+        os.environ["OPENPILOT_ZMQ_NAMESPACE"] = old_namespace
+
+  @staticmethod
+  def _clear_messaging_sockets():
+    for attr in ("radar", "controls_state", "selfdrive_state", "car_state", "plan"):
+      if hasattr(Plant, attr):
+        delattr(Plant, attr)
+    with Plant._messaging_socket_env():
+      messaging.reset_context()
+    gc.collect()
 
   def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0,
                enabled=True, only_lead2=False, only_radar=False, e2e=False, personality=0, force_decel=False):
     self.rate = 1. / DT_MDL
 
-    if not Plant.messaging_initialized:
-      Plant.radar = messaging.pub_sock('radarState')
-      Plant.controls_state = messaging.pub_sock('controlsState')
-      Plant.selfdrive_state = messaging.pub_sock('selfdriveState')
-      Plant.car_state = messaging.pub_sock('carState')
-      Plant.plan = messaging.sub_sock('longitudinalPlan')
-      Plant.messaging_initialized = True
+    current_prefix = os.environ.get("OPENPILOT_PREFIX")
+    if Plant.messaging_prefix != current_prefix:
+      Plant._clear_messaging_sockets()
+      Plant.messaging_initialized = False
+
+    with Plant._messaging_socket_env():
+      if not Plant.messaging_initialized:
+        Plant.radar = messaging.pub_sock('radarState')
+        Plant.controls_state = messaging.pub_sock('controlsState')
+        Plant.selfdrive_state = messaging.pub_sock('selfdriveState')
+        Plant.car_state = messaging.pub_sock('carState')
+        Plant.plan = messaging.sub_sock('longitudinalPlan')
+        Plant.messaging_initialized = True
+        Plant.messaging_prefix = current_prefix
 
     self.v_lead_prev = 0.0
 
@@ -48,7 +89,8 @@ class Plant:
     self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
     self.ts = 1. / self.rate
     time.sleep(0.1)
-    self.sm = messaging.SubMaster(['longitudinalPlan'])
+    with Plant._messaging_socket_env():
+      self.sm = messaging.SubMaster(['longitudinalPlan'])
 
     from opendbc.car.honda.values import CAR
     from opendbc.car.honda.interface import CarInterface
@@ -56,7 +98,9 @@ class Plant:
     self.planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.HONDA_CIVIC), init_v=self.speed)
     self.starpilot_toggles = SimpleNamespace(
       taco_tune=False,
-      model_version=None,
+      classic_model=False,
+      tinygrad_model=True,
+      model_version="v11",
       stop_distance=6.0,
       longitudinalActuatorDelay=0.2,
       vEgoStopping=0.5,