mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-11-12 15:33:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Force Stop Signs behind leads

Browse Source
This commit is contained in:
whoisdomi
2026-11-08 12:35:58 -06:00
parent 3513840095
commit c86a4ac8d6
2 changed files with 91 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/starpilot/longitudinal/force_stop.py
+35 -5
View File
@@ -15,6 +15,13 @@ SIGN_SEED_M = 27.0
SIGN_ESTIMATE_HOLD_NS = 2_000_000_000
SIGN_APPROACH_DECEL = 1.0
MODEL_APPROACH_DECEL = .65
# Stopped behind a car at a sign: the line is assumed one car length past that car's rear.
QUEUE_LEAD_MAX_M = 8.0
QUEUE_LEAD_LENGTH_M = 4.5
# A car still inside this margin past the line may just be moving up the queue, not through the intersection.
QUEUE_CLEAR_M = 5.0
QUEUE_SIGN_RECENT_NS = 10_000_000_000
QUEUE_MAX_SPEED_MPS = 6.0
@dataclass(frozen=True)
@@ -101,6 +108,8 @@ class ForceStop:
self.previously_enabled = False
self.distance = 0.0
self.distance_cap = 0.0
self.queue_sign_ns = 0
self.queue_line = None
self._clear_sign_estimate()
def _clear_sign_estimate(self):
@@ -128,6 +137,20 @@ class ForceStop:
estimates = [value for value in (self.dash_distance, self.vision_distance) if value is not None]
return sum(estimates) / len(estimates) if estimates else None
def _queue_step(self, frame: StopFrame, now: int, travel: float, lead_blocked: bool):
if frame.sign and lead_blocked:
self.queue_sign_ns = now
queued_lead = frame.lead_present and frame.lead_speed_mps < 1.0 and frame.lead_distance_m < QUEUE_LEAD_MAX_M
arm = frame.standstill and self.queue_sign_ns and now - self.queue_sign_ns <= QUEUE_SIGN_RECENT_NS
if queued_lead and (arm or self.queue_line is not None):
self.queue_line = frame.lead_distance_m + QUEUE_LEAD_LENGTH_M
elif self.queue_line is not None:
self.queue_line -= travel
if self.queue_line is not None and (frame.pedal_override or frame.resume_requested or self.queue_line <= 0 or
frame.speed_mps > QUEUE_MAX_SPEED_MPS):
self.queue_line = None
self.queue_sign_ns = 0
def _clear_standstill(self):
self.standstill_since_ns = 0
self.standstill_clear_ns = 0
@@ -157,7 +180,9 @@ class ForceStop:
return StopPlan()
available = frame.perception_available
sign_estimate = self._sign_estimate(frame, now, max(0.0, speed) * dt)
if available and (frame.light or frame.raw_model_stopped) or frame.sign:
lead = (frame.lead_present and frame.lead_distance_m < tune.lead_veto_m and frame.lead_speed_mps < speed + 2)
self._queue_step(frame, now, max(0.0, speed) * dt, lead or frame.tracking_lead)
if available and (frame.light or frame.raw_model_stopped) or frame.sign or self.queue_line is not None:
if not frame.standstill:
self.stop_seen_ns = now
if frame.standstill:
@@ -165,16 +190,19 @@ class ForceStop:
stop_then_turn = bool(self.stop_seen_ns and now - self.stop_seen_ns < 4_000_000_000)
turn = speed <= 18 * 0.44704 and frame.blinker and abs(frame.steering_angle_deg) >= 25 and not stop_then_turn
curved = available and abs(curvature) >= .003 and not stop_then_turn
lead = (frame.lead_present and frame.lead_distance_m < tune.lead_veto_m and frame.lead_speed_mps < speed + 2)
model_active = available and horizon < (108.0 if self.activation_gate and frame.light else 100.0)
self.activation_gate = model_active and frame.light
permitted = (available and now >= self.override_until_ns and not frame.driving_in_curve and not turn and
not frame.tracking_lead and not lead)
model_path = frame.light and model_active and permitted and not curved
sign_path = frame.sign and permitted and speed < SIGN_MAX_SPEED_MPS and (horizon < SIGN_MODEL_AGREE_M or self.forcing)
if sign_path:
line_blocked = (self.queue_line is not None and frame.lead_present and
frame.lead_distance_m < self.queue_line + QUEUE_CLEAR_M)
queue_path = (self.queue_line is not None and not line_blocked and not frame.standstill and available and
now >= self.override_until_ns and not frame.driving_in_curve and not turn)
if sign_path or queue_path:
self.sign_confirmed = True
active = model_path or sign_path
active = model_path or sign_path or queue_path
if model_path:
self.from_light = True
self.light_clear_ns = 0
@@ -213,7 +241,7 @@ class ForceStop:
if not available and not self.forcing:
self.timer = 0
elif active and not frame.standstill:
self.timer = min(2.0, self.timer + (SIGN_CONFIRM_STEP_S if frame.sign else DT_MDL))
self.timer = min(2.0, self.timer + (SIGN_CONFIRM_STEP_S if frame.sign or queue_path else DT_MDL))
elif turn and not self.forcing and not frame.standstill:
self.timer = 0
elif held:
@@ -264,6 +292,8 @@ class ForceStop:
self.distance = min(self.distance, cap)
if sign_estimate is not None and (self.sign_confirmed or frame.sign and available and horizon < SIGN_MODEL_AGREE_M):
self.distance = min(sign_estimate, cap)
if self.queue_line is not None and self.sign_confirmed:
self.distance = self.queue_line
effective = self.distance + offset + tune.distance_bias_m
decel = SIGN_APPROACH_DECEL if frame.sign else MODEL_APPROACH_DECEL
ceiling = math.sqrt(2 * decel * max(0, effective - tune.handoff_m))
openpilot/starpilot/longitudinal/tests/test_force_stop.py
+56
View File
@@ -289,6 +289,62 @@ class ForceStopTests(unittest.TestCase):
self.assertLess(plan.tracked_distance_m, 30.)
self.assertIsNone(self.owner.vision_distance)
def queue_behind_lead(self, sign=True):
for tick in range(10):
plan = self.owner.step(frame(tick, speed_mps=8., horizon_m=45., light=False, sign=sign, lead_present=True,
lead_distance_m=20., lead_speed_mps=0.), self.tune)
self.assertFalse(plan.forcing)
for tick in range(10, 30):
plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=5., light=False, standstill=True, lead_present=True,
lead_distance_m=5., lead_speed_mps=0.), self.tune)
self.assertFalse(plan.forcing)
def test_sign_behind_stopped_lead_stops_at_line_after_lead_leaves(self):
self.queue_behind_lead()
self.assertAlmostEqual(self.owner.queue_line, 9.5)
for tick in range(30, 35):
plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=12., light=False, standstill=True, lead_present=True,
lead_distance_m=7., lead_speed_mps=3.), self.tune)
self.assertFalse(plan.forcing)
for tick in range(35, 45):
plan = self.owner.step(frame(tick, speed_mps=1., horizon_m=15., light=False), self.tune)
self.assertTrue(plan.forcing)
self.assertTrue(self.owner.sign_confirmed)
self.assertAlmostEqual(plan.tracked_distance_m, 9.5 - 10 * .05)
for tick in range(45, 100):
plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=15., light=False, standstill=True), self.tune)
self.assertTrue(plan.manual_hold)
released = self.owner.step(frame(100, speed_mps=0., horizon_m=15., light=False, standstill=True,
pedal_override=True), self.tune)
self.assertFalse(released.manual_hold)
self.assertIsNone(self.owner.queue_line)
def test_lead_stop_without_sign_is_not_remembered(self):
self.queue_behind_lead(sign=False)
self.assertIsNone(self.owner.queue_line)
for tick in range(30, 50):
plan = self.owner.step(frame(tick, speed_mps=1., horizon_m=15., light=False), self.tune)
self.assertFalse(plan.forcing)
def test_lead_moving_up_the_queue_is_followed_not_stopped_for(self):
self.queue_behind_lead()
for tick in range(30, 60):
plan = self.owner.step(frame(tick, speed_mps=1., horizon_m=15., light=False, lead_present=True,
lead_distance_m=6., lead_speed_mps=1.5), self.tune)
self.assertFalse(plan.forcing)
plan = self.owner.step(frame(60, speed_mps=0., horizon_m=5., light=False, standstill=True, lead_present=True,
lead_distance_m=4., lead_speed_mps=0.), self.tune)
self.assertFalse(plan.forcing)
self.assertAlmostEqual(self.owner.queue_line, 8.5)
def test_driver_gas_while_queued_forgets_the_sign(self):
self.queue_behind_lead()
self.owner.step(frame(30, speed_mps=0., horizon_m=5., light=False, standstill=True, pedal_override=True), self.tune)
self.assertIsNone(self.owner.queue_line)
for tick in range(31, 50):
plan = self.owner.step(frame(tick, speed_mps=1., horizon_m=15., light=False), self.tune)
self.assertFalse(plan.forcing)
def test_positive_offset_keeps_stop_target_until_native_stop(self):
for tick in range(40):
plan = self.owner.step(frame(tick, horizon_m=12.), self.tune, 8)