diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index d4fe4aa26c..3a8356cf00 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/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) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 1f07b14471..b7f163265f 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/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) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_throttle_intent_arbitration.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_throttle_intent_arbitration.py new file mode 100644 index 0000000000..ade5f4eaea --- /dev/null +++ b/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)) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_throttle_intent_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_throttle_intent_controller.py new file mode 100644 index 0000000000..ee49eb5aab --- /dev/null +++ b/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]) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/throttle_intent_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/throttle_intent_controller.py new file mode 100644 index 0000000000..8f86fc40e2 --- /dev/null +++ b/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