Conditional Experimental Mode

Added toggles for "Conditional Experimental Mode".

Conditions based on road curvature, turn signals, speed, lead speed, navigation instructions, and stop signs/stop lights are all individually toggleable.

Co-Authored-By: eFini <16603033+efinilan@users.noreply.github.com>
Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>

cereal/custom.capnp

@@ -19,9 +19,12 @@ struct FrogPilotDeviceState @0xf35cc4560bbf6ec2 {
 }
 
 struct FrogPilotNavigation @0xda96579883444c35 {
+  approachingIntersection @0 :Bool;
+  approachingTurn @1 :Bool;
 }
 
 struct FrogPilotPlan @0x80ae746ee2596b11 {
+  conditionalExperimental @3 :Bool;
   jerk @7 :Float32;
   laneWidthLeft @8 :Float32;
   laneWidthRight @9 :Float32;

common/params.cc

@@ -220,6 +220,20 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"BlindSpotPath", PERSISTENT},
     {"CameraView", PERSISTENT},
     {"CameraViewReset", PERSISTENT},
+    {"CECurves", PERSISTENT},
+    {"CECurvesLead", PERSISTENT},
+    {"CENavigation", PERSISTENT},
+    {"CENavigationIntersections", PERSISTENT},
+    {"CENavigationLead", PERSISTENT},
+    {"CENavigationTurns", PERSISTENT},
+    {"CESignal", PERSISTENT},
+    {"CESlowerLead", PERSISTENT},
+    {"CESpeed", PERSISTENT},
+    {"CESpeedLead", PERSISTENT},
+    {"CEStatus", PERSISTENT},
+    {"CEStopLights", PERSISTENT},
+    {"CEStopLightsLead", PERSISTENT},
+    {"ConditionalExperimental", PERSISTENT},
     {"CustomAlerts", PERSISTENT},
     {"CustomPaths", PERSISTENT},
     {"CustomUI", PERSISTENT},
@@ -229,6 +243,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"FrogPilotTogglesUpdated", PERSISTENT},
     {"FrogsGoMoo", PERSISTENT},
     {"HideAOLStatusBar", PERSISTENT},
+    {"HideCEMStatusBar", PERSISTENT},
     {"LateralTune", PERSISTENT},
     {"LongitudinalTune", PERSISTENT},
     {"ManualUpdateInitiated", PERSISTENT},

release/files_common

