From 77babc4c855e521495f41fb8b594ea6e27257949 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 29 Oct 2026 22:10:01 -0500 Subject: [PATCH] Release an existing Force Stop hold with RES --- .../controls/lib/longitudinal_planner.py | 6 ++ openpilot/selfdrive/controls/plannerd.py | 6 ++ .../starpilot/longitudinal/force_stop.py | 10 ++- .../tests/test_cem_green_release.py | 51 ++++++++++++- .../longitudinal/tests/test_force_stop.py | 19 +++++ .../tests/test_force_stop_runtime.py | 76 +++++++++++++++++++ prebuilt.json | 2 +- 7 files changed, 166 insertions(+), 4 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index d70318fe80..a174612641 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -335,6 +335,12 @@ class LongitudinalPlanner: fallback=should_stop(v_ego, output_a_target_mpc)) output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop + if (self.force_stop_plan.resume_override and not self.force_stop_plan.forcing and + not self.force_stop_plan.should_stop and not reset_state and not self.fcw and self.allow_throttle and + abs(v_ego) < 2.0 and v_cruise > 0.3 and self.mpc.solution_status == 0 and + not any(lead.present for lead in (sm['radarState'].leadOne, sm['radarState'].leadTwo))): + output_a_target_e2e = max(output_a_target_e2e, 0.2) + output_should_stop_e2e = False relevant_lead = any(braking_lead_relevant(lead, v_ego) for lead in (sm['radarState'].leadOne, sm['radarState'].leadTwo)) diff --git a/openpilot/selfdrive/controls/plannerd.py b/openpilot/selfdrive/controls/plannerd.py index b47d4a0ef4..f4985f2123 100755 --- a/openpilot/selfdrive/controls/plannerd.py +++ b/openpilot/selfdrive/controls/plannerd.py @@ -231,6 +231,12 @@ def apply_conditional_stop_hold(host, planner, sm, CP, now_ns: int, drive_id: in if (now_ns - sm.logMonoTime['modelV2'] > 150_000_000 or now_ns - int(sm.recv_time['modelV2'] * 1e9) > 150_000_000): return unavailable() + stop_plan = getattr(planner, 'force_stop_plan', None) + if (stop_plan is not None and stop_plan.resume_override and stop_plan.model_ns == sm.logMonoTime['modelV2'] and + not stop_plan.forcing and not stop_plan.should_stop and abs(vehicle.vEgo) < 2.0 and planner.allow_throttle and + not any(lead.present for lead in (sm['radarState'].leadOne, sm['radarState'].leadTwo))): + host.mode.projector.stop_detector.reset() + return False from openpilot.starpilot.car.ford.stop_release import owner_for owner = getattr(host, 'ford_stopped_lead_release', None) if owner is None: diff --git a/openpilot/starpilot/longitudinal/force_stop.py b/openpilot/starpilot/longitudinal/force_stop.py index 180f976565..d4a6be77b4 100644 --- a/openpilot/starpilot/longitudinal/force_stop.py +++ b/openpilot/starpilot/longitudinal/force_stop.py @@ -62,6 +62,7 @@ class StopPlan: manual_hold: bool = False takeoff_light_observed: bool = False takeoff_light: bool = False + resume_override: bool = False class ForceStop: @@ -75,6 +76,7 @@ class ForceStop: self.from_light = False self.light_clear_ns = 0 self.override_until_ns = 0 + self.resume_until_ns = 0 self.entry_speed = None self.activation_gate = False self.stop_seen_ns = 0 @@ -132,11 +134,16 @@ class ForceStop: # Reopened geometry may withdraw a moving approach; it cannot classify # a stationary hold as a green light. go_scene = available and horizon >= 50.0 and not frame.raw_model_stopped and not frame.model_should_stop + held_before = self.previously_enabled and self.standstill_reason is not None engaged_stopped = not self.previously_enabled and frame.standstill if frame.standstill and (self.forcing or engaged_stopped and stop_scene) and self.standstill_reason is None: self.standstill_since_ns = now self.standstill_reason = 'unclassified' self.distance = 0 + if held_before and frame.standstill and frame.resume_requested: + self.resume_until_ns = now + 6_000_000_000 + if abs(speed) >= 2.0 or frame.lead_present or frame.tracking_lead: + self.resume_until_ns = 0 driver_release = frame.pedal_override or frame.resume_requested if self.standstill_reason is not None and driver_release: self.override_until_ns = now + 10_000_000_000 @@ -212,4 +219,5 @@ class ForceStop: obstacle = stop_length * .93 + offset + tune.distance_bias_m if stop_length > tune.handoff_m else None return StopPlan(now, ceiling, obstacle, self.forcing, bool(self.forcing and ceiling is not None and ceiling <= .5), self.distance if self.forcing else 0.0, approach, - tune.jerk_scale if self.forcing or approach > 0 else 1.0, bool(held and self.forcing and not overridden)) + tune.jerk_scale if self.forcing or approach > 0 else 1.0, bool(held and self.forcing and not overridden), + resume_override=now < self.resume_until_ns) diff --git a/openpilot/starpilot/longitudinal/tests/test_cem_green_release.py b/openpilot/starpilot/longitudinal/tests/test_cem_green_release.py index 9692d05bef..b6fe98cea4 100644 --- a/openpilot/starpilot/longitudinal/tests/test_cem_green_release.py +++ b/openpilot/starpilot/longitudinal/tests/test_cem_green_release.py @@ -119,7 +119,7 @@ class CemGreenCallerTests(unittest.TestCase): def cycle(self, *, stopped=False, position=None, velocity=None, should_stop=True, personality=log.LongitudinalPersonality.aggressive, unavailable=None, stale=None, - repeat_model=False, lead=False, desired_accel=None, horizon=20.): + repeat_model=False, lead=False, desired_accel=None, horizon=20., force_stop_owner=None, resume=False, aol=False): now = MONO + self.tick * 50_000_000 self.tick += 1 stamp = now - 2_000_000 @@ -146,8 +146,13 @@ class CemGreenCallerTests(unittest.TestCase): value.longControlState = LongCtrlState.stopping if stopped else LongCtrlState.pid elif name == 'selfdriveState': value.personality = personality + if aol: + value.enabled = False elif name == 'carControl': value.orientationNED = [0.] * 3 + if aol: + value.enabled = value.longActive = False + value.latActive = True elif name == 'carState': value.vEgoRaw = 0. if stopped else 4.27 payloads[name] = getattr(messaging.log_from_bytes(event.to_bytes()), name) @@ -162,7 +167,18 @@ class CemGreenCallerTests(unittest.TestCase): if repeat_model: sm.logMonoTime['modelV2'] = stamp - 50_000_000 sm.recv_time['modelV2'] = (stamp - 50_000_000) / 1e9 - self.planner.update(sm, now_ns=now, drive_id=self.drive) + provider = None + if force_stop_owner is not None: + event = messaging.new_message('carState', valid=True) + event.logMonoTime = stamp + event.carState = sm['carState'] + if resume: + event.carState.buttonEvents = [{'type': 'accelCruise', 'pressed': True}] + force_stop_owner.resume.observe(event, now_ns=now, drive_id=self.drive) + def provider(follow): + return force_stop_owner.sample(sm, self.cp, now_ns=now, now_boot_ns=boot, + drive_id=self.drive, follow_seconds=follow, traffic_mode=False) + self.planner.update(sm, now_ns=now, drive_id=self.drive, force_stop_provider=provider) self.assertEqual(self.planner.mpc.solution_status, 0) proposal, snapshot = self.owner.sample(sm, self.cp, self.planner, now_mono_ns=now, now_boot_ns=boot, sample_skew_ns=1000, drive_id=self.drive, native_plan_valid=sm.all_checks(payloads)) @@ -278,3 +294,34 @@ class CemGreenCallerTests(unittest.TestCase): self.cycle(stopped=True, position=CLEAR_64_X, velocity=CLEAR_64_V, should_stop=False) self.assertFalse(self.owner.mode.projector.stop_detector.committed) self.assertFalse(self.owner.mode.projector.stop_detector.standstill_committed) + + + def test_res_releases_existing_force_and_cem_holds_together(self): + from openpilot.starpilot.longitudinal.force_stop_runtime import ForceStopRuntime + self.params.put_bool('ForceStops', True, block=True) + force = ForceStopRuntime(self.params) + for _ in range(45): + self.cycle(force_stop_owner=force) + for _ in range(12): + _, _, held, plan = self.cycle(stopped=True, position=RED_97_X, velocity=RED_97_V, force_stop_owner=force) + self.assertTrue(held and plan.shouldStop and self.planner.force_stop_plan.manual_hold) + self.cycle(stopped=True, position=RED_97_X, velocity=RED_97_V, force_stop_owner=force, resume=True) + for _ in range(20): + _, _, held, plan = self.cycle(stopped=True, position=RED_97_X, velocity=RED_97_V, force_stop_owner=force) + self.assertFalse(held) + self.assertTrue(self.planner.force_stop_plan.resume_override) + self.assertFalse(plan.shouldStop) + self.assertGreaterEqual(plan.aTarget, .1) + + def test_res_engagement_from_aol_preserves_red_light_hold(self): + from openpilot.starpilot.longitudinal.force_stop_runtime import ForceStopRuntime + self.params.put_bool('ForceStops', True, block=True) + force = ForceStopRuntime(self.params) + for _ in range(45): + self.cycle(stopped=True, position=RED_97_X, velocity=RED_97_V, force_stop_owner=force, aol=True) + self.cycle(stopped=True, position=RED_97_X, velocity=RED_97_V, force_stop_owner=force, resume=True) + for _ in range(20): + _, _, _, plan = self.cycle(stopped=True, position=RED_97_X, velocity=RED_97_V, force_stop_owner=force) + self.assertFalse(self.planner.force_stop_plan.resume_override) + self.assertTrue(plan.shouldStop) + self.assertLessEqual(plan.aTarget, 0.) diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop.py b/openpilot/starpilot/longitudinal/tests/test_force_stop.py index 453adee9cd..ff8c4bbec2 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop.py +++ b/openpilot/starpilot/longitudinal/tests/test_force_stop.py @@ -73,12 +73,14 @@ class ForceStopTests(unittest.TestCase): 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) + self.assertEqual(self.owner.resume_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.assertTrue(released.resume_override) 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): @@ -228,3 +230,20 @@ class ForceStopTests(unittest.TestCase): if __name__ == '__main__': unittest.main() + + + def test_resume_override_is_only_for_a_previous_hold_and_ends_at_speed_lead_or_timeout(self): + for cause in ('speed', 'lead', 'tracked', 'timeout'): + with self.subTest(cause=cause): + self.owner.reset() + held = frame(0, speed_mps=0., horizon_m=30., standstill=True, raw_model_stopped=True) + self.owner.step(held, self.tune) + self.assertTrue(self.owner.step(replace(held, model_ns=BASE + TICK, resume_requested=True), self.tune).resume_override) + for tick in range(2, 123 if cause == 'timeout' else 3): + changes = {'speed': {'speed_mps': 2.}, 'lead': {'lead_present': True}, + 'tracked': {'tracking_lead': True}, 'timeout': {}}[cause] + result = self.owner.step(replace(held, model_ns=BASE + tick * TICK, **changes), self.tune) + self.assertFalse(result.resume_override) + self.owner.reset() + first = self.owner.step(frame(0, speed_mps=0., horizon_m=30., standstill=True, raw_model_stopped=True, resume_requested=True), self.tune) + self.assertFalse(first.resume_override) diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py b/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py index 171c7903ce..628126cfb7 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py +++ b/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py @@ -364,3 +364,79 @@ class ForceStopRuntimeTests(unittest.TestCase): if __name__ == '__main__': unittest.main() + + + def test_resume_from_aol_engages_but_does_not_override_red_light_model(self): + for same_tick in (False, True): + with self.subTest(same_tick=same_tick): + self.owner = ForceStopRuntime(self.params) + planner = LongitudinalPlanner(self.cp) + for tick in range(45): + sm = Frame(tick, stopped=True, horizon=30.) + sm['selfdriveState'].experimentalMode = True + sm['modelV2'].action.shouldStop = True + sm['modelV2'].action.desiredAcceleration = 0. + if tick < 40: + sm['carControl'].longActive = False + sm['carControl'].latActive = True + sm['selfdriveState'].enabled = False + if tick == 39: + self.owner.resume.observe(button_event(194), now_ns=BASE + 1_950_000_001, drive_id=DRIVE) + self.owner.resume.observe(button_event(195, [('accelCruise', True)]), now_ns=BASE + 1_950_000_001, drive_id=DRIVE) + if same_tick and tick == 40: + self.owner.resume.observe(button_event(199), now_ns=BASE + 2_000_000_001, drive_id=DRIVE) + self.owner.resume.observe(button_event(200, [('accelCruise', True)]), now_ns=BASE + 2_000_000_001, drive_id=DRIVE) + planner.update(sm, now_ns=sm.stamp + 1_000_000, drive_id=DRIVE, + force_stop_provider=lambda follow, source=sm: self.sample(source, follow_seconds=follow)) + self.assertFalse(getattr(planner.force_stop_plan, 'resume_override', False)) + if tick >= 40: + self.assertTrue(planner.output_should_stop) + self.assertLessEqual(planner.output_a_target, 0.) + + def test_res_releases_preexisting_force_hold_without_overriding_other_stop_candidates(self): + from dataclasses import replace + planner = LongitudinalPlanner(self.cp) + for tick in range(90): + sm = Frame(tick, stopped=True, horizon=30.) + sm['selfdriveState'].experimentalMode = True + sm['modelV2'].action.shouldStop = True + sm['modelV2'].action.desiredAcceleration = 0. + planner.update(sm, now_ns=sm.stamp + 1_000_000, drive_id=DRIVE, + force_stop_provider=lambda follow, source=sm: self.sample(source, follow_seconds=follow)) + self.assertTrue(planner.output_should_stop) + self.assertTrue(planner.force_stop_plan.manual_hold) + self.owner.resume.observe(button_event(448), now_ns=BASE + 4_490_000_001, drive_id=DRIVE) + self.owner.resume.observe(button_event(449, [('accelCruise', True)]), now_ns=BASE + 4_490_000_001, drive_id=DRIVE) + for tick in range(90, 110): + sm = Frame(tick, stopped=True, horizon=30.) + sm['carState'].vEgo = -1e-8 if tick % 2 else 0. + sm['selfdriveState'].experimentalMode = True + sm['modelV2'].action.shouldStop = True + sm['modelV2'].action.desiredAcceleration = 0. + planner.update(sm, now_ns=sm.stamp + 1_000_000, drive_id=DRIVE, + force_stop_provider=lambda follow, source=sm: self.sample(source, follow_seconds=follow)) + self.assertTrue(planner.force_stop_plan.resume_override) + self.assertFalse(planner.output_should_stop) + self.assertGreaterEqual(planner.output_a_target, .1) + resumed = planner.force_stop_plan + for defect in ('lead1', 'lead2', 'brake', 'gas', 'force', 'inactive', 'stale', 'zero_target'): + with self.subTest(defect=defect): + sm = Frame(110, stopped=True) + sm['selfdriveState'].experimentalMode = True + sm['modelV2'].action.shouldStop = True + sm['modelV2'].action.desiredAcceleration = 0. + if defect.startswith('lead'): + lead = sm['radarState'].leadOne if defect == 'lead1' else sm['radarState'].leadTwo + lead.present = True + elif defect in ('brake', 'gas'): + setattr(sm['carState'], defect + 'Pressed', True) + elif defect == 'force': + sm['controlsState'].forceDecel = True + elif defect == 'inactive': + sm['carControl'].longActive = False + elif defect == 'zero_target': + sm['carState'].vCruise = 0. + stamp = sm.stamp - 1 if defect == 'stale' else sm.stamp + planner.update(sm, now_ns=sm.stamp + 1_000_000, drive_id=DRIVE, + force_stop_provider=lambda _, source=stamp: replace(resumed, model_ns=source)) + self.assertTrue(planner.output_should_stop) diff --git a/prebuilt.json b/prebuilt.json index cccb809baa..3ec93a5729 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "a6f314fca4f5b54607ebf554388a182cd5afd1afeae910a917dc243024c46eb2", "schema_version": 1} +{"protected_inventory_sha256": "ad1ef8a01bea98c9638059a99aa8c875d91cd38d8448813d36846ed3f331fd72", "schema_version": 1}