Conditional Experimental Mode

2026-01-08 01:31:26 -07:00
parent 7388432fb9
commit 68b60b3635
17 changed files with 206 additions and 3 deletions
@@ -12,6 +12,7 @@ from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE

 from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME, THRESHOLD
+from openpilot.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
 from openpilot.frogpilot.controls.lib.frogpilot_acceleration import FrogPilotAcceleration
 from openpilot.frogpilot.controls.lib.frogpilot_events import FrogPilotEvents
 from openpilot.frogpilot.controls.lib.frogpilot_following import FrogPilotFollowing
@@ -23,6 +24,7 @@ class FrogPilotPlanner:
    self.params_memory = Params(memory=True)

    self.frogpilot_acceleration = FrogPilotAcceleration(self)
+    self.frogpilot_cem = ConditionalExperimentalMode(self)
    self.frogpilot_events = FrogPilotEvents(self)
    self.frogpilot_following = FrogPilotFollowing(self)
    self.frogpilot_vcruise = FrogPilotVCruise(self)
@@ -54,6 +56,12 @@ class FrogPilotPlanner:
      self.frogpilot_acceleration.max_accel = 0
      self.frogpilot_acceleration.min_accel = 0

+    if long_control_active and frogpilot_toggles.conditional_experimental_mode:
+      self.frogpilot_cem.update(v_ego, sm, frogpilot_toggles)
+    else:
+      self.frogpilot_cem.experimental_mode = False
+      self.frogpilot_cem.stop_sign_and_light(v_ego, sm, PLANNER_TIME - 2)
+
    self.frogpilot_events.update(v_cruise, sm, frogpilot_toggles)

    self.frogpilot_following.update(long_control_active, v_ego, sm, frogpilot_toggles)
@@ -95,6 +103,8 @@ class FrogPilotPlanner:
    frogpilotPlan.speedJerk = float(J_EGO_COST * self.frogpilot_following.speed_jerk)
    frogpilotPlan.tFollow = float(self.frogpilot_following.t_follow)

+    frogpilotPlan.experimentalMode = self.frogpilot_cem.experimental_mode
+
    frogpilotPlan.frogpilotEvents = self.frogpilot_events.events.to_msg()

    frogpilotPlan.frogpilotToggles = json.dumps(vars(frogpilot_toggles))
@@ -104,6 +114,8 @@ class FrogPilotPlanner:
    frogpilotPlan.maxAcceleration = float(self.frogpilot_acceleration.max_accel)
    frogpilotPlan.minAcceleration = float(self.frogpilot_acceleration.min_accel)

+    frogpilotPlan.redLight = self.frogpilot_cem.stop_light_detected
+
    frogpilotPlan.vCruise = float(self.v_cruise)

    pm.send("frogpilotPlan", frogpilot_plan_send)
