Nudgeless lane change + lane detection

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

common/params.cc

@@ -258,6 +258,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"GoatScream", PERSISTENT},
     {"GreenLightAlert", PERSISTENT},
     {"HideSpeed", PERSISTENT},
+    {"LaneChangeTime", PERSISTENT},
+    {"LaneDetection", PERSISTENT},
     {"LaneLinesWidth", PERSISTENT},
     {"LateralTune", PERSISTENT},
     {"LeadInfo", PERSISTENT},
@@ -282,7 +284,9 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"NNFFModelFuzzyMatch", PERSISTENT},
     {"NNFFModelName", PERSISTENT},
     {"NoLogging", PERSISTENT},
+    {"NudgelessLaneChange", PERSISTENT},
     {"OfflineMode", PERSISTENT},
+    {"OneLaneChange", PERSISTENT},
     {"PathEdgeWidth", PERSISTENT},
     {"PathWidth", PERSISTENT},
     {"RelaxedFollow", PERSISTENT},

selfdrive/controls/lib/desire_helper.py

@@ -43,6 +43,11 @@ class DesireHelper:
     self.desire = log.LateralPlan.Desire.none
 
     # FrogPilot variables
+    self.lane_change_completed = False
+
+    self.lane_change_wait_timer = 0
+    self.lane_width_left = 0
+    self.lane_width_right = 0
 
   def update(self, carstate, modeldata, lateral_active, lane_change_prob, frogpilot_planner):
     v_ego = carstate.vEgo
@@ -59,6 +64,20 @@ class DesireHelper:
       self.lane_width_left = 0
       self.lane_width_right = 0
 
+    # Calculate the desired lane width for nudgeless lane change with lane detection
+    if not (frogpilot_planner.lane_detection and one_blinker) or below_lane_change_speed or turning:
+      lane_available = True
+    else:
+      # Set the minimum lane threshold to 2.8 meters
+      min_lane_threshold = 2.8
+      # Set the blinker index based on which signal is on
+      blinker_index = 0 if carstate.leftBlinker else 1
+      current_lane = modeldata.laneLines[blinker_index + 1]
+      desired_lane = modeldata.laneLines[blinker_index if carstate.leftBlinker else blinker_index + 2]
+      road_edge = modeldata.roadEdges[blinker_index]
+      # Check if the lane width exceeds the threshold
+      lane_available = calculate_lane_width(desired_lane, current_lane, road_edge) >= min_lane_threshold
+
     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
@@ -67,6 +86,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:
@@ -81,10 +101,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 true
+        self.lane_change_wait_timer += DT_MDL
+        if frogpilot_planner.nudgeless and lane_available and not self.lane_change_completed and self.lane_change_wait_timer >= frogpilot_planner.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:
+          # Set the "lane_change_completed" flag to prevent any more lane changes if the toggle is on
+          self.lane_change_completed = frogpilot_planner.one_lane_change
           self.lane_change_state = LaneChangeState.laneChangeStarting
 
       # LaneChangeState.laneChangeStarting
@@ -115,6 +143,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

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -157,3 +157,8 @@ class FrogPilotPlanner:
     self.increased_stopping_distance = params.get_int("StoppingDistance") * (1 if self.is_metric else CV.FOOT_TO_METER) if longitudinal_tune else 0
 
     self.map_turn_speed_controller = params.get_bool("MTSCEnabled")
+
+    self.nudgeless = params.get_bool("NudgelessLaneChange")
+    self.lane_change_delay = params.get_int("LaneChangeTime") if self.nudgeless else 0
+    self.lane_detection = params.get_bool("LaneDetection") if self.nudgeless else False
+    self.one_lane_change = params.get_bool("OneLaneChange") if self.nudgeless else False

selfdrive/frogpilot/ui/control_settings.cc

@@ -34,6 +34,11 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
 
     {"Model", "Model Selector", "Choose your preferred openpilot model.", "../assets/offroad/icon_calibration.png"},
     {"MTSCEnabled", "Map Turn Speed Control", "Slow down for anticipated curves detected by your downloaded maps.", "../frogpilot/assets/toggle_icons/icon_speed_map.png"},
+
+    {"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.", ""},
+    {"OneLaneChange", "One Lane Change Per Signal", "Limit to one nudgeless lane change per turn signal activation.", ""},
   };
 
   for (const auto &[param, title, desc, icon] : controlToggles) {
@@ -200,6 +205,22 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
       modelSelectorButton->setValue(models[params.getInt("Model")]);
       addItem(modelSelectorButton);
 
+    } 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 {
       toggle = new ParamControl(param, title, desc, icon, this);
     }
@@ -235,7 +256,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
 
   conditionalExperimentalKeys = {"CECurves", "CECurvesLead", "CESlowerLead", "CENavigation", "CEStopLights", "CESignal"};
   fireTheBabysitterKeys = {"NoLogging", "MuteDM", "MuteDoor", "MuteOverheated", "MuteSeatbelt"};
-  laneChangeKeys = {};
+  laneChangeKeys = {"LaneChangeTime", "LaneDetection", "OneLaneChange"};
   lateralTuneKeys = {"AverageCurvature", "NNFF"};
   longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "StoppingDistance"};
   speedLimitControllerKeys = {};