From 54c9cc3195626fc3aafbc4c677c590521c66bf7e Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Fri, 12 Jan 2024 22:39:30 -0700 Subject: [PATCH] Increase the stopping distance Added toggle to increase the stopping distance. --- common/params.cc | 1 + .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 12 ++++++------ selfdrive/controls/lib/longitudinal_planner.py | 2 +- selfdrive/frogpilot/functions/frogpilot_planner.py | 1 + selfdrive/frogpilot/ui/control_settings.cc | 12 +++++++++++- 5 files changed, 20 insertions(+), 8 deletions(-) diff --git a/common/params.cc b/common/params.cc index 7761ba9..b59a104 100644 --- a/common/params.cc +++ b/common/params.cc @@ -282,6 +282,7 @@ std::unordered_map keys = { {"ShowStorageUsed", PERSISTENT}, {"StandardFollow", PERSISTENT}, {"StandardJerk", PERSISTENT}, + {"StoppingDistance", PERSISTENT}, {"UnlimitedLength", PERSISTENT}, {"Updated", PERSISTENT}, {"UpdateSchedule", PERSISTENT}, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 43e2574..bfcf207 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -322,10 +322,10 @@ class LongitudinalMpc: lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) return lead_xv - def process_lead(self, lead): + def process_lead(self, lead, increased_stopping_distance): v_ego = self.x0[1] if lead is not None and lead.status: - x_lead = lead.dRel + x_lead = lead.dRel - increased_stopping_distance v_lead = lead.vLead a_lead = lead.aLeadK a_lead_tau = lead.aLeadTau @@ -351,19 +351,19 @@ class LongitudinalMpc: self.cruise_min_a = min_a self.max_a = max_a - def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, custom_personalities, aggressive_follow, standard_follow, relaxed_follow, personality=log.LongitudinalPersonality.standard): + def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, custom_personalities, aggressive_follow, standard_follow, relaxed_follow, increased_stopping_distance, personality=log.LongitudinalPersonality.standard): t_follow = get_T_FOLLOW(custom_personalities, aggressive_follow, standard_follow, relaxed_follow, personality) self.t_follow = t_follow v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status - lead_xv_0 = self.process_lead(radarstate.leadOne) - lead_xv_1 = self.process_lead(radarstate.leadTwo) + lead_xv_0 = self.process_lead(radarstate.leadOne, increased_stopping_distance) + lead_xv_1 = self.process_lead(radarstate.leadTwo, increased_stopping_distance) # Offset by FrogAi for FrogPilot for a more natural takeoff with a lead if aggressive_acceleration: distance_factor = np.maximum(1, lead_xv_0[:,0] - (lead_xv_0[:,1] * t_follow)) - t_follow_offset = np.clip((lead_xv_0[:,1] - v_ego) + (STOP_DISTANCE - v_ego), 1, distance_factor) + t_follow_offset = np.clip((lead_xv_0[:,1] - v_ego) + (STOP_DISTANCE + increased_stopping_distance - v_ego), 1, distance_factor) t_follow = t_follow / t_follow_offset # LongitudinalPlan variables for onroad driving insights diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index cda535d..ce9ce2b 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -139,7 +139,7 @@ class LongitudinalPlanner: self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) - self.mpc.update(sm['radarState'], frogpilot_planner.v_cruise, x, v, a, j, frogpilot_planner.aggressive_acceleration, + self.mpc.update(sm['radarState'], frogpilot_planner.v_cruise, x, v, a, j, frogpilot_planner.aggressive_acceleration, frogpilot_planner.increased_stopping_distance, frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_follow, frogpilot_planner.standard_follow, frogpilot_planner.relaxed_follow, personality=self.personality) self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution) diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 14666dd..d934ff1 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -139,3 +139,4 @@ class FrogPilotPlanner: longitudinal_tune = params.get_bool("LongitudinalTune") self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0 self.aggressive_acceleration = params.get_bool("AggressiveAcceleration") and longitudinal_tune + self.increased_stopping_distance = params.get_int("StoppingDistance") * (1 if self.is_metric else CV.FOOT_TO_METER) if longitudinal_tune else 0 diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 7481939..d2fefd1 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -29,6 +29,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, {"AccelerationProfile", "Acceleration Profile", "Change the acceleration rate to be either sporty or eco-friendly.", ""}, {"AggressiveAcceleration", "Aggressive Acceleration With Lead", "Increase acceleration aggressiveness when following a lead vehicle from a stop.", ""}, + {"StoppingDistance", "Increased Stopping Distance", "Increase the stopping distance for a more comfortable stop.", ""}, }; for (const auto &[param, title, desc, icon] : controlToggles) { @@ -170,6 +171,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil "I understand the risks.", this); } }); + } else if (param == "StoppingDistance") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 10, std::map(), this, false, " feet"); } else { toggle = new ParamControl(param, title, desc, icon, this); @@ -208,7 +211,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil fireTheBabysitterKeys = {"NoLogging", "MuteDM", "MuteDoor", "MuteOverheated", "MuteSeatbelt"}; laneChangeKeys = {}; lateralTuneKeys = {"AverageCurvature"}; - longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration"}; + longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "StoppingDistance"}; speedLimitControllerKeys = {}; visionTurnControlKeys = {}; @@ -237,12 +240,19 @@ void FrogPilotControlsPanel::updateMetric() { double speedConversion = isMetric ? MILE_TO_KM : KM_TO_MILE; params.putInt("CESpeed", std::nearbyint(params.getInt("CESpeed") * speedConversion)); params.putInt("CESpeedLead", std::nearbyint(params.getInt("CESpeedLead") * speedConversion)); + params.putInt("StoppingDistance", std::nearbyint(params.getInt("StoppingDistance") * distanceConversion)); } + FrogPilotParamValueControl *stoppingDistanceToggle = static_cast(toggles["StoppingDistance"]); + if (isMetric) { + stoppingDistanceToggle->updateControl(0, 5, " meters"); } else { + stoppingDistanceToggle->updateControl(0, 10, " feet"); } + stoppingDistanceToggle->refresh(); + previousIsMetric = isMetric; }