Release an existing Force Stop hold with RES

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))

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:

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)

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.)

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)

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)

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "a6f314fca4f5b54607ebf554388a182cd5afd1afeae910a917dc243024c46eb2", "schema_version": 1}
+{"protected_inventory_sha256": "ad1ef8a01bea98c9638059a99aa8c875d91cd38d8448813d36846ed3f331fd72", "schema_version": 1}