fix(long): stabilize throttle-intent coast gating

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -87,7 +87,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     v_ego = sm['carState'].vEgo
     v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
     v_cruise = v_cruise_kph * CV.KPH_TO_MS
-    if sm['controlsState'].forceDecel:
+    force_decel = sm['controlsState'].forceDecel
+    if force_decel:
       v_cruise = 0.0
 
     long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
@@ -100,7 +101,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     throttle_probs = sm['modelV2'].meta.disengagePredictions.gasPressProbs
     throttle_prob = throttle_probs[1] if len(throttle_probs) > 1 else 1.0
-    self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
+    self.allow_throttle = self.update_allow_throttle(throttle_prob, low_speed_override=v_ego <= MIN_ALLOW_THROTTLE_SPEED,
+                                                     threshold=ALLOW_THROTTLE_THRESHOLD)
 
     steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['vehicleParameters'].angleOffsetDeg
 
@@ -146,9 +148,15 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     is_e2e = self.is_e2e(sm)
 
     max_accel_override = self.get_max_accel_override(v_ego, is_e2e)
-    self.a_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego,
-                                     self.a_cruise, steer_angle_without_offset, self.CP, self.dt,
-                                     accel_coast, self.allow_throttle, max_accel_override)
+    a_cruise_prev = self.a_cruise
+    gated_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, a_cruise_prev, steer_angle_without_offset,
+                                    self.CP, self.dt, accel_coast, self.allow_throttle, max_accel_override)
+    ungated_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, a_cruise_prev, steer_angle_without_offset,
+                                      self.CP, self.dt, accel_coast, True, max_accel_override)
+    self.a_cruise = self.arbitrate_cruise_candidate(
+      sm, gated_cruise, ungated_cruise, output_a_target_mpc, self.mpc.source,
+      allow_throttle=self.allow_throttle, e2e=is_e2e, force_decel=force_decel,
+    )
     cruise_should_stop = should_stop(v_ego, self.a_cruise)
 
     candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
@@ -183,6 +191,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     longitudinalPlan.aTarget = float(self.output_a_target)
     longitudinalPlan.shouldStop = bool(self.output_should_stop)
     longitudinalPlan.allowBrake = True
+    # Raw model throttle intent used for path visualization; lead MPC can still request positive acceleration.
     longitudinalPlan.allowThrottle = bool(self.allow_throttle)
 
     pm.send('longitudinalPlan', plan_send)

openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -5,7 +5,9 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
 
-from openpilot.cereal import messaging, custom
+import math
+
+from openpilot.cereal import messaging, custom, log
 from opendbc.car import structs
 from openpilot.common.constants import CV
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
@@ -16,17 +18,20 @@ from openpilot.sunnypilot.selfdrive.controls.lib.lead_departure_controller impor
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver
+from openpilot.sunnypilot.selfdrive.controls.lib.throttle_intent_controller import ThrottleIntentController
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 from openpilot.sunnypilot.models.helpers import get_active_bundle
 
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
+MpcPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
 
 
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
     self.accel_controller = AccelController()
     self.accel_controller_active = False
+    self.throttle_intent_controller = ThrottleIntentController(mpc.dt)
     self.lead_departure_controller = LeadDepartureController(CP.openpilotLongitudinalControl and CP.autoResumeSng and not CP.notCar)
     self.events_sp = EventsSP()
     self.resolver = SpeedLimitResolver()
@@ -55,6 +60,25 @@ class LongitudinalPlannerSP:
       return None
     return self.accel_controller.get_max_accel(v_ego)
 
