From b287e50982dbe709fc74ecaaee42bc548b7fb783 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Wed, 31 Jul 2024 17:33:31 -0700
Subject: [PATCH] Controls - Conditional Experimental Mode - Stop Lights and
 Stop Signs

Switch to 'Experimental Mode' when a stop light or stop sign is detected.
---
 .../frogpilot/controls/frogpilot_planner.py    |  6 +++++-
 .../lib/conditional_experimental_mode.py       | 18 ++++++++++++++++++
 2 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py
index be72f0fbe..71f0d2bff 100644
--- a/selfdrive/frogpilot/controls/frogpilot_planner.py
+++ b/selfdrive/frogpilot/controls/frogpilot_planner.py
@@ -13,7 +13,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHA
                                                                            get_jerk_factor, get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
 from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, Lead, get_max_accel
 
-from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
+from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode import MODEL_LENGTH, PLANNER_TIME, ConditionalExperimentalMode
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator, calculate_lane_width, calculate_road_curvature
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, PROBABILITY
 
@@ -25,11 +25,13 @@ class FrogPilotPlanner:
 
     self.cem = ConditionalExperimentalMode(self)
 
+    self.model_stopped = False
     self.slower_lead = False
     self.tracking_lead = False
 
     self.acceleration_jerk = 0
     self.danger_jerk = 0
+    self.model_length = 0
     self.road_curvature = 0
     self.speed_jerk = 0
     self.v_cruise = 0
@@ -59,6 +61,8 @@ class FrogPilotPlanner:
       self.lane_width_left = 0
       self.lane_width_right = 0
 
+    self.model_length = modelData.position.x[MODEL_LENGTH - 1]
+    self.model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME
     self.road_curvature = calculate_road_curvature(modelData, v_ego) if not carState.standstill and driving_gear else 1
 
     self.set_acceleration(controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles)
diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
index 0cdf7ec49..cf58dbe03 100644
--- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
+++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
@@ -1,8 +1,12 @@
 from openpilot.common.params import Params
+from openpilot.selfdrive.modeld.constants import ModelConstants
 
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, PROBABILITY
 
+MODEL_LENGTH = ModelConstants.IDX_N
+PLANNER_TIME = ModelConstants.T_IDXS[MODEL_LENGTH - 1]
+
 class ConditionalExperimentalMode:
   def __init__(self, FrogPilotPlanner):
     self.params_memory = Params("/dev/shm/params")
@@ -11,9 +15,11 @@ class ConditionalExperimentalMode:
 
     self.curvature_mac = MovingAverageCalculator()
     self.slow_lead_mac = MovingAverageCalculator()
+    self.stop_light_mac = MovingAverageCalculator()
 
     self.curve_detected = False
     self.experimental_mode = False
+    self.stop_light_detected = False
 
   def update(self, carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles):
     if not carState.standstill:
@@ -43,11 +49,16 @@ class ConditionalExperimentalMode:
       self.status_value = 13 if v_lead < 1 else 14
       return True
 
+    if frogpilot_toggles.conditional_stop_lights and self.stop_light_detected:
+      self.status_value = 15
+      return True
+
     return False
 
   def update_conditions(self, tracking_lead, v_ego, v_lead, frogpilot_toggles):
     self.curve_detection(v_ego, frogpilot_toggles)
     self.slow_lead(tracking_lead, v_lead, frogpilot_toggles)
+    self.stop_sign_and_light(tracking_lead, v_ego, frogpilot_toggles)
 
   def curve_detection(self, v_ego, frogpilot_toggles):
     curve_detected = (1 / self.frogpilot_planner.road_curvature)**0.5 < v_ego
@@ -66,3 +77,10 @@ class ConditionalExperimentalMode:
     else:
       self.slow_lead_mac.reset_data()
       self.slow_lead_detected = False
+
+  def stop_sign_and_light(self, tracking_lead, v_ego, frogpilot_toggles):
+    model_projection = PLANNER_TIME - (5 if frogpilot_toggles.less_sensitive_lights else 3)
+    model_stopping = self.frogpilot_planner.model_length < v_ego * model_projection
+
+    self.stop_light_mac.add_data((self.frogpilot_planner.model_stopped or model_stopping) and not (self.curve_detected or tracking_lead))
+    self.stop_light_detected = self.stop_light_mac.get_moving_average() >= PROBABILITY