vision speed scheduler

2026-04-06 18:57:38 -05:00
parent 3aa6a8ce52
commit eb04ec95bf
6 changed files with 15 additions and 87 deletions
@@ -21,7 +21,6 @@ from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose

 from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles
-from openpilot.starpilot.common.testing_grounds import testing_ground
 from openpilot.starpilot.controls.lib.neural_network_feedforward import LatControlNNFF

 State = log.SelfdriveState.OpenpilotState
@@ -132,14 +131,6 @@ class Controls:

    # accel PID loop
    pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
-    if testing_ground.use_2:
-      # Keep the actuator loop inside the planner's envelope so Eco/custom accel caps
-      # constrain the real GM output instead of only the planner target.
-      pid_accel_limits = [
-        max(pid_accel_limits[0], self.sm['starpilotPlan'].minAcceleration),
-        min(pid_accel_limits[1], self.sm['starpilotPlan'].maxAcceleration, self.starpilot_toggles.max_desired_acceleration),
-      ]
-      pid_accel_limits[1] = max(pid_accel_limits[0], pid_accel_limits[1])
    self.LoC.experimental_mode = bool(self.sm['selfdriveState'].experimentalMode)
    actuators.accel = float(min(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits, self.starpilot_toggles), self.starpilot_toggles.max_desired_acceleration))

@@ -13,14 +13,11 @@ from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.simple_kalman import KF1D
 from openpilot.selfdrive.controls.lib.desire_helper import LaneChangeDirection, LaneChangeState
-
-from openpilot.starpilot.common.testing_grounds import testing_ground
 from openpilot.starpilot.common.starpilot_variables import THRESHOLD, get_starpilot_toggles


 # Default lead acceleration decay set to 50% at 1s
 _LEAD_ACCEL_TAU = 0.6
-RADAR_ONLY_FAR_LEAD_MIN_DISTANCE = 35.0  # m

 # radar tracks
 SPEED, ACCEL = 0, 1     # Kalman filter states enum
@@ -222,10 +219,7 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn
        lead_dict = closest_track.get_RadarState()

    if not lead_dict['status'] and len(tracks) > 0:
-      radar_only_far_lead_min_distance = RADAR_ONLY_FAR_LEAD_MIN_DISTANCE if testing_ground.use_2 else 0.0
-      far_lead_tracks = [c for c in tracks.values()
-                         if c.dRel >= radar_only_far_lead_min_distance and
-                         c.potential_far_lead(standstill, model_data) and c.radarfulFilter.x >= THRESHOLD]
+      far_lead_tracks = [c for c in tracks.values() if c.potential_far_lead(standstill, model_data) and c.radarfulFilter.x >= THRESHOLD]
      if len(far_lead_tracks) > 0:
        closest_track = min(far_lead_tracks, key=lambda c: c.dRel)
        lead_dict = closest_track.get_RadarState()
