From a4aef29a4489c57a2ea71aed194998df1b330fde Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Fri, 6 Nov 2026 05:18:08 -0600 Subject: [PATCH] No twitch test? --- .../starpilot/lateral/model_turn_assist.py | 22 +++++++ .../tests/test_model_turn_assist_stop_hold.py | 60 +++++++++++++++++++ 2 files changed, 82 insertions(+) create mode 100644 openpilot/starpilot/lateral/tests/test_model_turn_assist_stop_hold.py diff --git a/openpilot/starpilot/lateral/model_turn_assist.py b/openpilot/starpilot/lateral/model_turn_assist.py index 8620e1e180..e915dec32e 100644 --- a/openpilot/starpilot/lateral/model_turn_assist.py +++ b/openpilot/starpilot/lateral/model_turn_assist.py @@ -34,6 +34,10 @@ TWITCH_GUARD_FLOOR = 0.002 TWITCH_GUARD_STRAIGHT_LO = 0.005 TWITCH_GUARD_STRAIGHT_HI = 0.014 TWITCH_GUARD_MIN_REACH = 12.0 +STOP_HOLD_MAX_SPEED = 2.5 +STOP_HOLD_DIST = 1.5 +STOP_HOLD_RELEASE_DIST = 3.0 +STOP_HOLD_MIN_DECEL = 0.3 def _plan_circle_curvature(xs, ys, lookahead: float) -> float: @@ -124,6 +128,12 @@ def update_twitch_guard(remaining: float, v_ego: float, standstill: bool) -> flo if standstill or abs(v_ego) <= 0.3: return TWITCH_GUARD_DURATION return max(remaining - DT_CTRL, 0.0) + + +def stopping_distance(v_ego: float, a_ego: float) -> float: + if not (math.isfinite(v_ego) and math.isfinite(a_ego)) or v_ego >= STOP_HOLD_MAX_SPEED: + return math.inf + return v_ego ** 2 / (2.0 * max(-a_ego, 0.01)) TURN_LEAD_T = 1.3 TURN_LEAD_MIN_M = 4.0 TURN_LEAD_MAX_M = 14.0 @@ -150,11 +160,23 @@ class ModelTurnAssist: self.turn_hold_handoff_t = 0.0 self.turn_blinker_swept = 0.0 self.twitch_guard_remaining = 0.0 + self.stop_hold_curvature = None self.curvature = 0.0 self.turn_hold_done = False def update(self, CS, model_v2, CC, new_desired_curvature): blinker_dir = float(CS.rightBlinker) - float(CS.leftBlinker) + stop_dist = stopping_distance(CS.vEgo, CS.aEgo) + entering = CS.aEgo < -STOP_HOLD_MIN_DECEL and stop_dist < STOP_HOLD_DIST + holding = self.stop_hold_curvature is not None and stop_dist < STOP_HOLD_RELEASE_DIST + if CC.latActive and blinker_dir == 0.0 and self.turn_hold_curvature == 0.0 and (entering or holding): + if CS.steeringPressed: + self.stop_hold_curvature = self.curvature + elif self.stop_hold_curvature is None: + self.stop_hold_curvature = new_desired_curvature + new_desired_curvature = self.stop_hold_curvature + else: + self.stop_hold_curvature = None if CC.latActive and self.twitch_guard_remaining > 0.0 and (blinker_dir == 0.0) and (self.turn_hold_curvature == 0.0): new_desired_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo) if blinker_dir == 0.0: diff --git a/openpilot/starpilot/lateral/tests/test_model_turn_assist_stop_hold.py b/openpilot/starpilot/lateral/tests/test_model_turn_assist_stop_hold.py new file mode 100644 index 0000000000..069e188f6e --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_model_turn_assist_stop_hold.py @@ -0,0 +1,60 @@ +import unittest +from types import SimpleNamespace + +from openpilot.starpilot.lateral.model_turn_assist import STOP_HOLD_MAX_SPEED, ModelTurnAssist + +STRAIGHT = SimpleNamespace(position=SimpleNamespace(x=[0., 1., 2., 3.], y=[0., 0., 0., 0.]), + meta=SimpleNamespace(laneChangeState=0)) + + +def car(speed, accel, blinker=0, pressed=False): + return SimpleNamespace(vEgo=speed, aEgo=accel, rightBlinker=blinker == 1, leftBlinker=blinker == -1, + steeringPressed=pressed, steeringTorque=0.) + + +def step(assist, speed, accel, desired, active=True, **kwargs): + return assist.update(car(speed, accel, **kwargs), STRAIGHT, SimpleNamespace(latActive=active), desired) + + +class TestStopHold(unittest.TestCase): + def test_holds_command_through_final_metre_of_stop(self): + assist = ModelTurnAssist() + self.assertEqual(step(assist, 2.0, -2.0, .002), .002) + for speed, desired in ((1.5, .004), (1.0, .010), (.6, .014)): + self.assertEqual(step(assist, speed, -2.0, desired), .002) + + def test_releases_when_car_stops_slowing(self): + assist = ModelTurnAssist() + step(assist, 1.0, -2.0, .001) + self.assertEqual(step(assist, 1.0, -.5, .01), .001) + self.assertEqual(step(assist, 1.0, 0., .01), .01) + self.assertIsNone(assist.stop_hold_curvature) + + def test_does_not_engage_far_from_stop_or_above_speed(self): + for speed, accel in ((2.0, -.5), (1.0, -.2), (STOP_HOLD_MAX_SPEED, -5.), (1.0, 1.)): + with self.subTest(speed=speed, accel=accel): + assist = ModelTurnAssist() + self.assertEqual(step(assist, speed, accel, .01), .01) + self.assertIsNone(assist.stop_hold_curvature) + + def test_blinker_or_inactive_never_holds(self): + for direction in (-1, 1): + assist = ModelTurnAssist() + step(assist, 1.0, -2.0, .001) + self.assertNotEqual(step(assist, 1.0, -2.0, direction * .02, blinker=direction), .001) + self.assertIsNone(assist.stop_hold_curvature) + assist = ModelTurnAssist() + self.assertEqual(step(assist, 1.0, -2.0, .01, active=False), .01) + self.assertIsNone(assist.stop_hold_curvature) + + def test_driver_steering_moves_hold_to_measured_curvature(self): + assist = ModelTurnAssist() + step(assist, 1.0, -2.0, .001) + assist.curvature = -.003 + self.assertEqual(step(assist, .8, -2.0, .015, pressed=True), -.003) + assist.curvature = .009 + self.assertEqual(step(assist, .6, -2.0, .015), -.003) + + +if __name__ == '__main__': + unittest.main()