+  def update_allow_throttle(self, throttle_prob: float, low_speed_override: bool, threshold: float) -> bool:
+    return self.throttle_intent_controller.update(throttle_prob, low_speed_override=low_speed_override, threshold=threshold)
+
+  def _has_valid_selected_lead(self, sm: messaging.SubMaster, source: MpcPlanSource) -> bool:
+    radar_valid = sm.valid.get('radarState', False) and getattr(sm, 'alive', {}).get('radarState', False)
+    return radar_valid and ((source == MpcPlanSource.lead0 and sm['radarState'].leadOne.present) or
+                            (source == MpcPlanSource.lead1 and sm['radarState'].leadTwo.present))
+
+  def arbitrate_cruise_candidate(self, sm: messaging.SubMaster, gated: float, ungated: float,
+                                 mpc_accel: float, mpc_source: MpcPlanSource, *, allow_throttle: bool,
+                                 e2e: bool, force_decel: bool) -> float:
+    finite = all(math.isfinite(value) for value in (gated, ungated, mpc_accel))
+    coast_gate_changed_source = gated < mpc_accel <= ungated
+    if (finite and not allow_throttle and not e2e and not force_decel
+        and self._has_valid_selected_lead(sm, mpc_source) and coast_gate_changed_source):
+      return ungated
+
+    return gated
+
   def update_lead_departure(self, sm: messaging.SubMaster, a_target: float, should_stop: bool, reset: bool) -> bool:
     radar_valid = sm.valid.get('radarState', False) and getattr(sm, 'alive', {}).get('radarState', False)
     return self.lead_departure_controller.update(sm, self.mpc.source, a_target, should_stop, reset, radar_valid)

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_throttle_intent_arbitration.py

@@ -0,0 +1,115 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from types import SimpleNamespace
+from typing import cast
+
+from openpilot.cereal import messaging, log
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.controls.lib.longitudinal_planner import get_coast_accel, get_cruise_accel
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
+
+PlanSource = log.LongitudinalPlan.LongitudinalPlanSource
+
+
+class FakeSubMaster(dict):
+  def __init__(self, lead_one: bool = True, lead_two: bool = False, *, valid: bool = True, alive: bool = True):
+    super().__init__(radarState=SimpleNamespace(
+      leadOne=SimpleNamespace(present=lead_one),
+      leadTwo=SimpleNamespace(present=lead_two),
+    ))
+    self.valid = {'radarState': valid}
+    self.alive = {'radarState': alive}
+
+
+def arbitrate(sm, mpc_accel=0.19, gated_cruise=-0.26, ungated_cruise=0.5, source=PlanSource.lead0,
+              allow_throttle=False, e2e=False, force_decel=False):
+  planner = object.__new__(LongitudinalPlannerSP)
+  return planner.arbitrate_cruise_candidate(
+    cast(messaging.SubMaster, sm), gated_cruise, ungated_cruise, mpc_accel, source,
+    allow_throttle=allow_throttle, e2e=e2e, force_decel=force_decel,
+  )
+
+
+class TestThrottleIntentArbitration(OpenpilotTestCase):
+  def test_coast_gate_cannot_add_braking_a_valid_lead_does_not_need(self):
+    selected = arbitrate(FakeSubMaster())
+    self.assertEqual(selected, 0.5)
+    self.assertEqual(min(selected, 0.19), 0.19)
+
+  def test_counterfactual_must_select_the_lead(self):
+    self.assertEqual(arbitrate(FakeSubMaster(), ungated_cruise=0.1), -0.26)
+
+  def test_legitimate_cruise_braking_is_preserved(self):
+    self.assertEqual(arbitrate(FakeSubMaster(), gated_cruise=-0.5, ungated_cruise=-0.5), -0.5)
+
+  def test_hard_lead_braking_remains_authoritative(self):
+    selected = arbitrate(FakeSubMaster(), mpc_accel=-0.6)
+    self.assertEqual(selected, -0.26)
+    self.assertEqual(min(selected, -0.6), -0.6)
+
+  def test_policy_requires_live_valid_selected_lead(self):
+    cases = (
+      FakeSubMaster(valid=False),
+      FakeSubMaster(alive=False),
+      FakeSubMaster(lead_one=False),
+      FakeSubMaster(lead_one=True, lead_two=False),
+    )
+    sources = (PlanSource.lead0, PlanSource.lead0, PlanSource.lead0, PlanSource.lead1)
+    for sm, source in zip(cases, sources, strict=True):
+      with self.subTest(source=source, valid=sm.valid, alive=sm.alive):
+        self.assertEqual(arbitrate(sm, source=source), -0.26)
+
+  def test_second_lead_is_supported(self):
+    self.assertEqual(arbitrate(FakeSubMaster(False, True), source=PlanSource.lead1), 0.5)
+
+  def test_bypass_modes_are_stock_identical(self):
+    for allow_throttle, e2e, force_decel in ((True, False, False), (False, True, False), (False, False, True)):
+      with self.subTest(allow_throttle=allow_throttle, e2e=e2e, force_decel=force_decel):
+        selected = arbitrate(FakeSubMaster(), allow_throttle=allow_throttle, e2e=e2e, force_decel=force_decel)
+        self.assertEqual(selected, -0.26)
+
+  def test_nonfinite_inputs_do_not_bypass_gate(self):
+    for gated, ungated, mpc_accel in ((float('nan'), 0.5, 0.2),
+                                      (-0.26, float('inf'), 0.2),
+                                      (-0.26, 0.5, float('nan'))):
+      with self.subTest(gated=gated, ungated=ungated, mpc_accel=mpc_accel):
+        selected = arbitrate(FakeSubMaster(), mpc_accel, gated, ungated)
+        if gated != gated:
+          self.assertNotEqual(selected, selected)
+        else:
+          self.assertEqual(selected, gated)
+
+  def test_5c4_shape_keeps_positive_lead_authoritative_until_it_brakes(self):
+    class CarParams:
+      steerRatio = 15.0
+      wheelbase = 2.7
+
+    planner = object.__new__(LongitudinalPlannerSP)
+    sm = FakeSubMaster()
+    v_ego = 6.9
+    v_cruise = 30.0
+    accel_coast = get_coast_accel(-0.007)
+    cruise_accel = 0.185
+    mpc_trace = (0.18, 0.175, 0.165, 0.126, 0.06, -0.10, -0.30)
+    outputs = []
+
+    for mpc_accel in mpc_trace:
+      previous_cruise_accel = cruise_accel
+      gated = get_cruise_accel(False, v_cruise, v_ego, previous_cruise_accel, 0.0,
+                                CarParams(), DT_MDL, accel_coast, False, 1.45)
+      ungated = get_cruise_accel(False, v_cruise, v_ego, previous_cruise_accel, 0.0,
+                                  CarParams(), DT_MDL, accel_coast, True, 1.45)
+      cruise_accel = planner.arbitrate_cruise_candidate(
+        cast(messaging.SubMaster, sm), gated, ungated, mpc_accel, PlanSource.lead0,
+        allow_throttle=False, e2e=False, force_decel=False,
+      )
+      outputs.append(min(cruise_accel, mpc_accel))
+      self.assertLessEqual(abs(cruise_accel - previous_cruise_accel), 0.07)
+
+    self.assertEqual(outputs, list(mpc_trace))

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_throttle_intent_controller.py

