From 9563a0e09a72862d209eaa126dc8e91f4a529186 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 22 Apr 2026 08:43:15 -0500 Subject: [PATCH] planner --- .../controls/lib/longitudinal_planner.py | 30 +++++- selfdrive/controls/tests/test_longcontrol.py | 4 +- .../tests/test_longitudinal_planner.py | 100 +++++++++++++++--- selfdrive/modeld/modeld.py | 4 +- .../test/longitudinal_maneuvers/plant.py | 62 +++++++++-- 5 files changed, 172 insertions(+), 28 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 525a71910..fa34c28cb 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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) diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index 11633d7fb..a4ccb0ffd 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/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)) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index fa5480cad..b39cfb2ee 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/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 diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 279be2a05..ff90ad1c1 100755 --- a/selfdrive/modeld/modeld.py +++ b/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: diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index ddef99f76..1b688a1e9 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/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,