mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 05:23:41 +08:00
firestar5683
e4f3d6661c
Implement acceleration profiles, FollowJerk, force-stop and lead-departure owners. Keep selected acceleration limits and hazard arbitration explicit at their shared boundaries. Co-authored-by: whoisdomi <dcantu83@gmail.com>
231 lines
11 KiB
Python
231 lines
11 KiB
Python
from dataclasses import replace
|
|
import math
|
|
import unittest
|
|
|
|
from openpilot.starpilot.longitudinal.force_stop import ForceStop, StopFrame, vehicle_tune
|
|
|
|
|
|
BASE = 100_000_000_000
|
|
TICK = 50_000_000
|
|
|
|
|
|
def frame(tick, **changes):
|
|
return replace(StopFrame(BASE + tick * TICK, 10., 80., True, False, False, False, True, False,
|
|
False, False, 200., 30., False, 0., False, 0.), **changes)
|
|
|
|
|
|
class ForceStopTests(unittest.TestCase):
|
|
def setUp(self):
|
|
self.owner = ForceStop()
|
|
self.tune = vehicle_tune('HYUNDAI_IONIQ_6')
|
|
|
|
def test_original_approach_commit_kinematics_and_obstacle(self):
|
|
for tick in range(40):
|
|
plan = self.owner.step(frame(tick, model_should_stop=True), self.tune)
|
|
# The existing 0.5s accumulator crosses .5 on frame 10 with binary floats.
|
|
distance = 80. if tick < 10 else 80. - (tick - 9) * .5
|
|
self.assertEqual(plan.forcing, tick >= 10)
|
|
assert plan.speed_ceiling_mps is not None
|
|
self.assertAlmostEqual(plan.speed_ceiling_mps, math.sqrt(1.3 * (distance - 6)))
|
|
assert plan.obstacle_m is not None
|
|
self.assertAlmostEqual(plan.obstacle_m, distance * .93)
|
|
self.assertEqual(plan.jerk_scale, .32)
|
|
self.assertFalse(plan.should_stop)
|
|
|
|
def test_activation_threshold_hysteresis_preserves_commit_timer(self):
|
|
for tick in range(20):
|
|
plan = self.owner.step(frame(tick, horizon_m=99. if tick % 2 == 0 else 103.), self.tune)
|
|
self.assertTrue(plan.forcing)
|
|
self.assertGreaterEqual(self.owner.timer, .5)
|
|
cold = ForceStop()
|
|
for tick in range(20):
|
|
plan = cold.step(frame(tick, horizon_m=103.), self.tune)
|
|
self.assertFalse(plan.forcing)
|
|
self.assertGreater(plan.approach_distance_m, 0)
|
|
|
|
def test_lead_and_curve_veto_before_commit(self):
|
|
for changes in ({'lead_present': True, 'lead_distance_m': 40., 'lead_speed_mps': 9.},
|
|
{'tracking_lead': True}, {'driving_in_curve': True}):
|
|
with self.subTest(changes=changes):
|
|
self.owner.reset()
|
|
for tick in range(20):
|
|
plan = self.owner.step(frame(tick, **changes), self.tune)
|
|
self.assertFalse(plan.forcing)
|
|
self.assertIsNone(plan.speed_ceiling_mps)
|
|
self.assertIsNone(plan.obstacle_m)
|
|
|
|
def test_lead_appearing_does_not_abandon_committed_stop(self):
|
|
for tick in range(15):
|
|
self.owner.step(frame(tick), self.tune)
|
|
plan = self.owner.step(frame(15, lead_present=True, lead_distance_m=20, lead_speed_mps=0, tracking_lead=True), self.tune)
|
|
self.assertTrue(plan.forcing)
|
|
|
|
def test_accelerator_releases_then_suppresses_for_ten_seconds(self):
|
|
for tick in range(15):
|
|
self.owner.step(frame(tick), self.tune)
|
|
for tick in range(15, 215):
|
|
plan = self.owner.step(frame(tick, pedal_override=tick == 15), self.tune)
|
|
self.assertFalse(plan.forcing)
|
|
self.assertIsNone(plan.speed_ceiling_mps)
|
|
resumed = self.owner.step(frame(215), self.tune)
|
|
assert resumed.speed_ceiling_mps is not None
|
|
|
|
def test_resume_releases_standstill_hold_but_does_not_create_override_when_idle(self):
|
|
self.owner.step(frame(0, light=False, horizon_m=190., resume_requested=True), self.tune)
|
|
self.assertEqual(self.owner.override_until_ns, 0)
|
|
held = frame(1, speed_mps=0., horizon_m=30., standstill=True, raw_model_stopped=True)
|
|
self.owner.reset()
|
|
self.assertTrue(self.owner.step(held, self.tune).should_stop)
|
|
released = self.owner.step(replace(held, model_ns=held.model_ns + TICK, resume_requested=True), self.tune)
|
|
self.assertFalse(released.should_stop)
|
|
self.assertFalse(released.forcing)
|
|
self.assertEqual(self.owner.override_until_ns, held.model_ns + TICK + 10_000_000_000)
|
|
|
|
def test_light_release_requires_half_second_continuously_clear(self):
|
|
for tick in range(15):
|
|
self.owner.step(frame(tick), self.tune)
|
|
for tick in range(15, 25):
|
|
self.assertTrue(self.owner.step(frame(tick, light=False), self.tune).forcing)
|
|
cleared = self.owner.step(frame(25, light=False), self.tune)
|
|
self.assertFalse(cleared.forcing)
|
|
self.assertIsNone(cleared.obstacle_m)
|
|
|
|
def test_committed_stop_survives_unavailable_perception_and_uses_elapsed_odometry(self):
|
|
for tick in range(15):
|
|
previous = self.owner.step(frame(tick, horizon_m=30.), self.tune)
|
|
# A valid current model event may have unusable predicted stop geometry.
|
|
# Repeated unknown perception cannot become a clear-light release.
|
|
for tick in range(15, 35):
|
|
plan = self.owner.step(frame(tick, horizon_m=None, road_curvature=None,
|
|
light=False, perception_available=False), self.tune)
|
|
self.assertTrue(plan.forcing)
|
|
self.assertAlmostEqual(plan.tracked_distance_m, max(0., previous.tracked_distance_m - .5))
|
|
assert plan.speed_ceiling_mps is not None
|
|
assert previous.speed_ceiling_mps is not None
|
|
self.assertLessEqual(plan.speed_ceiling_mps, previous.speed_ceiling_mps)
|
|
previous = plan
|
|
held = self.owner.step(frame(35, speed_mps=0., standstill=True, horizon_m=None,
|
|
road_curvature=None, light=False, perception_available=False), self.tune)
|
|
self.assertTrue(held.should_stop)
|
|
for tick in range(36, 76):
|
|
self.assertTrue(self.owner.step(frame(tick, speed_mps=-1e-44, standstill=True, horizon_m=None,
|
|
road_curvature=None, light=False, perception_available=False), self.tune).should_stop)
|
|
# Reopened geometry cannot classify a held stop or authorize departure.
|
|
for tick in range(76, 92):
|
|
plan = self.owner.step(frame(tick, speed_mps=0., standstill=True, horizon_m=190., light=False), self.tune)
|
|
self.assertTrue(plan.forcing)
|
|
self.assertTrue(plan.manual_hold)
|
|
|
|
def test_missing_perception_distance_uses_actual_model_interval(self):
|
|
for tick in range(15):
|
|
previous = self.owner.step(frame(tick, horizon_m=30.), self.tune)
|
|
# One skipped 20 Hz tick still carries fresh source authority. Odometry
|
|
# integrates both intervals instead of moving the remembered stop line.
|
|
plan = self.owner.step(frame(16, horizon_m=None, road_curvature=None, perception_available=False), self.tune)
|
|
self.assertTrue(plan.forcing)
|
|
self.assertAlmostEqual(plan.tracked_distance_m, previous.tracked_distance_m - 1.)
|
|
|
|
def test_negative_light_with_stopping_path_cannot_release_commitment(self):
|
|
for tick in range(15):
|
|
self.owner.step(frame(tick, horizon_m=30.), self.tune)
|
|
for tick in range(15, 55):
|
|
plan = self.owner.step(frame(tick, horizon_m=30., light=False, raw_model_stopped=True), self.tune)
|
|
self.assertTrue(plan.forcing)
|
|
# It is not necessary to reach standstill after a qualified reopened path.
|
|
for tick in range(55, 66):
|
|
plan = self.owner.step(frame(tick, horizon_m=190., light=False), self.tune)
|
|
self.assertFalse(plan.forcing)
|
|
|
|
def test_unknown_perception_never_acquires_and_driver_release_still_works(self):
|
|
for tick in range(20):
|
|
plan = self.owner.step(frame(tick, horizon_m=None, road_curvature=None, perception_available=False), self.tune)
|
|
self.assertFalse(plan.forcing)
|
|
self.assertIsNone(plan.speed_ceiling_mps)
|
|
for action in ('pedal_override', 'resume_requested'):
|
|
self.owner.reset()
|
|
for tick in range(15):
|
|
self.owner.step(frame(tick), self.tune)
|
|
plan = self.owner.step(frame(15, horizon_m=None, road_curvature=None, perception_available=False,
|
|
**{action: True}), self.tune)
|
|
self.assertFalse(plan.forcing)
|
|
self.assertIsNone(plan.speed_ceiling_mps)
|
|
|
|
def test_unclassified_standstill_holds_until_driver_release(self):
|
|
for tick in range(80):
|
|
plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=30., raw_model_stopped=True, standstill=True), self.tune)
|
|
self.assertTrue(plan.forcing)
|
|
self.assertTrue(plan.should_stop)
|
|
for tick in range(80, 96):
|
|
plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=190., raw_model_stopped=False,
|
|
light=False, standstill=True), self.tune)
|
|
self.assertTrue(plan.forcing)
|
|
self.assertTrue(plan.should_stop)
|
|
self.assertTrue(plan.manual_hold)
|
|
for action in ('resume_requested', 'pedal_override'):
|
|
released = self.owner.step(frame(96 if action == 'resume_requested' else 97, speed_mps=0.,
|
|
horizon_m=190., light=False, standstill=True, **{action: True}), self.tune)
|
|
self.assertFalse(released.manual_hold)
|
|
self.assertFalse(released.forcing)
|
|
self.assertFalse(released.should_stop)
|
|
|
|
def test_manual_hold_survives_clear_geometry_and_roll_until_each_driver_action(self):
|
|
for action in ('resume_requested', 'pedal_override'):
|
|
self.owner.reset()
|
|
held = self.owner.step(frame(0, speed_mps=0., horizon_m=30., raw_model_stopped=True, standstill=True), self.tune)
|
|
self.assertTrue(held.manual_hold)
|
|
for tick in range(1, 401):
|
|
plan = self.owner.step(frame(tick, speed_mps=.01, horizon_m=190., light=False, standstill=False), self.tune)
|
|
self.assertTrue(plan.manual_hold)
|
|
self.assertEqual(plan.speed_ceiling_mps, 0.)
|
|
release = self.owner.step(frame(401, speed_mps=0., horizon_m=190., light=False, standstill=True,
|
|
**{action: True}), self.tune)
|
|
self.assertFalse(release.manual_hold)
|
|
self.assertFalse(release.forcing)
|
|
self.assertFalse(release.should_stop)
|
|
|
|
def test_odometry_ceiling_bounds_reopened_model(self):
|
|
for tick in range(15):
|
|
self.owner.step(frame(tick), self.tune)
|
|
cap = self.owner.distance_cap
|
|
plan = self.owner.step(frame(15, horizon_m=190.), self.tune)
|
|
self.assertAlmostEqual(self.owner.distance_cap, cap - .5)
|
|
self.assertLessEqual(plan.tracked_distance_m, cap - .5 + 15)
|
|
|
|
def test_offset_and_handoff_keep_native_should_stop(self):
|
|
for offset in (-20, 0, 20):
|
|
with self.subTest(offset=offset):
|
|
self.owner.reset()
|
|
for tick in range(15):
|
|
plan = self.owner.step(frame(tick, horizon_m=12.), self.tune, offset)
|
|
expected = max(0., plan.tracked_distance_m + offset * .3048 - 6)
|
|
assert plan.speed_ceiling_mps is not None
|
|
self.assertAlmostEqual(plan.speed_ceiling_mps, math.sqrt(1.3 * expected))
|
|
assert plan.speed_ceiling_mps is not None
|
|
self.assertEqual(plan.should_stop, plan.speed_ceiling_mps <= .5)
|
|
|
|
def test_current_platform_rules(self):
|
|
self.assertEqual(vehicle_tune('TOYOTA_CAMRY_TSS2').handoff_m, 4.5)
|
|
self.assertEqual(vehicle_tune('TOYOTA_CAMRY_TSS2').distance_bias_m, 6.)
|
|
self.assertEqual(vehicle_tune('HYUNDAI_ELANTRA_2021').lead_veto_m, 90.)
|
|
self.assertEqual(vehicle_tune('HYUNDAI_ELANTRA_2021').jerk_scale, .8)
|
|
santa_fe = vehicle_tune('HYUNDAI_SANTA_FE_2022')
|
|
for tick in range(15):
|
|
self.owner.step(frame(tick), santa_fe)
|
|
for tick in range(15, 35):
|
|
plan = self.owner.step(frame(tick, speed_mps=2., light=False, horizon_m=30.), santa_fe)
|
|
self.assertTrue(plan.forcing)
|
|
|
|
def test_old_duplicate_gap_and_disabled_clear_owner(self):
|
|
for defect in ('duplicate', 'gap', 'disabled'):
|
|
self.owner.reset()
|
|
for tick in range(15):
|
|
self.owner.step(frame(tick), self.tune)
|
|
value = frame(14 if defect == 'duplicate' else 20 if defect == 'gap' else 15,
|
|
enabled=defect != 'disabled')
|
|
self.assertFalse(self.owner.step(value, self.tune).forcing)
|
|
self.assertFalse(self.owner.forcing)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
unittest.main()
|