@@ -0,0 +1,99 @@
+#!/usr/bin/env python3
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.realtime import DT_MDL
+
+from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, THRESHOLD
+
+CEStatus = {
+  "OFF": 0,              # Off
+  "USER_DISABLED": 1,    # "Experimental Mode" disabled by user
+  "USER_OVERRIDDEN": 2,  # "Experimental Mode" enabled by user
+  "CURVATURE": 3,        # Road curvature condition
+  "LEAD": 4,             # Slower lead vehicle condition
+  "SIGNAL": 5,           # Turn signal condition
+  "SPEED": 6,            # Speed condition
+  "SPEED_LIMIT": 7,      # Speed limit controller condition
+  "STOP_LIGHT": 8        # Stop light or sign condition
+}
+
+class ConditionalExperimentalMode:
+  def __init__(self, FrogPilotPlanner):
+    self.frogpilot_planner = FrogPilotPlanner
+
+    self.curvature_filter = FirstOrderFilter(0, 0.5, DT_MDL)
+    self.slow_lead_filter = FirstOrderFilter(0, 1, DT_MDL)
+    self.stop_light_filter = FirstOrderFilter(0, 0.5, DT_MDL)
+
+    self.experimental_mode = False
+    self.stop_light_detected = False
+
+  def update(self, v_ego, sm, frogpilot_toggles):
+    if frogpilot_toggles.experimental_mode_via_press:
+      self.status_value = self.frogpilot_planner.params_memory.get("CEStatus")
+    else:
+      self.status_value = CEStatus["OFF"]
+
+    if self.status_value not in (CEStatus["USER_DISABLED"], CEStatus["USER_OVERRIDDEN"]) and not sm["carState"].standstill:
+      self.update_conditions(v_ego, sm, frogpilot_toggles)
+      self.experimental_mode = self.check_conditions(v_ego, sm, frogpilot_toggles)
+      self.frogpilot_planner.params_memory.put("CEStatus", self.status_value)
+    else:
+      self.experimental_mode &= sm["carState"].standstill and self.frogpilot_planner.model_stopped
+      self.experimental_mode &= self.status_value != CEStatus["USER_DISABLED"]
+      self.experimental_mode |= self.status_value == CEStatus["USER_OVERRIDDEN"]
+
+      self.stop_light_detected &= self.status_value not in (CEStatus["USER_DISABLED"], CEStatus["USER_OVERRIDDEN"])
+      self.stop_light_filter.x = 0
+
+  def check_conditions(self, v_ego, sm, frogpilot_toggles):
+    if self.curve_detected and (not self.frogpilot_planner.frogpilot_following.following_lead or frogpilot_toggles.conditional_curves_lead) and frogpilot_toggles.conditional_curves:
+      self.status_value = CEStatus["CURVATURE"]
+      return True
+
+    if self.slow_lead_detected and frogpilot_toggles.conditional_lead:
+      self.status_value = CEStatus["LEAD"]
+      return True
+
+    if (sm["carState"].leftBlinker or sm["carState"].rightBlinker) and v_ego < frogpilot_toggles.conditional_signal:
+      desired_lane = self.frogpilot_planner.lane_width_left if sm["carState"].leftBlinker else self.frogpilot_planner.lane_width_right
+      if desired_lane < frogpilot_toggles.lane_detection_width or not frogpilot_toggles.conditional_signal_lane_detection:
+        self.status_value = CEStatus["SIGNAL"]
+        return True
+
+    if 1 <= v_ego < (frogpilot_toggles.conditional_limit_lead if self.frogpilot_planner.frogpilot_following.following_lead else frogpilot_toggles.conditional_limit):
+      self.status_value = CEStatus["SPEED"]
+      return True
+
+    if self.frogpilot_planner.frogpilot_vcruise.slc.experimental_mode:
+      self.status_value = CEStatus["SPEED_LIMIT"]
+      return True
+
+    if self.stop_light_detected and frogpilot_toggles.conditional_model_stop_time != 0:
+      self.status_value = CEStatus["STOP_LIGHT"]
+      return True
+
+    return False
+
+  def update_conditions(self, v_ego, sm, frogpilot_toggles):
+    self.curve_detection(v_ego, frogpilot_toggles)
+    self.slow_lead(v_ego, frogpilot_toggles)
+    self.stop_sign_and_light(v_ego, sm, frogpilot_toggles.conditional_model_stop_time)
+
+  def curve_detection(self, v_ego, frogpilot_toggles):
+    self.curvature_filter.update(self.frogpilot_planner.driving_in_curve or self.frogpilot_planner.road_curvature_detected)
+    self.curve_detected = self.curvature_filter.x >= THRESHOLD and v_ego > CRUISING_SPEED
+
+  def slow_lead(self, v_ego, frogpilot_toggles):
+    if self.frogpilot_planner.tracking_lead:
+      slower_lead = (v_ego - self.frogpilot_planner.lead_one.vLead) > CRUISING_SPEED and frogpilot_toggles.conditional_slower_lead
+      stopped_lead = self.frogpilot_planner.lead_one.vLead < 1 and frogpilot_toggles.conditional_stopped_lead
+
+      self.slow_lead_filter.update(slower_lead or stopped_lead)
+      self.slow_lead_detected = self.slow_lead_filter.x >= THRESHOLD
+    else:
+      self.slow_lead_filter.x = 0
+      self.slow_lead_detected = False
+
+  def stop_sign_and_light(self, v_ego, sm, model_time):
+    self.stop_light_filter.update((self.frogpilot_planner.model_length < v_ego * model_time) or self.frogpilot_planner.model_stopped)
+    self.stop_light_detected = self.stop_light_filter.x >= THRESHOLD and not self.frogpilot_planner.tracking_lead
@@ -47,4 +47,6 @@ FrogPilotUIState *frogpilotUIState() {

 void FrogPilotUIState::update() {
  update_state(this);
+
+  frogpilot_scene.conditional_status = frogpilot_scene.enabled ? params_memory.getInt("CEStatus") : 0;
 }
@@ -15,6 +15,7 @@ struct FrogPilotUIScene {
  bool reverse;
  bool standstill;

+  int conditional_status;
  int started_timer;

  QJsonObject frogpilot_toggles;
@@ -2,6 +2,14 @@

 FrogPilotAnnotatedCameraWidget::FrogPilotAnnotatedCameraWidget(QWidget *parent) : QWidget(parent) {
  QSize iconSize(img_size / 4, img_size / 4);
+
+  loadGif("../../frogpilot/assets/other_images/curve_icon.gif", cemCurveIcon, QSize(widget_size, widget_size), this);
+  loadGif("../../frogpilot/assets/other_images/lead_icon.gif", cemLeadIcon, QSize(widget_size, widget_size), this);
+  loadGif("../../frogpilot/assets/other_images/speed_icon.gif", cemSpeedIcon, QSize(widget_size, widget_size), this);
+  loadGif("../../frogpilot/assets/other_images/light_icon.gif", cemStopIcon, QSize(widget_size, widget_size), this);
+  loadGif("../../frogpilot/assets/other_images/turn_icon.gif", cemTurnIcon, QSize(widget_size, widget_size), this);
+  loadGif("../../frogpilot/assets/other_images/chill_mode_icon.gif", chillModeIcon, QSize(widget_size, widget_size), this);
+  loadGif("../../frogpilot/assets/other_images/experimental_mode_icon.gif", experimentalModeIcon, QSize(widget_size, widget_size), this);
 }

 void FrogPilotAnnotatedCameraWidget::showEvent(QShowEvent *event) {
@@ -40,6 +48,7 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil

  blindspotLeft = carState.getLeftBlindspot();
  blindspotRight = carState.getRightBlindspot();
+  experimentalMode = selfdriveState.getExperimentalMode();

  hideBottomIcons = selfdriveState.getAlertSize() != cereal::SelfdriveState::AlertSize::NONE;
  hideBottomIcons |= frogpilotSelfdriveState.getAlertSize() != cereal::FrogPilotSelfdriveState::AlertSize::NONE;
@@ -50,6 +59,13 @@ void FrogPilotAnnotatedCameraWidget::mousePressEvent(QMouseEvent *mouseEvent) {
 }

 void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState &s) {
+  if (!hideBottomIcons && frogpilot_toggles.value("cem_status").toBool()) {
+    paintCEMStatus(p);
+  } else {
+    cemStatusPosition.setX(0);
+    cemStatusPosition.setY(0);
+  }
+
  if (!hideBottomIcons && frogpilot_toggles.value("compass").toBool()) {
    paintCompass(p);
  } else {
@@ -77,6 +93,53 @@ void FrogPilotAnnotatedCameraWidget::paintBlindSpotPath(QPainter &p) {
  p.restore();
 }

+void FrogPilotAnnotatedCameraWidget::paintCEMStatus(QPainter &p) {
+  if (dmIconPosition == QPoint(0, 0)) {
+    return;
+  }
+
+  p.save();
+
+  cemStatusPosition.setX(dmIconPosition.x() + (rightHandDM ? -img_size - widget_size : widget_size));
+  cemStatusPosition.setY(dmIconPosition.y() - widget_size / 2);
+
+  QRect cemWidget(cemStatusPosition, QSize(widget_size, widget_size));
+
+  p.setBrush(blackColor(166));
+  if (frogpilot_scene.conditional_status == 1) {
+    p.setPen(QPen(QColor(bg_colors[STATUS_CEM_DISABLED]), 10));
+  } else if (experimentalMode) {
+    p.setPen(QPen(QColor(bg_colors[STATUS_EXPERIMENTAL_MODE_ENABLED]), 10));
+  } else {
+    p.setPen(QPen(blackColor(), 10));
+  }
+  p.drawRoundedRect(cemWidget, 24, 24);
+
+  QSharedPointer<QMovie> icon = chillModeIcon;
+  if (experimentalMode) {
+    if (frogpilot_scene.conditional_status == 1) {
+      icon = chillModeIcon;
+    } else if (frogpilot_scene.conditional_status == 2) {
+      icon = experimentalModeIcon;
+    } else if (frogpilot_scene.conditional_status == 3) {
+      icon = cemCurveIcon;
+    } else if (frogpilot_scene.conditional_status == 4) {
+      icon = cemLeadIcon;
+    } else if (frogpilot_scene.conditional_status == 5) {
+      icon = cemTurnIcon;
+    } else if (frogpilot_scene.conditional_status == 6 || frogpilot_scene.conditional_status == 7) {
+      icon = cemSpeedIcon;
+    } else if (frogpilot_scene.conditional_status == 8) {
+      icon = cemStopIcon;
+    } else {
+      icon = experimentalModeIcon;
+    }
+  }
+  p.drawPixmap(cemWidget, icon->currentPixmap());
+
+  p.restore();
+}
+
 void FrogPilotAnnotatedCameraWidget::paintCompass(QPainter &p) {
  if (dmIconPosition == QPoint(0, 0)) {
    return;
@@ -41,10 +41,12 @@ protected:
  void showEvent(QShowEvent *event) override;

 private:
+  void paintCEMStatus(QPainter &p);
  void paintCompass(QPainter &p);

  bool blindspotLeft;
  bool blindspotRight;
+  bool experimentalMode;

  float distanceConversion;
  float setSpeed;
@@ -57,8 +59,17 @@ private:
  QColor blackColor(int alpha = 255) { return QColor(0, 0, 0, alpha); }
  QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); }

+  QPoint cemStatusPosition;
  QPoint compassPosition;

+  QSharedPointer<QMovie> cemCurveIcon;
+  QSharedPointer<QMovie> cemLeadIcon;
+  QSharedPointer<QMovie> cemSpeedIcon;
+  QSharedPointer<QMovie> cemStopIcon;
+  QSharedPointer<QMovie> cemTurnIcon;
+  QSharedPointer<QMovie> chillModeIcon;
+  QSharedPointer<QMovie> experimentalModeIcon;
+
  QString leadDistanceUnit;
  QString leadSpeedUnit;
  QString speedUnit;
@@ -136,7 +136,7 @@ class VCruiseHelper:
    if self.CP.pcmCruise and not self.gm_cc_only:
      return

-    initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL
+    initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode and not frogpilot_toggles.conditional_experimental_mode else V_CRUISE_INITIAL

    if (any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents)
      and self.v_cruise_initialized or (self.gm_cc_only and resume_prev_button)):
@@ -434,6 +434,11 @@ class SelfdriveD:
    # FrogPilot variables
    self.frogpilot_events.add_from_msg(self.sm['frogpilotPlan'].frogpilotEvents)

+    if self.frogpilot_toggles.conditional_experimental_mode:
+      self.experimental_mode = self.sm['frogpilotPlan'].experimentalMode
+    else:
+      self.experimental_mode |= self.sm['frogpilotPlan'].experimentalMode
+
  def data_sample(self):
    _car_state = messaging.recv_one(self.car_state_sock)
    CS = _car_state.carState if _car_state else self.CS_prev
@@ -572,7 +577,8 @@ class SelfdriveD:
      self.is_metric = self.params.get_bool("IsMetric")
      self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
      self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
-      self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
+      if not self.frogpilot_toggles.conditional_experimental_mode:
+        self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
      self.personality = self.params.get("LongitudinalPersonality", return_default=True)
      time.sleep(0.1)

@@ -31,7 +31,12 @@ void ExperimentalButton::changeMode() {
  bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed");
  if (can_change) {
    // FrogPilot variables
-    params.putBool("ExperimentalMode", !experimental_mode);
+    if (frogpilot_toggles.value("conditional_experimental_mode").toBool()) {
+      int override_value = (frogpilot_scene.conditional_status == 1 || frogpilot_scene.conditional_status == 2) ? 0 : experimental_mode ? 1 : 2;
+      params_memory.putInt("CEStatus", override_value);
+    } else {
+      params.putBool("ExperimentalMode", !experimental_mode);
+    }
  }
 }

@@ -65,6 +70,8 @@ void ExperimentalButton::updateBackgroundColor() {
    background_color = QColor(0, 0, 0, 166);
  } else if (frogpilot_scene.always_on_lateral_active) {
    background_color = bg_colors[STATUS_ALWAYS_ON_LATERAL_ACTIVE];
+  } else if (frogpilot_scene.conditional_status == 1) {
+    background_color = bg_colors[STATUS_CEM_DISABLED];
  } else if (experimental_mode) {
    background_color = bg_colors[STATUS_EXPERIMENTAL_MODE_ENABLED];
  } else {
@@ -47,6 +47,7 @@ typedef enum UIStatus {

  // FrogPilot variables
  STATUS_ALWAYS_ON_LATERAL_ACTIVE,
+  STATUS_CEM_DISABLED,
  STATUS_EXPERIMENTAL_MODE_ENABLED,
 } UIStatus;

@@ -57,6 +58,7 @@ const QColor bg_colors [] = {

  // FrogPilot variables
  [STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1),
+  [STATUS_CEM_DISABLED] = QColor(0xff, 0xff, 0x00, 0xf1),
  [STATUS_EXPERIMENTAL_MODE_ENABLED] = QColor(0xda, 0x6f, 0x25, 0xf1),
 };