Restore Dom low-speed vision stop buffer for HKG longitudinal

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -35,6 +35,7 @@ from openpilot.starpilot.longitudinal.force_stop import StopPlan
 from openpilot.starpilot.longitudinal.follow_jerk import FollowJerk
 from openpilot.starpilot.longitudinal.lead_takeoff import LeadTakeoff, project as project_takeoff
 from openpilot.starpilot.longitudinal.lead_behavior import adjust as adjust_lead_behavior
+from openpilot.starpilot.longitudinal.hkg_vision_stop import owner_for as vision_stop_owner
 from openpilot.starpilot.longitudinal.lead_approach import LeadApproach, LeadApproachKey, project as project_approach
 from openpilot.starpilot.lateral.lane_change_preferences import LaneChangePolicy
 
@@ -130,6 +131,7 @@ class LongitudinalPlanner:
     self.vehicle_cost_owner = planner_cost_owner(CP)
     self.lead_takeoff = LeadTakeoff()
     self.vehicle_far_follow = far_follow_owner(CP, dt)
+    self.vision_stop_buffer = vision_stop_owner(CP)
     self.preap_adaptive = preap_adaptive_owner(CP)
 
   def update(self, sm, *, cruise_ceiling: CruiseCeiling | None = None,
@@ -465,6 +467,11 @@ class LongitudinalPlanner:
         target=float(output_a_target), previous_target=float(a_prev), follow_seconds=float(self.mpc.params[0, 4]),
         blocked=bool(self.output_should_stop or self.fcw or self.force_stop_plan.forcing or
                      self.force_stop_plan.approach_distance_m > 0 or self.mpc.solution_status != 0))
+    if self.vision_stop_buffer is not None:
+      output_a_target, self.output_should_stop = self.vision_stop_buffer.sample(
+        sm, now_ns=sample_now_ns, drive_id=drive_id, target=float(output_a_target),
+        stopping=self.output_should_stop, accel_min=ACCEL_MIN,
+        active=profile_eligible and not reset_state and self.mpc.solution_status == 0)
     if preap_acceleration_cap is not None:
       output_a_target = min(output_a_target, preap_acceleration_cap)
     self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX)

openpilot/starpilot/longitudinal/hkg_vision_stop.py

