Force Stop at "Detected" Stop Lights/Signs

frogpilot/controls/frogpilot_planner.py

@@ -123,6 +123,9 @@ class FrogPilotPlanner:
 
     frogpilotPlan.experimentalMode = self.frogpilot_cem.experimental_mode
 
+    frogpilotPlan.forcingStop = self.frogpilot_vcruise.forcing_stop
+    frogpilotPlan.forcingStopLength = self.frogpilot_vcruise.tracked_model_length
+
     frogpilotPlan.frogpilotEvents = self.frogpilot_events.events.to_msg()
 
     frogpilotPlan.frogpilotToggles = json.dumps(vars(frogpilot_toggles))

frogpilot/controls/lib/frogpilot_events.py

@@ -30,6 +30,9 @@ class FrogPilotEvents:
     else:
       self.max_acceleration = 0
 
+    if self.frogpilot_planner.frogpilot_vcruise.forcing_stop:
+      self.events.add(FrogPilotEventName.forcingStop)
+
     if self.error_log.is_file():
       self.events.add(FrogPilotEventName.openpilotCrashed)
 

frogpilot/controls/lib/frogpilot_vcruise.py

@@ -1,16 +1,41 @@
 #!/usr/bin/env python3
 from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_MDL
 
-from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED
+from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME
 from openpilot.frogpilot.controls.lib.curve_speed_controller import CurveSpeedController
 
+OVERRIDE_FORCE_STOP_TIMER = 10
+
 class FrogPilotVCruise:
   def __init__(self, FrogPilotPlanner):
     self.frogpilot_planner = FrogPilotPlanner
 
     self.csc = CurveSpeedController(self)
 
+    self.forcing_stop = False
+    self.override_force_stop = False
+
+    self.override_force_stop_timer = 0
+
   def update(self, long_control_active, now, time_validated, v_cruise, v_ego, sm, frogpilot_toggles):
+    force_stop = self.frogpilot_planner.frogpilot_cem.stop_light_detected and long_control_active and frogpilot_toggles.force_stops
+    force_stop &= self.frogpilot_planner.model_stopped
+    force_stop &= self.override_force_stop_timer <= 0
+
+    self.force_stop_timer = self.force_stop_timer + DT_MDL if force_stop else 0
+
+    force_stop_enabled = self.force_stop_timer >= 1
+
+    self.override_force_stop |= sm["carState"].gasPressed
+    self.override_force_stop |= sm["frogpilotCarState"].accelPressed
+    self.override_force_stop &= force_stop_enabled
+
+    if self.override_force_stop:
+      self.override_force_stop_timer = OVERRIDE_FORCE_STOP_TIMER
+    elif self.override_force_stop_timer > 0:
+      self.override_force_stop_timer -= DT_MDL
+
     v_cruise_cluster = max(sm["carState"].vCruiseCluster * CV.KPH_TO_MS, v_cruise)
     v_cruise_diff = v_cruise_cluster - v_cruise
 
@@ -32,7 +57,18 @@ class FrogPilotVCruise:
 
       self.csc_target = v_cruise
 
-    targets = [self.csc_target, v_cruise]
-    v_cruise = min([target if target >= CRUISING_SPEED else v_cruise for target in targets])
+    if force_stop_enabled and not self.override_force_stop:
+      self.forcing_stop |= not sm["carState"].standstill
+
+      self.tracked_model_length = max(self.tracked_model_length - (v_ego * DT_MDL), 0)
+      v_cruise = min((self.tracked_model_length // PLANNER_TIME), v_cruise)
+
+    else:
+      self.forcing_stop = False
+
+      self.tracked_model_length = self.frogpilot_planner.model_length
+
+      targets = [self.csc_target, v_cruise]
+      v_cruise = min([target if target >= CRUISING_SPEED else v_cruise for target in targets])
 
     return v_cruise

selfdrive/selfdrived/events.py

@@ -410,6 +410,17 @@ def custom_startup_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM
   return StartupAlert(frogpilot_toggles.startup_alert_top, frogpilot_toggles.startup_alert_bottom, alert_status=FrogPilotAlertStatus.frogpilot)
 
 
+def forcing_stop_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality, frogpilot_toggles: SimpleNamespace) -> Alert:
+  model_length = sm["frogpilotPlan"].forcingStopLength
+  model_length_msg = f"{model_length:.1f} meters" if metric else f"{model_length * CV.METER_TO_FOOT:.1f} feet"
+
+  return Alert(
+    f"Forcing the car to stop in {model_length_msg}",
+    "Press the gas pedal or 'Resume' button to override",
+    FrogPilotAlertStatus.frogpilot, AlertSize.mid,
+    Priority.MID, VisualAlert.none, AudibleAlert.prompt, 1.)
+
+
 
 EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
   # ********** events with no alerts **********
@@ -1055,6 +1066,10 @@ FROGPILOT_EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.PERMANENT: custom_startup_alert,
   },
 
+  FrogPilotEventName.forcingStop: {
+    ET.WARNING: forcing_stop_alert,
+  },
+
   FrogPilotEventName.openpilotCrashed: {
     ET.IMMEDIATE_DISABLE: Alert(
       "openpilot crashed",