@@ -566,4 +566,5 @@ tinygrad_repo/tinygrad/*.py
 
 selfdrive/frogpilot/frogpilot_process.py
 selfdrive/frogpilot/controls/frogpilot_planner.py
+selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
 selfdrive/frogpilot/controls/lib/frogpilot_functions.py

selfdrive/controls/controlsd.py

@@ -866,7 +866,8 @@ class Controls:
   def params_thread(self, evt):
     while not evt.is_set():
       self.is_metric = self.params.get_bool("IsMetric")
-      self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
+      if self.CP.openpilotLongitudinalControl and not self.frogpilot_variables.conditional_experimental_mode:
+        self.experimental_mode = self.params.get_bool("ExperimentalMode")
       self.personality = self.read_personality_param()
       if self.CP.notCar:
         self.joystick_mode = self.params.get_bool("JoystickDebugMode")
@@ -898,6 +899,9 @@ class Controls:
     self.FPCC.alwaysOnLateral &= self.driving_gear
     self.FPCC.alwaysOnLateral &= not (CS.brakePressed and CS.vEgo < self.always_on_lateral_pause_speed) or CS.standstill
 
+    if self.CP.openpilotLongitudinalControl and self.frogpilot_variables.conditional_experimental_mode:
+      self.experimental_mode = self.sm['frogpilotPlan'].conditionalExperimental
+
     fpcc_send = messaging.new_message('frogpilotCarControl')
     fpcc_send.valid = CS.canValid
     fpcc_send.frogpilotCarControl = self.FPCC
@@ -909,6 +913,8 @@ class Controls:
   def update_frogpilot_params(self):
     self.always_on_lateral_pause_speed = self.always_on_lateral and self.params.get_int("PauseAOLOnBrake")
 
+    self.frogpilot_variables.conditional_experimental_mode = self.CP.openpilotLongitudinalControl and self.params.get_bool("ConditionalExperimental")
+
     custom_alerts = self.params.get_bool("CustomAlerts")
 
     lateral_tune = self.params.get_bool("LateralTune")

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -14,6 +14,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN,
 
 from openpilot.system.version import get_short_branch
 
+from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, calculate_lane_width, calculate_road_curvature
 
 # Acceleration profiles - Credit goes to the DragonPilot team!
@@ -47,6 +48,8 @@ class FrogPilotPlanner:
     self.params = Params()
     self.params_memory = Params("/dev/shm/params")
 
+    self.cem = ConditionalExperimentalMode()
+
     self.staging = get_short_branch() in ["FrogPilot-Development", "FrogPilot-Staging", "FrogPilot-Testing"]
 
     self.jerk = 0
@@ -95,6 +98,9 @@ class FrogPilotPlanner:
 
     self.v_cruise = self.update_v_cruise(carState, controlsState, controlsState.enabled, liveLocationKalman, modelData, road_curvature, v_cruise, v_ego)
 
+    if self.conditional_experimental_mode and self.CP.openpilotLongitudinalControl:
+      self.cem.update(carState, controlsState.enabled, frogpilotNavigation, modelData, radarState, road_curvature, self.t_follow, v_ego)
+
   def update_follow_values(self, jerk, radarState, t_follow, v_ego, v_lead):
     lead_distance = radarState.leadOne.dRel
 
@@ -132,6 +138,7 @@ class FrogPilotPlanner:
     frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
     frogpilotPlan = frogpilot_plan_send.frogpilotPlan
 
+    frogpilotPlan.conditionalExperimental = self.cem.experimental_mode
     frogpilotPlan.jerk = float(self.jerk)
     frogpilotPlan.laneWidthLeft = self.lane_width_left
     frogpilotPlan.laneWidthRight = self.lane_width_right
@@ -145,6 +152,10 @@ class FrogPilotPlanner:
   def update_frogpilot_params(self):
     self.is_metric = self.params.get_bool("IsMetric")
 
+    self.conditional_experimental_mode = self.CP.openpilotLongitudinalControl and self.params.get_bool("ConditionalExperimental")
+    if self.conditional_experimental_mode:
+      self.cem.update_frogpilot_params()
+
     custom_alerts = self.params.get_bool("CustomAlerts")
 
     custom_ui = self.params.get_bool("CustomUI")

selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py

@@ -0,0 +1,182 @@
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import interp
+from openpilot.common.params import Params
+
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, PROBABILITY, MovingAverageCalculator
+
+SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
+SLOW_DOWN_DISTANCE = [20, 30., 50., 70., 80., 90., 105., 120.]
+TRAJECTORY_SIZE = 33
+
+class ConditionalExperimentalMode:
+  def __init__(self):
+    self.params = Params()
+    self.params_memory = Params("/dev/shm/params")
+
+    self.curve_detected = False
+    self.experimental_mode = False
+    self.lead_detected = False
+    self.red_light_detected = False
+    self.slower_lead_detected = False
+
+    self.previous_status_value = 0
+    self.previous_v_ego = 0
+    self.previous_v_lead = 0
+    self.status_value = 0
+
+    self.curvature_mac = MovingAverageCalculator()
+    self.lead_detection_mac = MovingAverageCalculator()
+    self.lead_slowing_down_mac = MovingAverageCalculator()
+    self.slow_lead_mac = MovingAverageCalculator()
+    self.slowing_down_mac = MovingAverageCalculator()
+    self.stop_light_mac = MovingAverageCalculator()
+
+    self.update_frogpilot_params()
+
+  def update(self, carState, enabled, frogpilotNavigation, modelData, radarState, road_curvature, t_follow, v_ego):
+    lead = radarState.leadOne
+    lead_distance = lead.dRel
+    standstill = carState.standstill
+    v_lead = lead.vLead
+
+    self.update_conditions(lead_distance, lead.status, modelData, road_curvature, standstill, t_follow, v_ego, v_lead)
+
+    condition_met = self.check_conditions(carState, frogpilotNavigation, lead, modelData, standstill, v_ego) and enabled
+    if condition_met:
+      self.experimental_mode = True
+    else:
+      self.experimental_mode = False
+      self.status_value = 0
+
+    if self.status_value != self.previous_status_value:
+      self.params_memory.put_int("CEStatus", self.status_value)
+      self.previous_status_value = self.status_value
+
+    if self.params_memory.get_bool("FrogPilotTogglesUpdated"):
+      self.update_frogpilot_params()
+
+  def check_conditions(self, carState, frogpilotNavigation, lead, modelData, standstill, v_ego):
+    if standstill:
+      self.status_value = 0
+      return self.experimental_mode
+
+    # Keep Experimental Mode active if stopping for a red light
+    if self.status_value == 15 and self.slowing_down(v_ego):
+      return True
+
+    if self.navigation and modelData.navEnabled and (frogpilotNavigation.approachingIntersection or frogpilotNavigation.approachingTurn) and (self.navigation_lead or not self.lead_detected):
+      self.status_value = 7 if frogpilotNavigation.approachingIntersection else 8
+      return True
+
+    if (not self.lead_detected and v_ego <= self.limit) or (self.lead_detected and v_ego <= self.limit_lead):
+      self.status_value = 10 if self.lead_detected else 11
+      return True
+
+    if self.slower_lead and self.slower_lead_detected:
+      self.status_value = 12
+      return True
+
+    if self.signal and v_ego <= CITY_SPEED_LIMIT and (carState.leftBlinker or carState.rightBlinker):
+      self.status_value = 13
+      return True
+
+    if self.curves and self.curve_detected:
+      self.status_value = 14
+      return True
+
+    if self.stop_lights and self.red_light_detected:
+      self.status_value = 15
+      return True
+
+    return False
+
+  def update_conditions(self, lead_distance, lead_status, modelData, road_curvature, standstill, t_follow, v_ego, v_lead):
+    self.lead_detection(lead_status)
+    self.road_curvature(road_curvature)
+    self.slow_lead(lead_distance, t_follow, v_ego)
+    self.stop_sign_and_light(lead_distance, modelData, standstill, v_ego, v_lead)
+
+  def lead_detection(self, lead_status):
+    self.lead_detection_mac.add_data(lead_status)
+    self.lead_detected = self.lead_detection_mac.get_moving_average() >= PROBABILITY
+
+  def lead_slowing_down(self, lead_distance, v_ego, v_lead):
+    if self.lead_detected:
+      lead_close = lead_distance < CITY_SPEED_LIMIT
+      lead_far = lead_distance >= CITY_SPEED_LIMIT and (v_lead >= self.previous_v_lead > 1 or v_lead > v_ego)
+      lead_slowing_down = v_lead < self.previous_v_lead
+      lead_stopped = v_lead < 1
+
+      self.previous_v_lead = v_lead
+
+      self.lead_slowing_down_mac.add_data((lead_close or lead_slowing_down or lead_stopped) and not lead_far)
+      return self.lead_slowing_down_mac.get_moving_average() >= PROBABILITY
+    else:
+      self.lead_slowing_down_mac.reset_data()
+      self.previous_v_lead = 0
+      return False
+
+  # Determine the road curvature - Credit goes to to Pfeiferj!
+  def road_curvature(self, road_curvature):
+    lead_check = self.curves_lead or not self.lead_detected
+
+    if lead_check and not self.red_light_detected:
+      # Setting a limit of 3.5 helps prevent it triggering for red lights
+      curve_detected = 3.5 >= road_curvature > 1.6
+      curve_active = 3.5 >= road_curvature > 1.1 and self.curve_detected
+
+      self.curvature_mac.add_data(curve_detected or curve_active)
+      self.curve_detected = self.curvature_mac.get_moving_average() >= PROBABILITY
+    else:
+      self.curvature_mac.reset_data()
+      self.curve_detected = False
+
+  def slow_lead(self, lead_distance, t_follow, v_ego):
+    if self.lead_detected:
+      slower_lead_ahead = lead_distance < (v_ego - 1) * t_follow
+
+      self.slow_lead_mac.add_data(slower_lead_ahead)
+      self.slower_lead_detected = self.slow_lead_mac.get_moving_average() >= PROBABILITY
+    else:
+      self.slow_lead_mac.reset_data()
+      self.slower_lead_detected = False
+
+  def slowing_down(self, v_ego):
+    slowing_down = v_ego <= self.previous_v_ego
+    speed_check = v_ego < CRUISING_SPEED
+
+    self.previous_v_ego = v_ego
+
+    self.slowing_down_mac.add_data(slowing_down and speed_check)
+    return self.slowing_down_mac.get_moving_average() >= PROBABILITY
+
+  # Stop sign/stop light detection - Credit goes to the DragonPilot team!
+  def stop_sign_and_light(self, lead_distance, modelData, standstill, v_ego, v_lead):
+    lead_check = self.stop_lights_lead or not self.lead_slowing_down(lead_distance, v_ego, v_lead) or standstill
+
+    model_check = len(modelData.orientation.x) == len(modelData.position.x) == TRAJECTORY_SIZE
+    model_stopping = modelData.position.x[TRAJECTORY_SIZE - 1] < interp(v_ego * CV.MS_TO_KPH, SLOW_DOWN_BP, SLOW_DOWN_DISTANCE)
+
+    model_filtered = not (self.curve_detected or self.slower_lead_detected)
+
+    self.stop_light_mac.add_data(lead_check and model_check and model_stopping and model_filtered)
+    self.red_light_detected = self.stop_light_mac.get_moving_average() >= PROBABILITY
+
+  def update_frogpilot_params(self):
+    is_metric = self.params.get_bool("IsMetric")
+
+    self.curves = self.params.get_bool("CECurves")
+    self.curves_lead = self.curves and self.params.get_bool("CECurvesLead")
+
+    self.limit = self.params.get_int("CESpeed") * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS)
+    self.limit_lead = self.params.get_int("CESpeedLead") * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS)
+
+    self.navigation = self.params.get_bool("CENavigation")
+    self.navigation_lead = self.navigation and self.params.get_bool("CENavigationLead")
+
+    self.signal = self.params.get_bool("CESignal")
+
+    self.slower_lead = self.params.get_bool("CESlowerLead")
+
+    self.stop_lights = self.params.get_bool("CEStopLights")
+    self.stop_lights_lead = self.stop_lights and self.params.get_bool("CEStopLightsLead")

selfdrive/frogpilot/ui/qt/offroad/control_settings.cc

@@ -10,6 +10,14 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     {"PauseAOLOnBrake", tr("Pause On Brake"), tr("Pause 'Always On Lateral' when the brake pedal is being pressed below the set speed."), ""},
     {"HideAOLStatusBar", tr("Hide the Status Bar"), tr("Don't use the status bar for 'Always On Lateral'."), ""},
 
+    {"ConditionalExperimental", tr("Conditional Experimental Mode"), tr("Automatically switches to 'Experimental Mode' under predefined conditions."), "../frogpilot/assets/toggle_icons/icon_conditional.png"},
+    {"CECurves", tr("Curve Detected Ahead"), tr("Switch to 'Experimental Mode' when a curve is detected."), ""},
+    {"CENavigation", tr("Navigation Based"), tr("Switch to 'Experimental Mode' based on navigation data. (i.e. Intersections, stop signs, upcoming turns, etc.)"), ""},
+    {"CESlowerLead", tr("Slower Lead Detected Ahead"), tr("Switch to 'Experimental Mode' when a slower lead vehicle is detected ahead."), ""},
+    {"CEStopLights", tr("Stop Lights and Stop Signs"), tr("Switch to 'Experimental Mode' when a stop light or stop sign is detected."), ""},
+    {"CESignal", tr("Turn Signal When Below Highway Speeds"), tr("Switch to 'Experimental Mode' when using turn signals below highway speeds to help assist with turns."), ""},
+    {"HideCEMStatusBar", tr("Hide the Status Bar"), tr("Don't use the status bar for 'Conditional Experimental Mode'."), ""},
+
     {"LateralTune", tr("Lateral Tuning"), tr("Modify openpilot's steering behavior."), "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"},
 
     {"LongitudinalTune", tr("Longitudinal Tuning"), tr("Modify openpilot's acceleration and braking behavior."), "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"},
@@ -35,6 +43,44 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     } else if (param == "PauseAOLOnBrake") {
       toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 99, std::map<int, QString>(), this, false, tr(" mph"));
 
+    } else if (param == "ConditionalExperimental") {
+      FrogPilotParamManageControl *conditionalExperimentalToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
+      QObject::connect(conditionalExperimentalToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
+        openParentToggle();
+        conditionalSpeedsImperial->setVisible(!isMetric);
+        conditionalSpeedsMetric->setVisible(isMetric);
+        for (auto &[key, toggle] : toggles) {
+          toggle->setVisible(conditionalExperimentalKeys.find(key.c_str()) != conditionalExperimentalKeys.end());
+        }
+      });
+      toggle = conditionalExperimentalToggle;
+    } else if (param == "CECurves") {
+      FrogPilotParamValueControl *CESpeedImperial = new FrogPilotParamValueControl("CESpeed", tr("Below"), tr("Switch to 'Experimental Mode' below this speed when not following a lead vehicle."), "", 0, 99,
+                                                                                   std::map<int, QString>(), this, false, tr(" mph"));
+      FrogPilotParamValueControl *CESpeedLeadImperial = new FrogPilotParamValueControl("CESpeedLead", tr("  w/Lead"), tr("Switch to 'Experimental Mode' below this speed when following a lead vehicle."), "", 0, 99,
+                                                                                       std::map<int, QString>(), this, false, tr(" mph"));
+      conditionalSpeedsImperial = new FrogPilotDualParamControl(CESpeedImperial, CESpeedLeadImperial, this);
+      addItem(conditionalSpeedsImperial);
+
+      FrogPilotParamValueControl *CESpeedMetric = new FrogPilotParamValueControl("CESpeed", tr("Below"), tr("Switch to 'Experimental Mode' below this speed in absence of a lead vehicle."), "", 0, 150,
+                                                                                 std::map<int, QString>(), this, false, tr(" kph"));
+      FrogPilotParamValueControl *CESpeedLeadMetric = new FrogPilotParamValueControl("CESpeedLead", tr("  w/Lead"), tr("Switch to 'Experimental Mode' below this speed when following a lead vehicle."), "", 0, 150,
+                                                                                     std::map<int, QString>(), this, false, tr(" kph"));
+      conditionalSpeedsMetric = new FrogPilotDualParamControl(CESpeedMetric, CESpeedLeadMetric, this);
+      addItem(conditionalSpeedsMetric);
+
+      std::vector<QString> curveToggles{"CECurvesLead"};
+      std::vector<QString> curveToggleNames{tr("With Lead")};
+      toggle = new FrogPilotParamToggleControl(param, title, desc, icon, curveToggles, curveToggleNames);
+    } else if (param == "CENavigation") {
+      std::vector<QString> navigationToggles{"CENavigationIntersections", "CENavigationTurns", "CENavigationLead"};
+      std::vector<QString> navigationToggleNames{tr("Intersections"), tr("Turns"), tr("With Lead")};
+      toggle = new FrogPilotParamToggleControl(param, title, desc, icon, navigationToggles, navigationToggleNames);
+    } else if (param == "CEStopLights") {
+      std::vector<QString> stopLightToggles{"CEStopLightsLead"};
+      std::vector<QString> stopLightToggleNames{tr("With Lead")};
+      toggle = new FrogPilotParamToggleControl(param, title, desc, icon, stopLightToggles, stopLightToggleNames);
+
     } else if (param == "LateralTune") {
       FrogPilotParamManageControl *lateralTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
       QObject::connect(lateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
@@ -189,6 +235,8 @@ void FrogPilotControlsPanel::updateMetric() {
     double distanceConversion = isMetric ? FOOT_TO_METER : METER_TO_FOOT;
     double speedConversion = isMetric ? MILE_TO_KM : KM_TO_MILE;
 
+    params.putIntNonBlocking("CESpeed", std::nearbyint(params.getInt("CESpeed") * speedConversion));
+    params.putIntNonBlocking("CESpeedLead", std::nearbyint(params.getInt("CESpeedLead") * speedConversion));
     params.putIntNonBlocking("PauseAOLOnBrake", std::nearbyint(params.getInt("PauseAOLOnBrake") * speedConversion));
   }
 
@@ -204,7 +252,10 @@ void FrogPilotControlsPanel::updateMetric() {
 }
 
 void FrogPilotControlsPanel::hideToggles() {
-  std::set<QString> longitudinalKeys = {"LongitudinalTune"};
+  conditionalSpeedsImperial->setVisible(false);
+  conditionalSpeedsMetric->setVisible(false);
+
+  std::set<QString> longitudinalKeys = {"ConditionalExperimental", "LongitudinalTune"};
 
   for (auto &[key, toggle] : toggles) {
     toggle->setVisible(false);

selfdrive/frogpilot/ui/qt/offroad/control_settings.h

@@ -25,8 +25,11 @@ private:
   void updateState();
   void updateToggles();
 
+  FrogPilotDualParamControl *conditionalSpeedsImperial;
+  FrogPilotDualParamControl *conditionalSpeedsMetric;
+
   std::set<QString> aolKeys = {"AlwaysOnLateralMain", "HideAOLStatusBar", "PauseAOLOnBrake"};
-  std::set<QString> conditionalExperimentalKeys = {};
+  std::set<QString> conditionalExperimentalKeys = {"CECurves", "CECurvesLead", "CENavigation", "CESignal", "CESlowerLead", "CEStopLights", "HideCEMStatusBar"};
   std::set<QString> deviceManagementKeys = {};
   std::set<QString> experimentalModeActivationKeys = {};
   std::set<QString> laneChangeKeys = {};

selfdrive/navd/navd.py

@@ -9,6 +9,7 @@ import requests
 import cereal.messaging as messaging
 from cereal import log
 from openpilot.common.api import Api
+from openpilot.common.numpy_fast import interp
 from openpilot.common.params import Params
 from openpilot.common.realtime import Ratekeeper
 from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
@@ -61,6 +62,12 @@ class RouteEngine:
       self.mapbox_host = "https://maps.comma.ai"
 
     # FrogPilot variables
+    self.stop_coord = []
+    self.stop_signal = []
+
+    self.approaching_intersection = False
+    self.approaching_turn = False
+
     self.update_frogpilot_params()
 
   def update(self):
@@ -169,6 +176,17 @@ class RouteEngine:
         self.route = r['routes'][0]['legs'][0]['steps']
         self.route_geometry = []
 
+        # Iterate through the steps in self.route to find "stop_sign" and "traffic_light"
+        if self.conditional_navigation_intersections:
+          self.stop_signal = []
+          self.stop_coord = []
+
+          for step in self.route:
+            for intersection in step["intersections"]:
+              if "stop_sign" in intersection or "traffic_signal" in intersection:
+                self.stop_signal.append(intersection["geometry_index"])
+                self.stop_coord.append(Coordinate.from_mapbox_tuple(intersection["location"]))
+
         maxspeed_idx = 0
         maxspeeds = r['routes'][0]['legs'][0]['annotation']['maxspeed']
 
@@ -309,9 +327,35 @@ class RouteEngine:
           self.params.remove("NavDestination")
           self.clear_route()
 
+    # 5-10 Seconds to stop condition based on the current speed or minimum of 25 meters
+    if self.conditional_navigation:
+      v_ego = self.sm['carState'].vEgo
+      seconds_to_stop = interp(v_ego, [0, 22.3, 44.7], [5, 10, 10])
+
+      # Determine the location of the closest upcoming stopSign or trafficLight
+      closest_condition_indices = [idx for idx in self.stop_signal if idx >= closest_idx]
+      if closest_condition_indices:
+        closest_condition_index = min(closest_condition_indices, key=lambda idx: abs(closest_idx - idx))
+        index = self.stop_signal.index(closest_condition_index)
+
+        # Calculate the distance to the stopSign or trafficLight
+        distance_to_condition = self.last_position.distance_to(self.stop_coord[index])
+        self.approaching_intersection = self.conditional_navigation_intersections and distance_to_condition < max((seconds_to_stop * v_ego), 25)
+      else:
+        self.approaching_intersection = False  # No more stopSign or trafficLight in array
+
+      # Determine if NoO distance to maneuver is upcoming
+      self.approaching_turn = self.conditional_navigation_turns and distance_to_maneuver_along_geometry < max((seconds_to_stop * v_ego), 25)
+    else:
+      self.approaching_intersection = False
+      self.approaching_turn = False
+
     frogpilot_plan_send = messaging.new_message('frogpilotNavigation')
     frogpilotNavigation = frogpilot_plan_send.frogpilotNavigation
 
+    frogpilotNavigation.approachingIntersection = self.approaching_intersection
+    frogpilotNavigation.approachingTurn = self.approaching_turn
+
     self.pm.send('frogpilotNavigation', frogpilot_plan_send)
 
   def send_route(self):
@@ -363,10 +407,13 @@ class RouteEngine:
     # TODO: Check for going wrong way in segment
 
   def update_frogpilot_params(self):
+    self.conditional_navigation = self.params.get_bool("CENavigation")
+    self.conditional_navigation_intersections = self.conditional_navigation and self.params.get_bool("CENavigationIntersections")
+    self.conditional_navigation_turns = self.conditional_navigation and self.params.get_bool("CENavigationTurns")
 
 def main():
   pm = messaging.PubMaster(['navInstruction', 'navRoute', 'frogpilotNavigation'])
-  sm = messaging.SubMaster(['liveLocationKalman', 'managerState'])
+  sm = messaging.SubMaster(['carState', 'liveLocationKalman', 'managerState'])
 
   rk = Ratekeeper(1.0)
   route_engine = RouteEngine(sm, pm)

selfdrive/ui/qt/offroad/settings.cc

@@ -194,7 +194,12 @@ void TogglesPanel::updateToggles() {
     op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable() && !is_release);
     if (hasLongitudinalControl(CP)) {
       // normal description and toggle
-      experimental_mode_toggle->setEnabled(true);
+      bool conditional_experimental = params.getBool("ConditionalExperimental");
+      if (conditional_experimental) {
+        params.putBool("ExperimentalMode", true);
+        experimental_mode_toggle->refresh();
+      }
+      experimental_mode_toggle->setEnabled(!conditional_experimental);
       experimental_mode_toggle->setDescription(e2e_description);
       long_personality_setting->setEnabled(true);
     } else {

selfdrive/ui/qt/onroad.cc

@@ -173,7 +173,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) {
 
   int margin = 40;
   int radius = 30;
-  int offset = scene.show_aol_status_bar ? 25 : 0;
+  int offset = scene.show_aol_status_bar || scene.show_cem_status_bar ? 25 : 0;
   if (alert.size == cereal::ControlsState::AlertSize::FULL) {
     margin = 0;
     radius = 0;
@@ -563,7 +563,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s)
   // base icon
   int offset = UI_BORDER_SIZE + btn_size / 2;
   int x = rightHandDM ? width() - offset : offset;
-  offset += showAlwaysOnLateralStatusBar ? 25 : 0;
+  offset += showAlwaysOnLateralStatusBar || showConditionalExperimentalStatusBar ? 25 : 0;
   int y = height() - offset;
   float opacity = dmActive ? 0.65 : 0.2;
   drawIcon(painter, QPoint(x, y), dm_img, blackColor(70), opacity);
@@ -760,13 +760,16 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() {
 
   cameraView = scene.camera_view;
 
+  conditionalStatus = scene.conditional_status;
+  showConditionalExperimentalStatusBar = scene.show_cem_status_bar;
+
   experimentalMode = scene.experimental_mode;
 
   mapOpen = scene.map_open;
 }
 
 void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) {
-  if (showAlwaysOnLateralStatusBar) {
+  if (showAlwaysOnLateralStatusBar || showConditionalExperimentalStatusBar) {
     drawStatusBar(p);
   }
 
@@ -797,8 +800,29 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
   p.setOpacity(1.0);
   p.drawRoundedRect(statusBarRect, 30, 30);
 
+  std::map<int, QString> conditionalStatusMap = {
+    {0, tr("Conditional Experimental Mode ready")},
+    {1, tr("Conditional Experimental overridden")},
+    {2, tr("Experimental Mode manually activated")},
+    {3, tr("Conditional Experimental overridden")},
+    {4, tr("Experimental Mode manually activated")},
+    {5, tr("Conditional Experimental overridden")},
+    {6, tr("Experimental Mode manually activated")},
+    {7, tr("Experimental Mode activated for") + (mapOpen ? tr(" intersection") : tr(" upcoming intersection"))},
+    {8, tr("Experimental Mode activated for") + (mapOpen ? tr(" turn") : tr(" upcoming turn"))},
+    {9, tr("Experimental Mode activated due to") + (mapOpen ? tr(" SLC") : tr(" no speed limit set"))},
+    {10, tr("Experimental Mode activated due to") + (mapOpen ? tr(" speed") : tr(" speed being less than ") + QString::number(scene.conditional_speed_lead) + (is_metric ? tr(" kph") : tr(" mph")))},
+    {11, tr("Experimental Mode activated due to") + (mapOpen ? tr(" speed") : tr(" speed being less than ") + QString::number(scene.conditional_speed) + (is_metric ? tr(" kph") : tr(" mph")))},
+    {12, tr("Experimental Mode activated for slower lead")},
+    {13, tr("Experimental Mode activated for turn") + (mapOpen ? "" : tr(" / lane change"))},
+    {14, tr("Experimental Mode activated for curve")},
+    {15, tr("Experimental Mode activated for stop") + (mapOpen ? "" : tr(" sign / stop light"))},
+  };
+
   if (alwaysOnLateralActive && showAlwaysOnLateralStatusBar) {
     newStatus = tr("Always On Lateral active") + (mapOpen ? "" : tr(". Press the \"Cruise Control\" button to disable"));
+  } else if (showConditionalExperimentalStatusBar) {
+    newStatus = conditionalStatusMap[status != STATUS_DISENGAGED ? conditionalStatus : 0];
   }
 
   if (newStatus != lastShownStatus) {

selfdrive/ui/qt/onroad.h

@@ -124,9 +124,11 @@ private:
   bool experimentalMode;
   bool mapOpen;
   bool showAlwaysOnLateralStatusBar;
+  bool showConditionalExperimentalStatusBar;
 
   int alertSize;
   int cameraView;
+  int conditionalStatus;
 
 protected:
   void paintGL() override;

selfdrive/ui/ui.cc

@@ -263,6 +263,11 @@ void ui_update_frogpilot_params(UIState *s) {
   bool always_on_lateral = params.getBool("AlwaysOnLateral");
   scene.show_aol_status_bar = always_on_lateral && !params.getBool("HideAOLStatusBar");
 
+  scene.conditional_experimental = scene.longitudinal_control && params.getBool("ConditionalExperimental");
+  scene.conditional_speed = scene.conditional_experimental ? params.getInt("CESpeed") : 0;
+  scene.conditional_speed_lead = scene.conditional_experimental ? params.getInt("CESpeedLead") : 0;
+  scene.show_cem_status_bar = scene.conditional_experimental && !params.getBool("HideCEMStatusBar");
+
   bool custom_onroad_ui = params.getBool("CustomUI");
   bool custom_paths = custom_onroad_ui && params.getBool("CustomPaths");
   scene.acceleration_path = custom_paths && params.getBool("AccelerationPath");
@@ -340,6 +345,7 @@ void UIState::update() {
   }
 
   // FrogPilot live variables that need to be constantly checked
+  scene.conditional_status = scene.conditional_experimental ? paramsMemory.getInt("CEStatus") : 0;
 }
 
 void UIState::setPrimeType(PrimeType type) {

selfdrive/ui/ui.h

@@ -176,11 +176,13 @@ typedef struct UIScene {
   bool blind_spot_left;
   bool blind_spot_path;
   bool blind_spot_right;
+  bool conditional_experimental;
   bool enabled;
   bool experimental_mode;
   bool map_open;
   bool right_hand_drive;
   bool show_aol_status_bar;
+  bool show_cem_status_bar;
 
   float acceleration;
   float lane_width_left;
@@ -188,6 +190,9 @@ typedef struct UIScene {
 
   int alert_size;
   int camera_view;
+  int conditional_speed;
+  int conditional_speed_lead;
+  int conditional_status;
 
   QPolygonF track_adjacent_vertices[6];