@@ -1,67 +0,0 @@
-from types import SimpleNamespace
-from unittest.mock import patch
-
-from openpilot.selfdrive.controls import radard
-
-
-def make_model_data():
-  lane_lines = [
-    SimpleNamespace(x=[0.0, 150.0], y=[-3.5, -3.5]),
-    SimpleNamespace(x=[0.0, 150.0], y=[-1.8, -1.8]),
-    SimpleNamespace(x=[0.0, 150.0], y=[1.8, 1.8]),
-    SimpleNamespace(x=[0.0, 150.0], y=[3.5, 3.5]),
-  ]
-  meta = SimpleNamespace(
-    laneChangeState=radard.LaneChangeState.off,
-    laneChangeDirection=radard.LaneChangeDirection.none,
-  )
-  return SimpleNamespace(laneLines=lane_lines, meta=meta)
-
-
-def make_track(d_rel):
-  track = radard.Track(123, 20.0, radard.KalmanParams(0.05))
-  track.update(d_rel=d_rel, y_rel=0.0, v_rel=0.0, v_lead=20.0, measured=1.0)
-  return track
-
-
-def make_radar_only_lead(tracks):
-  return radard.get_lead(
-    v_ego=20.0,
-    ready=False,
-    tracks=tracks,
-    lead_msg=SimpleNamespace(prob=0.0),
-    model_v_ego=20.0,
-    model_data=make_model_data(),
-    standstill=False,
-    starpilot_plan=SimpleNamespace(increasedStoppedDistance=0.0),
-    starpilot_toggles=SimpleNamespace(lead_detection_probability=0.35, human_lane_changes=False),
-  )
-
-
-def test_close_radar_only_far_lead_is_blocked_in_testing_ground_2():
-  track = make_track(5.0)
-
-  with patch.object(radard, "testing_ground", SimpleNamespace(use_2=False)):
-    lead = {"status": False}
-    for _ in range(20):
-      lead = make_radar_only_lead({track.identifier: track})
-    assert lead["status"]
-    assert lead["radarTrackId"] == track.identifier
-
-  track = make_track(5.0)
-  with patch.object(radard, "testing_ground", SimpleNamespace(use_2=True)):
-    lead = {"status": False}
-    for _ in range(20):
-      lead = make_radar_only_lead({track.identifier: track})
-    assert not lead["status"]
-
-
-def test_far_radar_only_lead_survives_testing_ground_2():
-  track = make_track(80.0)
-
-  with patch.object(radard, "testing_ground", SimpleNamespace(use_2=True)):
-    lead = {"status": False}
-    for _ in range(20):
-      lead = make_radar_only_lead({track.identifier: track})
-    assert lead["status"]
-    assert lead["radarTrackId"] == track.identifier
@@ -41,9 +41,8 @@ TESTING_GROUNDS_SLOT_DEFINITIONS = (
  {
    "id": TESTING_GROUND_2,
    "name": "volt test tune",
-    "description": "A/B sandbox for planner-aware GM longitudinal limits and a radar-only far-lead distance guard.",
-    "aLabel": "A - Current behavior",
-    "bLabel": "B - Clamp + guard",
+    "description": "Volt longitudinal tuning sandbox.",
+    "aLabel": "A - Installed tune",
  },
  {
    "id": TESTING_GROUND_3,
@@ -13,6 +13,8 @@ import cv2
 import numpy as np

 from openpilot.common.constants import CV
+from openpilot.common.realtime import set_core_affinity
+from openpilot.system.hardware import PC

 INFERENCE_INTERVAL = 0.2
 FOLLOWUP_INFERENCE_INTERVAL = 0.1
@@ -161,6 +163,7 @@ SCHOOL_ZONE_SHORT_CIRCUIT_CONFIDENCE = 0.78
 DEBUG_BASE_DIR = Path("/data/media/0/vision_speed_limit_debug")
 DEBUG_CAPTURE_DIRNAME = "captures"
 SNAPSHOT_JPEG_QUALITY = 85
+SPEED_LIMIT_VISION_AFFINITY_CORES = [0, 1, 2, 3]


@dataclass
@@ -1783,6 +1786,14 @@ class SpeedLimitVisionDaemon:


 def main():
+  # Keep this best-effort helper off the critical control/model/camera cores.
+  if not PC:
+    set_core_affinity(SPEED_LIMIT_VISION_AFFINITY_CORES)
+
+  # OpenCV may otherwise fan out across many worker threads and starve more
+  # important daemons during detection bursts.
+  cv2.setNumThreads(1)
+
  daemon = SpeedLimitVisionDaemon()
  daemon.run()

@@ -144,7 +144,7 @@ procs += [
  PythonProcess("the_pond", "starpilot.system.the_pond.the_pond", always_run, nice=19),
  PythonProcess("galaxy", "starpilot.system.galaxy.galaxy", always_run, nice=19),
  PythonProcess("speed_limit_filler", "starpilot.system.speed_limit_filler", run_speed_limit_filler),
-  PythonProcess("speed_limit_vision", "starpilot.system.speed_limit_vision", run_speed_limit_vision),
+  PythonProcess("speed_limit_vision", "starpilot.system.speed_limit_vision", run_speed_limit_vision, nice=19),
 ]

 managed_processes = {p.name: p for p in procs}