diff --git a/cereal/car.capnp b/cereal/car.capnp index a9f0927c7..f4ed3f4c7 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -124,6 +124,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { holidayActive @127; laneChangeBlockedLoud @128; leadDeparting @129; + noLaneAvailable @130; torqueNNLoad @135; radarCanErrorDEPRECATED @15; diff --git a/common/params.cc b/common/params.cc index 681d6224f..9ae25c6b0 100644 --- a/common/params.cc +++ b/common/params.cc @@ -296,6 +296,8 @@ std::unordered_map keys = { {"HigherBitrate", PERSISTENT}, {"HolidayThemes", PERSISTENT}, {"IncreaseThermalLimits", PERSISTENT}, + {"LaneChangeTime", PERSISTENT}, + {"LaneDetectionWidth", PERSISTENT}, {"LaneLinesWidth", PERSISTENT}, {"LateralTune", PERSISTENT}, {"LeadDepartingAlert", PERSISTENT}, @@ -324,7 +326,9 @@ std::unordered_map keys = { {"NNFFModelName", PERSISTENT}, {"NoLogging", PERSISTENT}, {"NoUploads", PERSISTENT}, + {"NudgelessLaneChange", PERSISTENT}, {"OfflineMode", PERSISTENT}, + {"OneLaneChange", PERSISTENT}, {"PathEdgeWidth", PERSISTENT}, {"PathWidth", PERSISTENT}, {"PauseAOLOnBrake", PERSISTENT}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 09baccde9..ec8fdc822 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -304,9 +304,15 @@ class Controls: self.events.add(EventName.laneChangeBlocked) else: if direction == LaneChangeDirection.left: - self.events.add(EventName.preLaneChangeLeft) + if self.sm['frogpilotPlan'].laneWidthLeft >= self.lane_detection_width: + self.events.add(EventName.preLaneChangeLeft) + else: + self.events.add(EventName.noLaneAvailable) else: - self.events.add(EventName.preLaneChangeRight) + if self.sm['frogpilotPlan'].laneWidthRight >= self.lane_detection_width: + self.events.add(EventName.preLaneChangeRight) + else: + self.events.add(EventName.noLaneAvailable) elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing): self.events.add(EventName.laneChange) @@ -996,6 +1002,9 @@ class Controls: self.frogpilot_variables.experimental_mode_via_distance = experimental_mode_activation and self.params.get_bool("ExperimentalModeViaDistance") self.experimental_mode_via_lkas = experimental_mode_activation and self.params.get_bool("ExperimentalModeViaLKAS") + lane_detection = self.params.get_bool("NudgelessLaneChange") and self.params.get_int("LaneDetectionWidth") != 0 + self.lane_detection_width = self.params.get_int("LaneDetectionWidth") * (1 if self.is_metric else CV.FOOT_TO_METER) / 10 if lane_detection else 0 + lateral_tune = self.params.get_bool("LateralTune") self.force_auto_tune = lateral_tune and self.params.get_float("ForceAutoTune") diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 4432124d9..6bfb4374c 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -45,6 +45,10 @@ class DesireHelper: self.params = Params() self.params_memory = Params("/dev/shm/params") + self.lane_change_completed = False + + self.lane_change_wait_timer = 0 + self.update_frogpilot_params() def update(self, carstate, lateral_active, lane_change_prob, frogpilotPlan): @@ -52,6 +56,12 @@ class DesireHelper: one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN + if not (self.lane_detection and one_blinker) or below_lane_change_speed: + lane_available = True + else: + desired_lane = frogpilotPlan.laneWidthLeft if carstate.leftBlinker else frogpilotPlan.laneWidthRight + lane_available = desired_lane >= self.lane_detection_width + if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none @@ -60,6 +70,7 @@ class DesireHelper: if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 + self.lane_change_wait_timer = 0 # LaneChangeState.preLaneChange elif self.lane_change_state == LaneChangeState.preLaneChange: @@ -74,11 +85,17 @@ class DesireHelper: blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) + self.lane_change_wait_timer += DT_MDL + if self.nudgeless and lane_available and not self.lane_change_completed and self.lane_change_wait_timer >= self.lane_change_delay: + torque_applied = True + self.lane_change_wait_timer = 0 + if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none elif torque_applied and not blindspot_detected: self.lane_change_state = LaneChangeState.laneChangeStarting + self.lane_change_completed = True if self.one_lane_change else False # LaneChangeState.laneChangeStarting elif self.lane_change_state == LaneChangeState.laneChangeStarting: @@ -106,6 +123,7 @@ class DesireHelper: else: self.lane_change_timer += DT_MDL + self.lane_change_completed &= one_blinker self.prev_one_blinker = one_blinker self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] @@ -127,3 +145,9 @@ class DesireHelper: is_metric = self.params.get_bool("IsMetric") lateral_tune = self.params.get_bool("LateralTune") + + self.nudgeless = self.params.get_bool("NudgelessLaneChange") + self.lane_change_delay = self.params.get_int("LaneChangeTime") if self.nudgeless else 0 + self.lane_detection = self.nudgeless and self.params.get_int("LaneDetectionWidth") != 0 + self.lane_detection_width = self.params.get_int("LaneDetectionWidth") * (1 if is_metric else CV.FOOT_TO_METER) / 10 if self.lane_detection else 0 + self.one_lane_change = self.nudgeless and self.params.get_bool("OneLaneChange") diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 1b91ffa7b..c974ee544 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -376,6 +376,16 @@ def holiday_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, AlertStatus.normal, AlertSize.small, Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 5.) +def no_lane_available_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + lane_width = sm['frogpilotPlan'].laneWidthLeft if CS.leftBlinker else sm['frogpilotPlan'].laneWidthRight + lane_width_msg = f"{lane_width:.1f} meters" if metric else f"{lane_width * CV.METER_TO_FOOT:.1f} feet" + + return Alert( + "No lane available", + f"Detected lane width is only {lane_width_msg}", + AlertStatus.normal, AlertSize.mid, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) + EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { # ********** events with no alerts ********** @@ -1042,6 +1052,10 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.), }, + EventName.noLaneAvailable : { + ET.PERMANENT: no_lane_available_alert, + }, + EventName.torqueNNLoad: { ET.PERMANENT: torque_nn_load_alert, }, diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_lane.png b/selfdrive/frogpilot/assets/toggle_icons/icon_lane.png new file mode 100644 index 000000000..cd8e40aab Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_lane.png differ diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 7be184d82..56f09641b 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -91,7 +91,7 @@ class FrogPilotPlanner: else: self.min_accel = ACCEL_MIN - check_lane_width = self.adjacent_lanes or self.blind_spot_path + check_lane_width = self.adjacent_lanes or self.blind_spot_path or self.lane_detection if check_lane_width and v_ego >= LANE_CHANGE_SPEED_MIN: self.lane_width_left = float(calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0])) self.lane_width_right = float(calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1])) @@ -223,6 +223,9 @@ class FrogPilotPlanner: self.adjacent_lanes = custom_ui and self.params.get_bool("AdjacentPath") self.blind_spot_path = custom_ui and self.params.get_bool("BlindSpotPath") + nudgeless_lane_change = self.params.get_bool("NudgelessLaneChange") + self.lane_detection = nudgeless_lane_change and self.params.get_int("LaneDetectionWidth") != 0 + longitudinal_tune = self.CP.openpilotLongitudinalControl and self.params.get_bool("LongitudinalTune") self.acceleration_profile = self.params.get_int("AccelerationProfile") if longitudinal_tune else 0 self.deceleration_profile = self.params.get_int("DecelerationProfile") if longitudinal_tune else 0 diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc index 568d87eee..aceb56d9e 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc @@ -94,6 +94,11 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"ModelSelector", tr("Model Selector"), tr("Manage openpilot's driving models."), "../assets/offroad/icon_calibration.png"}, + {"NudgelessLaneChange", tr("Nudgeless Lane Change"), tr("Enable lane changes without requiring manual steering input."), "../frogpilot/assets/toggle_icons/icon_lane.png"}, + {"LaneChangeTime", tr("Lane Change Timer"), tr("Set a delay before executing a nudgeless lane change."), ""}, + {"LaneDetectionWidth", tr("Lane Detection Threshold"), tr("Set the required lane width to be qualified as a lane."), ""}, + {"OneLaneChange", tr("One Lane Change Per Signal"), tr("Only allow one nudgeless lane change per turn signal activation."), ""}, + {"QOLControls", tr("Quality of Life"), tr("Miscellaneous quality of life changes to improve your overall openpilot experience."), "../frogpilot/assets/toggle_icons/quality_of_life.png"}, {"CustomCruise", tr("Cruise Increase Interval"), tr("Set a custom interval to increase the max set speed by."), ""}, {"CustomCruiseLong", tr("Cruise Increase Interval (Long Press)"), tr("Set a custom interval to increase the max set speed by when holding down the cruise increase button."), ""}, @@ -518,6 +523,24 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil std::vector reverseCruiseNames{tr("Control Via UI")}; toggle = new FrogPilotParamToggleControl(param, title, desc, icon, reverseCruiseToggles, reverseCruiseNames); + } else if (param == "NudgelessLaneChange") { + FrogPilotParamManageControl *laneChangeToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(laneChangeToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + openParentToggle(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(laneChangeKeys.find(key.c_str()) != laneChangeKeys.end()); + } + }); + toggle = laneChangeToggle; + } else if (param == "LaneChangeTime") { + std::map laneChangeTimeLabels; + for (int i = 0; i <= 10; ++i) { + laneChangeTimeLabels[i] = i == 0 ? "Instant" : QString::number(i / 2.0) + " seconds"; + } + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 10, laneChangeTimeLabels, this, false); + } else if (param == "LaneDetectionWidth") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 100, std::map(), this, false, " feet", 10); + } else { toggle = new ParamControl(param, title, desc, icon, this); } @@ -662,29 +685,34 @@ void FrogPilotControlsPanel::updateMetric() { params.putIntNonBlocking("CESpeedLead", std::nearbyint(params.getInt("CESpeedLead") * speedConversion)); params.putIntNonBlocking("CustomCruise", std::nearbyint(params.getInt("CustomCruise") * speedConversion)); params.putIntNonBlocking("CustomCruiseLong", std::nearbyint(params.getInt("CustomCruiseLong") * speedConversion)); + params.putIntNonBlocking("LaneDetectionWidth", std::nearbyint(params.getInt("LaneDetectionWidth") * distanceConversion)); params.putIntNonBlocking("PauseAOLOnBrake", std::nearbyint(params.getInt("PauseAOLOnBrake") * speedConversion)); params.putIntNonBlocking("StoppingDistance", std::nearbyint(params.getInt("StoppingDistance") * distanceConversion)); } FrogPilotParamValueControl *customCruiseToggle = static_cast(toggles["CustomCruise"]); FrogPilotParamValueControl *customCruiseLongToggle = static_cast(toggles["CustomCruiseLong"]); + FrogPilotParamValueControl *laneWidthToggle = static_cast(toggles["LaneDetectionWidth"]); FrogPilotParamValueControl *pauseAOLOnBrakeToggle = static_cast(toggles["PauseAOLOnBrake"]); FrogPilotParamValueControl *stoppingDistanceToggle = static_cast(toggles["StoppingDistance"]); if (isMetric) { customCruiseToggle->updateControl(1, 150, tr(" kph")); customCruiseLongToggle->updateControl(1, 150, tr(" kph")); + laneWidthToggle->updateControl(0, 30, tr(" meters"), 10); pauseAOLOnBrakeToggle->updateControl(0, 99, tr(" kph")); stoppingDistanceToggle->updateControl(0, 5, tr(" meters")); } else { customCruiseToggle->updateControl(1, 99, tr(" mph")); customCruiseLongToggle->updateControl(1, 99, tr(" mph")); + laneWidthToggle->updateControl(0, 100, tr(" feet"), 10); pauseAOLOnBrakeToggle->updateControl(0, 99, tr(" mph")); stoppingDistanceToggle->updateControl(0, 10, tr(" feet")); } customCruiseToggle->refresh(); customCruiseLongToggle->refresh(); + laneWidthToggle->refresh(); pauseAOLOnBrakeToggle->refresh(); stoppingDistanceToggle->refresh(); } diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h index b5a1191ef..f341317b2 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h @@ -41,7 +41,7 @@ private: std::set conditionalExperimentalKeys = {"CECurves", "CECurvesLead", "CENavigation", "CESignal", "CESlowerLead", "CEStopLights", "HideCEMStatusBar"}; std::set deviceManagementKeys = {"DeviceShutdown", "HigherBitrate", "IncreaseThermalLimits", "LowVoltageShutdown", "NoLogging", "NoUploads", "OfflineMode"}; std::set experimentalModeActivationKeys = {"ExperimentalModeViaDistance", "ExperimentalModeViaLKAS", "ExperimentalModeViaTap"}; - std::set laneChangeKeys = {}; + std::set laneChangeKeys = {"LaneChangeTime", "LaneDetectionWidth", "OneLaneChange"}; std::set lateralTuneKeys = {"ForceAutoTune", "NNFF", "NNFFLite"}; std::set longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "StoppingDistance"}; std::set mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck"}; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 78fd5c218..69da6859e 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -1138,6 +1138,7 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() { experimentalMode = scene.experimental_mode; + laneDetectionWidth = scene.lane_detection_width; laneWidthLeft = scene.lane_width_left; laneWidthRight = scene.lane_width_right; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 049da1e0f..3b0022591 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -137,6 +137,7 @@ private: float cruiseAdjustment; float distanceConversion; + float laneDetectionWidth; float laneWidthLeft; float laneWidthRight; float speedConversion; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 1e0e1190f..8ce941ea5 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -330,6 +330,9 @@ void ui_update_frogpilot_params(UIState *s) { scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing"); scene.experimental_mode_via_screen = scene.longitudinal_control && params.getBool("ExperimentalModeActivation") && params.getBool("ExperimentalModeViaTap"); + bool lane_detection = params.getBool("NudgelessLaneChange") && params.getInt("LaneDetectionWidth") != 0; + scene.lane_detection_width = lane_detection ? params.getInt("LaneDetectionWidth") * (scene.is_metric ? 1 : FOOT_TO_METER) / 10 : 9.0f; + scene.model_ui = params.getBool("ModelUI"); scene.dynamic_path_width = scene.model_ui && params.getBool("DynamicPathWidth"); scene.hide_lead_marker = scene.model_ui && params.getBool("HideLeadMarker"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 583347292..d26b5bbb5 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -218,6 +218,7 @@ typedef struct UIScene { float ego_jerk; float ego_jerk_difference; float friction; + float lane_detection_width; float lane_line_width; float lane_width_left; float lane_width_right;