diff --git a/common/params.cc b/common/params.cc index fb132af..157ffda 100644 --- a/common/params.cc +++ b/common/params.cc @@ -258,6 +258,8 @@ std::unordered_map keys = { {"GoatScream", PERSISTENT}, {"GreenLightAlert", PERSISTENT}, {"HideSpeed", PERSISTENT}, + {"LaneChangeTime", PERSISTENT}, + {"LaneDetection", PERSISTENT}, {"LaneLinesWidth", PERSISTENT}, {"LateralTune", PERSISTENT}, {"LeadInfo", PERSISTENT}, @@ -282,7 +284,9 @@ std::unordered_map keys = { {"NNFFModelFuzzyMatch", PERSISTENT}, {"NNFFModelName", PERSISTENT}, {"NoLogging", PERSISTENT}, + {"NudgelessLaneChange", PERSISTENT}, {"OfflineMode", PERSISTENT}, + {"OneLaneChange", PERSISTENT}, {"PathEdgeWidth", PERSISTENT}, {"PathWidth", PERSISTENT}, {"RelaxedFollow", PERSISTENT}, diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 0dc991c..f6f6578 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/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 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 0000000..cd8e40a Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_lane.png differ diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 918bbdd..a4434b6 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/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 diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 0f56ae2..5e074e9 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/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 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 = {};