diff --git a/frogpilot/assets/other_images/chill_mode_icon.gif b/frogpilot/assets/other_images/chill_mode_icon.gif new file mode 100644 index 00000000..69bc2479 Binary files /dev/null and b/frogpilot/assets/other_images/chill_mode_icon.gif differ diff --git a/frogpilot/assets/other_images/curve_icon.gif b/frogpilot/assets/other_images/curve_icon.gif new file mode 100644 index 00000000..34574806 Binary files /dev/null and b/frogpilot/assets/other_images/curve_icon.gif differ diff --git a/frogpilot/assets/other_images/experimental_mode_icon.gif b/frogpilot/assets/other_images/experimental_mode_icon.gif new file mode 100644 index 00000000..933d711f Binary files /dev/null and b/frogpilot/assets/other_images/experimental_mode_icon.gif differ diff --git a/frogpilot/assets/other_images/lead_icon.gif b/frogpilot/assets/other_images/lead_icon.gif new file mode 100644 index 00000000..edc24495 Binary files /dev/null and b/frogpilot/assets/other_images/lead_icon.gif differ diff --git a/frogpilot/assets/other_images/light_icon.gif b/frogpilot/assets/other_images/light_icon.gif new file mode 100644 index 00000000..8d03f41d Binary files /dev/null and b/frogpilot/assets/other_images/light_icon.gif differ diff --git a/frogpilot/assets/other_images/speed_icon.gif b/frogpilot/assets/other_images/speed_icon.gif new file mode 100644 index 00000000..6020909f Binary files /dev/null and b/frogpilot/assets/other_images/speed_icon.gif differ diff --git a/frogpilot/assets/other_images/turn_icon.gif b/frogpilot/assets/other_images/turn_icon.gif new file mode 100644 index 00000000..a7d080a4 Binary files /dev/null and b/frogpilot/assets/other_images/turn_icon.gif differ diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index 0b89912a..f9fca93d 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/frogpilot/controls/frogpilot_planner.py @@ -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) diff --git a/frogpilot/controls/lib/conditional_experimental_mode.py b/frogpilot/controls/lib/conditional_experimental_mode.py new file mode 100644 index 00000000..5ef640a5 --- /dev/null +++ b/frogpilot/controls/lib/conditional_experimental_mode.py @@ -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 diff --git a/frogpilot/ui/frogpilot_ui.cc b/frogpilot/ui/frogpilot_ui.cc index 49dc46ec..05ab0b51 100644 --- a/frogpilot/ui/frogpilot_ui.cc +++ b/frogpilot/ui/frogpilot_ui.cc @@ -47,4 +47,6 @@ FrogPilotUIState *frogpilotUIState() { void FrogPilotUIState::update() { update_state(this); + + frogpilot_scene.conditional_status = frogpilot_scene.enabled ? params_memory.getInt("CEStatus") : 0; } diff --git a/frogpilot/ui/frogpilot_ui.h b/frogpilot/ui/frogpilot_ui.h index 808cb88f..8eba66a1 100644 --- a/frogpilot/ui/frogpilot_ui.h +++ b/frogpilot/ui/frogpilot_ui.h @@ -15,6 +15,7 @@ struct FrogPilotUIScene { bool reverse; bool standstill; + int conditional_status; int started_timer; QJsonObject frogpilot_toggles; diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc index ae9efe1e..7f9e3492 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc @@ -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 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; diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h index ade80063..424987a8 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h @@ -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 cemCurveIcon; + QSharedPointer cemLeadIcon; + QSharedPointer cemSpeedIcon; + QSharedPointer cemStopIcon; + QSharedPointer cemTurnIcon; + QSharedPointer chillModeIcon; + QSharedPointer experimentalModeIcon; + QString leadDistanceUnit; QString leadSpeedUnit; QString speedUnit; diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index c3194027..c754291c 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -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)): diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index b9278465..ac8b0fa3 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -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) diff --git a/selfdrive/ui/qt/onroad/buttons.cc b/selfdrive/ui/qt/onroad/buttons.cc index 717d5adc..722cad7e 100644 --- a/selfdrive/ui/qt/onroad/buttons.cc +++ b/selfdrive/ui/qt/onroad/buttons.cc @@ -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 { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 484a4f7e..eadd87d7 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -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), };