@@ -0,0 +1,104 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import math
+
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.sunnypilot.selfdrive.controls.lib.throttle_intent_controller import ThrottleIntentController
+
+
+class TestAllowThrottle(OpenpilotTestCase):
+  def setUp(self):
+    self.controller = ThrottleIntentController()
+
+  def update(self, throttle_prob: float, low_speed_override: bool = False) -> bool:
+    return self.controller.update(throttle_prob, low_speed_override=low_speed_override, threshold=0.4)
+
+  def test_short_probability_dip(self):
+    self.assertTrue(self.update(1.0))
+    self.assertTrue(self.update(0.0))
+    self.assertTrue(self.update(0.0))
+    self.assertTrue(self.update(0.0))
+
+  def test_probability_chatter(self):
+    self.assertTrue(self.update(1.0))
+    for _ in range(100):
+      self.assertTrue(self.update(0.39))
+      self.assertTrue(self.update(0.46))
+
+    self.controller = ThrottleIntentController()
+    for _ in range(10):
+      self.update(0.0)
+    self.assertFalse(self.update(0.0))
+    for _ in range(100):
+      self.assertFalse(self.update(0.39))
+      self.assertFalse(self.update(0.46))
+
+  def test_sustained_probability_changes(self):
+    self.assertTrue(self.update(1.0))
+    states = [self.update(0.0) for _ in range(8)]
+    self.assertTrue(all(states[:-1]))
+    self.assertFalse(states[-1])
+
+    for _ in range(20):
+      self.assertFalse(self.update(0.0))
+    self.assertEqual([self.update(1.0) for _ in range(2)], [False, True])
+
+  def test_threshold_boundaries(self):
+    for _ in range(6):
+      state = self.update(0.4)
+    self.assertFalse(state)
+    self.controller.prob_filter.x = 0.44
+    self.controller.prob_filter.initialized = True
+    self.assertFalse(self.update(0.44))
+    self.controller.prob_filter.x = 0.46
+    self.assertTrue(self.update(0.46))
+
+  def test_low_speed_override(self):
+    for _ in range(20):
+      self.assertTrue(self.update(0.0, True))
+
+    self.assertTrue(self.update(1.0))
+    self.assertTrue(self.update(math.nan, True))
+    self.assertEqual(self.controller.prob_filter.x, 0.45)
+    self.assertTrue(self.controller.prob_filter.initialized)
+    self.assertTrue(self.update(1.0))
+    for _ in range(20):
+      self.assertTrue(self.update(0.0, True))
+    states = [self.update(0.0) for _ in range(6)]
+    self.assertTrue(all(states[:-1]))
+    self.assertFalse(states[-1])
+
+  def test_nonfinite_probability(self):
+    self.assertTrue(self.update(1.0))
+    for value in (math.inf, -math.inf, math.nan):
+      self.assertFalse(self.update(value))
+      self.assertTrue(math.isfinite(self.controller.prob_filter.x))
+
+    self.assertFalse(self.update(1.0))
+    self.assertTrue(self.update(1.0))
+
+  def test_route_probability_trace(self):
+    probabilities = (0.941, 0.093, 0.070, 0.429, 0.430, 0.083, 0.509, 0.068)
+    states = [self.update(probability) for probability in probabilities]
+    self.assertTrue(all(states))
+
+  def test_sustained_low_probability_still_disables_throttle(self):
+    self.assertTrue(self.update(1.0))
+    states = [self.update(0.0) for _ in range(20)]
+    self.assertFalse(all(states))
+    self.assertFalse(states[-1])
+
+  def test_filter_updates_once(self):
+    self.assertTrue(self.update(1.0))
+    self.assertTrue(self.update(0.0))
+    self.assertAlmostEqual(self.controller.prob_filter.x, 2.0 / 3.0)
+
+  def test_disable_dwell_uses_wall_time(self):
+    self.controller = ThrottleIntentController(dt=0.1)
+    states = [self.update(0.0) for _ in range(3)]
+    self.assertEqual(states, [True, True, False])