@@ -0,0 +1,94 @@
+"""Dom's existing low-speed vision-lead stop buffer for active HKG longitudinal."""
+import math
+
+from opendbc.car.hyundai.values import CAR
+
+SOURCES = ('modelV2', 'carState', 'radarState', 'carControl', 'controlsState', 'selfdriveState')
+MAX_SOURCE_AGE_NS = 250_000_000
+MAX_MODEL_GAP_NS = 150_000_000
+MAX_EGO_SPEED = 6.5
+MAX_LEAD_SPEED = 3.25
+HOLD_MAX_LEAD_SPEED = 4.0
+MIN_MODEL_PROB = .9
+MIN_CLOSING_SPEED = .35
+MIN_HOLD_REL_SPEED = -.2
+BASE_BUFFER = 3.8
+EGO_GAIN = .8
+LEAD_GAIN = .25
+RELEASE_MARGIN = .9
+HOLD_NS = 800_000_000
+MIN_BRAKE = 1.25
+BRAKE_GAIN = .25
+
+
+def owner_for(cp):
+  try:
+    if (cp.brand == 'hyundai' and cp.carFingerprint in CAR.__members__ and cp.openpilotLongitudinalControl and
+        not cp.passive and not cp.dashcamOnly and not cp.notCar):
+      return VisionStopBuffer()
+  except (AttributeError, TypeError):
+    pass
+  return None
+
+
+class VisionStopBuffer:
+  def __init__(self):
+    self.reset()
+
+  def reset(self):
+    self.drive_id = 0
+    self.last_model_ns = 0
+    self.hold_until_ns = 0
+
+  def cap(self, lead, speed, accel_min, now_ns):
+    if not lead.present or lead.radar or lead.modelProb < MIN_MODEL_PROB:
+      return None, False
+    lead_speed = max(float(lead.vLead), 0.)
+    relative_speed = speed - lead_speed
+    entry = speed <= MAX_EGO_SPEED and lead_speed <= MAX_LEAD_SPEED and max(0., relative_speed) >= MIN_CLOSING_SPEED
+    hold = speed <= MAX_EGO_SPEED and lead_speed <= HOLD_MAX_LEAD_SPEED and relative_speed >= MIN_HOLD_REL_SPEED
+    buffer = max(3.2, BASE_BUFFER + EGO_GAIN * speed + LEAD_GAIN * lead_speed)
+    if entry and lead.dRel <= buffer:
+      self.hold_until_ns = now_ns + HOLD_NS
+    active = bool((entry and lead.dRel <= buffer) or
+                  (now_ns < self.hold_until_ns and hold and lead.dRel <= buffer + RELEASE_MARGIN))
+    return (max(accel_min, -(MIN_BRAKE + BRAKE_GAIN * speed)), True) if active else (None, False)
+
+  def sample(self, sm, *, now_ns, drive_id, target, stopping, accel_min, active):
+    try:
+      if type(drive_id) is not int or drive_id <= 0 or type(now_ns) is not int or now_ns <= drive_id:
+        self.reset()
+        return target, stopping
+      if drive_id != self.drive_id:
+        self.reset()
+        self.drive_id = drive_id
+      model_ns = sm.logMonoTime['modelV2']
+      car = sm['carState']
+      fresh = all(sm.valid[name] and sm.alive[name] and drive_id < sm.logMonoTime[name] <= now_ns and
+                  now_ns - sm.logMonoTime[name] <= MAX_SOURCE_AGE_NS for name in SOURCES)
+      if (not active or not fresh or model_ns <= self.last_model_ns or
+          not car.canValid or car.canTimeout or car.gasPressed or car.brakePressed or
+          not sm['carControl'].longActive or not sm['selfdriveState'].enabled or sm['controlsState'].forceDecel):
+        self.reset()
+        return target, stopping
+      speed = float(car.vEgo)
+      leads = (sm['radarState'].leadOne, sm['radarState'].leadTwo)
+      if (not all(math.isfinite(float(v)) for v in (speed, target, accel_min)) or speed < 0 or accel_min > 0 or
+          any(not all(math.isfinite(float(getattr(lead, field))) for field in ('vLead', 'dRel', 'modelProb')) or
+              lead.dRel < 0 or not 0 <= lead.modelProb <= 1 for lead in leads if lead.present)):
+        self.reset()
+        return target, stopping
+      if self.last_model_ns and model_ns - self.last_model_ns > MAX_MODEL_GAP_NS:
+        self.hold_until_ns = 0
+      self.last_model_ns = model_ns
+      cap = None
+      should_stop = stopping
+      for lead in leads:
+        value, stop = self.cap(lead, speed, accel_min, now_ns)
+        if value is not None:
+          cap = value if cap is None else min(cap, value)
+        should_stop |= stop
+      return min(target, cap) if cap is not None else target, should_stop
+    except (AttributeError, KeyError, TypeError, ValueError, OverflowError):
+      self.reset()
+      return target, stopping

openpilot/starpilot/longitudinal/tests/test_hkg_vision_stop.py

