Nudgeless lane change

Added toggles for nudgeless lane changes, lane detection, and one lane change per signal activation with a lane change delay factor.

cereal/car.capnp

@@ -123,6 +123,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     holidayActive @124;
     laneChangeBlockedLoud @125;
     leadDeparting @126;
+    noLaneAvailable @127;
     torqueNNLoad @132;
 
     radarCanErrorDEPRECATED @15;

common/params.cc

@@ -282,6 +282,9 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"HideSpeedUI", PERSISTENT},
     {"HigherBitrate", PERSISTENT},
     {"HolidayThemes", PERSISTENT},
+    {"LaneChangeTime", PERSISTENT},
+    {"LaneDetection", PERSISTENT},
+    {"LaneDetectionWidth", PERSISTENT},
     {"LaneLinesWidth", PERSISTENT},
     {"LateralTune", PERSISTENT},
     {"LeadDepartingAlert", PERSISTENT},
@@ -310,6 +313,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"NNFFModelName", PERSISTENT},
     {"NoLogging", PERSISTENT},
     {"NoUploads", PERSISTENT},
+    {"NudgelessLaneChange", PERSISTENT},
+    {"OneLaneChange", PERSISTENT},
     {"PathEdgeWidth", PERSISTENT},
     {"PathWidth", PERSISTENT},
     {"PromptVolume", PERSISTENT},

selfdrive/controls/controlsd.py

@@ -414,12 +414,17 @@ class Controls:
     # Handle lane change
     if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
       direction = self.sm['modelV2'].meta.laneChangeDirection
+      desired_lane = frogpilot_plan.laneWidthLeft if direction == LaneChangeDirection.left else frogpilot_plan.laneWidthRight
+      lane_available = desired_lane >= self.lane_detection_width
+
       if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
          (CS.rightBlindspot and direction == LaneChangeDirection.right):
         if self.loud_blindspot_alert:
           self.events.add(EventName.laneChangeBlockedLoud)
         else:
           self.events.add(EventName.laneChangeBlocked)
+      elif not lane_available:
+        self.events.add(EventName.noLaneAvailable)
       else:
         if direction == LaneChangeDirection.left:
           self.events.add(EventName.preLaneChangeLeft)
@@ -1088,6 +1093,9 @@ class Controls:
     longitudinal_tune = self.params.get_bool("LongitudinalTune")
     self.frogpilot_variables.sport_plus = longitudinal_tune and self.params.get_int("AccelerationProfile") == 3
 
+    self.lane_detection = self.params.get_bool("LaneDetection") and self.params.get_bool("NudgelessLaneChange")
+    self.lane_detection_width = self.params.get_int("LaneDetectionWidth") * (1 if self.is_metric else CV.FOOT_TO_METER) / 10 if self.lane_detection else 0
+
     quality_of_life = self.params.get_bool("QOLControls")
     self.frogpilot_variables.reverse_cruise_increase = quality_of_life and self.params.get_bool("ReverseCruise")
 

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,13 @@ 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
+      # Check if the lane width exceeds the threshold
+      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 +71,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,10 +86,18 @@ class DesireHelper:
         blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
                               (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
 
+        # Conduct a nudgeless lane change if all the conditions are met
+        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:
+          self.lane_change_wait_timer = 0
+          torque_applied = True
+
         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:
+          # Set the "lane_change_completed" flag to prevent any more lane changes if the toggle is on
+          self.lane_change_completed = self.one_lane_change
           self.lane_change_state = LaneChangeState.laneChangeStarting
 
       # LaneChangeState.laneChangeStarting
@@ -108,6 +128,9 @@ class DesireHelper:
 
     self.prev_one_blinker = one_blinker
 
+    # Reset the flags
+    self.lane_change_completed &= one_blinker
+
     self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
 
     # Send keep pulse once per second during LaneChangeStart.preLaneChange
@@ -125,3 +148,9 @@ class DesireHelper:
 
   def update_frogpilot_params(self):
     is_metric = self.params.get_bool("IsMetric")
+
+    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_bool("LaneDetection")
+    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")

selfdrive/controls/lib/events.py

@@ -376,6 +376,15 @@ 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, Union[Alert, AlertCallbackType]]] = {
   # ********** events with no alerts **********
 
@@ -1036,6 +1045,10 @@ EVENTS: Dict[int, Dict[str, Union[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,
   },

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -68,7 +68,7 @@ class FrogPilotPlanner:
       self.cem.update(carState, enabled, sm['frogpilotNavigation'], modelData, sm['radarState'], self.road_curvature, self.stop_distance, mpc.t_follow, v_ego)
 
     # Update the current lane widths
-    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(self.fpf.calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0]))
       self.lane_width_right = float(self.fpf.calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1]))
@@ -156,6 +156,9 @@ class FrogPilotPlanner:
     self.adjacent_lanes = custom_ui and params.get_bool("AdjacentPath")
     self.blind_spot_path = custom_ui and params.get_bool("BlindSpotPath")
 