openpilot/sunnypilot/selfdrive/controls/lib/throttle_intent_controller.py

@@ -0,0 +1,53 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import math
+
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.realtime import DT_MDL
+
+THROTTLE_PROB_FILTER_RC = 0.10
+THROTTLE_PROB_HYSTERESIS = 0.05
+THROTTLE_DISABLE_TIME = 0.30
+
+
+class ThrottleIntentController:
+  def __init__(self, dt: float = DT_MDL):
+    self.disable_frames = max(1, math.ceil(THROTTLE_DISABLE_TIME / dt))
+    self.allow_throttle = True
+    self.disallow_frames = 0
+    self.prob_filter = FirstOrderFilter(0.0, THROTTLE_PROB_FILTER_RC, dt, initialized=False)
+
+  def update(self, throttle_prob: float, *, low_speed_override: bool, threshold: float) -> bool:
+    if low_speed_override:
+      self.allow_throttle = True
+      self.disallow_frames = 0
+      self.prob_filter.x = threshold + THROTTLE_PROB_HYSTERESIS
+      self.prob_filter.initialized = True
+      return True
+
+    if not math.isfinite(throttle_prob):
+      self.allow_throttle = False
+      self.disallow_frames = 0
+      self.prob_filter.x = 0.0
+      self.prob_filter.initialized = True
+      return False
+
+    filtered_throttle_prob = self.prob_filter.update(throttle_prob)
+    if self.allow_throttle:
+      if filtered_throttle_prob <= threshold:
+        self.disallow_frames += 1
+        if self.disallow_frames >= self.disable_frames:
+          self.allow_throttle = False
+          self.disallow_frames = 0
+      else:
+        self.disallow_frames = 0
+    elif filtered_throttle_prob > threshold + THROTTLE_PROB_HYSTERESIS:
+      self.allow_throttle = True
+      self.disallow_frames = 0
+
+    return self.allow_throttle