@@ -0,0 +1,171 @@
+"""Pinned Dom stop-buffer comparison and actual native planner publication."""
+import hashlib
+import itertools
+import json
+from pathlib import Path
+from types import SimpleNamespace as NS
+from unittest.mock import patch
+
+import pytest
+from opendbc.car import gen_empty_fingerprint
+from opendbc.car.hyundai.interface import CarInterface
+from opendbc.car.hyundai.values import CAR
+from openpilot.cereal import messaging
+from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+from openpilot.starpilot.longitudinal.hkg_vision_stop import VisionStopBuffer, owner_for
+from openpilot.starpilot.longitudinal.tests.test_lead_approach_planner import Frame, DRIVE_NS
+
+DATA = json.loads((Path(__file__).parent / 'testdata/hkg_dom_stop_buffer.json').read_text())
+
+
+def lead(speed=.2, distance=2., probability=.999, radar=False):
+  return NS(status=True, present=True, radar=radar, modelProb=probability, vLead=speed, dRel=distance)
+
+
+def dom_reference(clock):
+  source = DATA['sourceFunction']
+  assert hashlib.sha256(source.encode()).hexdigest() == DATA['sourceSha256']
+  namespace = dict(DATA['constants'], time=NS(monotonic=lambda: clock[0] / 1e9),
+                   get_vision_low_speed_stop_buffer_lead_speed_limits=lambda cp, a, b: (a, b))
+  exec(compile(source, '<pinned Dom stop buffer>', 'exec'), namespace)
+  return namespace['get_vision_low_speed_stop_buffer_cap']
+
+
+def test_exact_source_boundaries_without_a_new_calibration():
+  clock = [10_000_000_000]
+  reference = dom_reference(clock)
+  for speed, lead_speed, distance, probability, radar in itertools.product(
+      (0., .349, .35, .8, 3., 6.5, 6.50001), (0., .2, 3.25, 3.25001, 4.),
+      (1., 3.2, 5., 8., 10.), (.89999, .9, .9999), (False, True)):
+    original = NS(CP=NS(), vision_low_speed_stop_hold_until=0.)
+    current = VisionStopBuffer()
+    obj = lead(lead_speed, distance, probability, radar)
+    assert current.cap(obj, speed, -3.5, clock[0]) == reference(original, obj, speed, -3.5)
+
+
+def test_recorded_niro_frames_keep_dom_buffer_and_original_braking_bounds():
+  clock = [0]
+  reference = dom_reference(clock)
+  original = NS(CP=NS(), vision_low_speed_stop_hold_until=0.)
+  current = VisionStopBuffer()
+  changed = 0
+  for frame in DATA['frames']:
+    clock[0] = frame['ns']
+    for row in frame['leads']:
+      obj = lead(row['speed'], row['distance'], row['prob'])
+      cap, stopping = current.cap(obj, frame['speed'], -3.5, clock[0])
+      expected, expected_stop = reference(original, obj, frame['speed'], -3.5)
+      assert stopping == expected_stop
+      assert cap == expected
+      if cap is not None:
+        assert -3.5 <= min(frame['recordedTarget'], cap) <= frame['recordedTarget']
+        changed += cap < frame['recordedTarget']
+  assert changed > 0
+
+
+def params(identity=CAR.KIA_NIRO_EV, alpha=True):
+  return CarInterface.get_params(identity, gen_empty_fingerprint(), [], alpha, False, False)
+
+
+def frame(index=0):
+  sm = Frame(index)
+  sm['carState'].vEgo = .8
+  sm['carState'].aEgo = -.08
+  sm['carState'].standstill = False
+  sm['selfdriveState'].experimentalMode = True
+  sm['modelV2'].action.shouldStop = False
+  sm['modelV2'].action.desiredAcceleration = 0.
+  sm['radarState'].leadTwo.present = False
+  obj = sm['radarState'].leadOne
+  obj.present, obj.radar, obj.modelProb = True, False, .99994
+  obj.dRel, obj.vLead, obj.vLeadK, obj.vRel, obj.aLeadK = 2., .2, .2, -.6, 0.
+  return sm
+
+
+def test_native_planner_publishes_original_stop_buffer_with_unchanged_limits():
+  cp = params()
+  before = cp.to_dict()
+  original, current = (LongitudinalPlanner(cp, init_v=.8) for _ in range(2))
+  original.vision_stop_buffer = None
+  changed = 0
+  for index in range(10):
+    sm = frame(index)
+    original.update(sm, now_ns=sm.now_ns, drive_id=DRIVE_NS)
+    current.update(sm, now_ns=sm.now_ns, drive_id=DRIVE_NS)
+    assert original.mpc.solution_status == current.mpc.solution_status == 0
+    assert -3.5 <= current.output_a_target <= original.output_a_target
+    assert current.output_should_stop
+    changed += current.output_a_target < original.output_a_target or not original.output_should_stop
+  assert changed > 0 and cp.to_dict() == before
+  class Published:
+    messages = {}
+    def send(self, service, message):
+      self.messages[service] = message.as_reader()
+  published = Published()
+  sm.all_checks = lambda services: True
+  create_message = messaging.new_message
+  with patch.object(messaging, 'new_message', side_effect=lambda service: create_message(service, logMonoTime=sm.now_ns)):
+    current.publish(sm, published)
+  plan = published.messages['longitudinalPlan']
+  assert plan.valid and plan.longitudinalPlan.shouldStop
+  assert plan.longitudinalPlan.aTarget == pytest.approx(current.output_a_target)
+
+
+@pytest.mark.parametrize('defect', ['gas', 'brake', 'stale', 'future', 'invalid', 'long_off', 'disabled', 'force', 'inactive'])
+def test_invalid_or_inactive_sources_do_not_create_a_braking_override(defect):
+  owner = VisionStopBuffer()
+  sm = frame()
+  if defect in ('gas', 'brake'):
+    setattr(sm['carState'], defect + 'Pressed', True)
+  elif defect == 'stale':
+    sm.logMonoTime['radarState'] = sm.now_ns - 250_000_001
+  elif defect == 'future':
+    sm.logMonoTime['modelV2'] = sm.now_ns + 1
+  elif defect == 'invalid':
+    sm.valid['radarState'] = False
+  elif defect == 'long_off':
+    sm['carControl'].longActive = False
+  elif defect == 'disabled':
+    sm['selfdriveState'].enabled = False
+  elif defect == 'force':
+    sm['controlsState'].forceDecel = True
+  assert owner.sample(sm, now_ns=sm.now_ns, drive_id=DRIVE_NS, target=-.3, stopping=False,
+                      accel_min=-3.5, active=defect != 'inactive') == (-.3, False)
+  assert owner.hold_until_ns == 0
+
+
+@pytest.mark.parametrize('defect', ['radar', 'low_prob', 'fast_lead', 'high_speed'])
+def test_ordinary_moving_or_radar_following_keeps_its_existing_target(defect):
+  owner = VisionStopBuffer()
+  sm = frame()
+  obj = sm['radarState'].leadOne
+  if defect == 'radar':
+    obj.radar = True
+  elif defect == 'low_prob':
+    obj.modelProb = .89
+  elif defect == 'fast_lead':
+    obj.vLead = 4.1
+  else:
+    sm['carState'].vEgo = 6.50001
+  assert owner.sample(sm, now_ns=sm.now_ns, drive_id=DRIVE_NS, target=-.3, stopping=False,
+                      accel_min=-3.5, active=True) == (-.3, False)
+
+
+def test_stronger_braking_and_existing_stop_intent_are_never_relaxed():
+  sm = frame()
+  owner = VisionStopBuffer()
+  assert owner.sample(sm, now_ns=sm.now_ns, drive_id=DRIVE_NS, target=-3.5, stopping=True,
+                      accel_min=-3.5, active=True) == (-3.5, True)
+  assert owner.sample(frame(1), now_ns=sm.now_ns, drive_id=DRIVE_NS, target=-3.5, stopping=True,
+                      accel_min=-3.5, active=False) == (-3.5, True)
+
+
+def test_stock_long_unknown_profiles_and_foreign_brands_do_not_gain_an_owner():
+  assert owner_for(params(alpha=False)) is None
+  cp = params()
+  assert isinstance(owner_for(cp), VisionStopBuffer)
+  for field, value in (('passive', True), ('dashcamOnly', True), ('notCar', True), ('brand', 'toyota'),
+                       ('carFingerprint', 'UNKNOWN_HYUNDAI')):
+    changed = cp.as_reader().as_builder()
+    setattr(changed, field, value)
+    assert owner_for(changed) is None

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "daf022a4b1a5fcd784781c61fd23b6434bf720ab0fb5560dfb991a3c76193400", "schema_version": 1}
+{"protected_inventory_sha256": "f247bbcb9bc0eb85aad99680801a8f09501f9b1ee6f8d7605cac11727e2d64a5", "schema_version": 1}