+    nudgeless_lane_change = params.get_bool("NudgelessLaneChange")
+    self.lane_detection = nudgeless_lane_change and params.get_bool("LaneDetection")
+
     longitudinal_tune = params.get_bool("LongitudinalTune")
     self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0
     self.deceleration_profile = params.get_int("DecelerationProfile") if longitudinal_tune else 0

selfdrive/frogpilot/ui/control_settings.cc

@@ -42,6 +42,12 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     {"MTSCLimit", "Speed Change Hard Cap", "Set a hard cap for MTSC. If MTSC requests a speed decrease greater than this value, it ignores the requested speed from MTSC. Purely used as a failsafe to prevent false positives. Leave this off if you never experience false positives.", ""},
     {"MTSCAggressiveness", "Turn Speed Aggressiveness", "Set turn speed aggressiveness. Higher values result in faster turns, lower values yield gentler turns.\n\nA change of +- 1% results in the velocity being raised or lowered by about 1 mph.", ""},
 
+    {"NudgelessLaneChange", "Nudgeless Lane Change", "Enable lane changes without manual steering input.", "../frogpilot/assets/toggle_icons/icon_lane.png"},
+    {"LaneChangeTime", "Lane Change Timer", "Specify a delay before executing a nudgeless lane change.", ""},
+    {"LaneDetection", "Lane Detection", "Block nudgeless lane changes when a lane isn't detected.", ""},
+    {"LaneDetectionWidth", "Lane Detection Threshold", "Set the required lane width to be qualified as a lane.", ""},
+    {"OneLaneChange", "One Lane Change Per Signal", "Limit to one nudgeless lane change per turn signal activation.", ""},
+
     {"QOLControls", "Quality of Life", "Miscellaneous quality of life changes to improve your overall openpilot experience.", "../frogpilot/assets/toggle_icons/quality_of_life.png"},
     {"DisableOnroadUploads", "Disable Onroad Uploads", "Prevent large data uploads when onroad.", ""},
     {"HigherBitrate", "Higher Bitrate Recording", "Increases the quality of the footage uploaded to comma connect.", ""},
@@ -279,6 +285,24 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
       std::vector<QString> 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]() {
+        parentToggleClicked();
+        for (auto &[key, toggle] : toggles) {
+          toggle->setVisible(laneChangeKeys.find(key.c_str()) != laneChangeKeys.end());
+        }
+      });
+      toggle = laneChangeToggle;
+    } else if (param == "LaneChangeTime") {
+      std::map<int, QString> 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<int, QString>(), this, false, " feet", 10);
+
     } else {
       toggle = new ParamControl(param, title, desc, icon, this);
     }
@@ -361,23 +385,30 @@ void FrogPilotControlsPanel::updateMetric() {
     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("LaneDetectionWidth", std::nearbyint(params.getInt("LaneDetectionWidth") * distanceConversion));
     params.putIntNonBlocking("MTSCLimit", std::nearbyint(params.getInt("MTSCLimit") * speedConversion));
     params.putIntNonBlocking("StoppingDistance", std::nearbyint(params.getInt("StoppingDistance") * distanceConversion));
   }
 
+  FrogPilotParamValueControl *laneWidthToggle = static_cast<FrogPilotParamValueControl*>(toggles["LaneDetectionWidth"]);
   FrogPilotParamValueControl *mtscLimitToggle = static_cast<FrogPilotParamValueControl*>(toggles["MTSCLimit"]);
   FrogPilotParamValueControl *stoppingDistanceToggle = static_cast<FrogPilotParamValueControl*>(toggles["StoppingDistance"]);
 
   if (isMetric) {
+    laneWidthToggle->updateControl(0, 30, " meters", 10);
+
     mtscLimitToggle->updateControl(0, 99, " kph");
 
     stoppingDistanceToggle->updateControl(0, 5, " meters");
   } else {
+    laneWidthToggle->updateControl(0, 100, " feet", 10);
+
     mtscLimitToggle->updateControl(0, 99, " mph");
 
     stoppingDistanceToggle->updateControl(0, 10, " feet");
   }
 
+  laneWidthToggle->refresh();
   mtscLimitToggle->refresh();
   stoppingDistanceToggle->refresh();
 

selfdrive/frogpilot/ui/control_settings.h

@@ -40,7 +40,7 @@ private:
   std::set<QString> conditionalExperimentalKeys = {"CECurves", "CECurvesLead", "CESlowerLead", "CENavigation", "CEStopLights", "CESignal"};
   std::set<QString> experimentalModeActivationKeys = {"ExperimentalModeViaDistance", "ExperimentalModeViaLKAS", "ExperimentalModeViaScreen"};
   std::set<QString> fireTheBabysitterKeys = {"NoLogging", "MuteOverheated", "NoUploads"};
-  std::set<QString> laneChangeKeys = {};
+  std::set<QString> laneChangeKeys = {"LaneChangeTime", "LaneDetection", "LaneDetectionWidth", "OneLaneChange"};
   std::set<QString> lateralTuneKeys = {"ForceAutoTune", "NNFF"};
   std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "StoppingDistance"};
   std::set<QString> mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit"};