Speed limit controller
Added toggle to control the cruise set speed according to speed limit supplied by OSM, NOO, or the vehicle itself. Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev> Co-Authored-By: Efini <19368997+efini@users.noreply.github.com> Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>
This commit is contained in:
FrogAi
@@ -99,6 +99,11 @@ class ConditionalExperimentalMode:
|
||||
self.status_value = 5
|
||||
return True
|
||||
|
||||
# Speed Limit Controller check
|
||||
if self.params_memory.get_bool("SLCExperimentalMode"):
|
||||
self.status_value = 6
|
||||
return True
|
||||
|
||||
# Speed check
|
||||
if (not self.lead_detected and v_ego < self.limit) or (self.lead_detected and v_ego < self.limit_lead):
|
||||
self.status_value = 7 if self.lead_detected else 8
|
||||
|
||||
@@ -10,6 +10,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
|
||||
from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode import ConditionalExperimentalMode
|
||||
from openpilot.selfdrive.frogpilot.functions.map_turn_speed_controller import MapTurnSpeedController
|
||||
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
|
||||
|
||||
# Acceleration profiles - Credit goes to the DragonPilot team!
|
||||
# MPH = [0., 35, 35, 40, 40, 45, 45, 67, 67, 67, 123]
|
||||
@@ -54,7 +55,11 @@ class FrogPilotPlanner:
|
||||
self.cem = ConditionalExperimentalMode()
|
||||
self.mtsc = MapTurnSpeedController()
|
||||
|
||||
self.override_slc = False
|
||||
|
||||
self.overridden_speed = 0
|
||||
self.mtsc_target = 0
|
||||
self.slc_target = 0
|
||||
self.v_cruise = 0
|
||||
|
||||
self.x_desired_trajectory = np.zeros(CONTROL_N)
|
||||
@@ -99,8 +104,39 @@ class FrogPilotPlanner:
|
||||
else:
|
||||
self.mtsc_target = v_cruise
|
||||
|
||||
# Pfeiferj's Speed Limit Controller
|
||||
if self.speed_limit_controller:
|
||||
SpeedLimitController.update_current_max_velocity(v_cruise)
|
||||
self.slc_target = SpeedLimitController.desired_speed_limit
|
||||
|
||||
# Override SLC upon gas pedal press and reset upon brake/cancel button
|
||||
if self.speed_limit_controller_override:
|
||||
self.override_slc |= carState.gasPressed
|
||||
self.override_slc &= enabled
|
||||
self.override_slc &= v_ego > self.slc_target
|
||||
else:
|
||||
self.override_slc = False
|
||||
|
||||
self.overridden_speed *= enabled
|
||||
|
||||
# Use the override speed if SLC is being overridden
|
||||
if self.override_slc:
|
||||
if self.speed_limit_controller_override == 1:
|
||||
# Set the max speed to the manual set speed
|
||||
if carState.gasPressed:
|
||||
self.overridden_speed = np.clip(v_ego, self.slc_target, v_cruise)
|
||||
self.slc_target = self.overridden_speed
|
||||
elif self.speed_limit_controller_override == 2:
|
||||
self.overridden_speed = v_cruise
|
||||
self.slc_target = v_cruise
|
||||
if self.slc_target == 0:
|
||||
self.slc_target = v_cruise
|
||||
else:
|
||||
self.overriden_speed = 0
|
||||
self.slc_target = v_cruise
|
||||
|
||||
v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0)
|
||||
return min(v_cruise, self.mtsc_target) - v_ego_diff
|
||||
return min(v_cruise, self.mtsc_target, self.slc_target) - v_ego_diff
|
||||
|
||||
def publish_lateral(self, sm, pm, DH):
|
||||
frogpilot_lateral_plan_send = messaging.new_message('frogpilotLateralPlan')
|
||||
@@ -127,6 +163,11 @@ class FrogPilotPlanner:
|
||||
frogpilotLongitudinalPlan.desiredFollowDistance = mpc.safe_obstacle_distance - mpc.stopped_equivalence_factor
|
||||
frogpilotLongitudinalPlan.safeObstacleDistanceStock = mpc.safe_obstacle_distance_stock
|
||||
|
||||
frogpilotLongitudinalPlan.slcOverridden = self.override_slc
|
||||
frogpilotLongitudinalPlan.slcOverriddenSpeed = float(self.overridden_speed)
|
||||
frogpilotLongitudinalPlan.slcSpeedLimit = float(self.slc_target)
|
||||
frogpilotLongitudinalPlan.slcSpeedLimitOffset = float(SpeedLimitController.offset)
|
||||
|
||||
pm.send('frogpilotLongitudinalPlan', frogpilot_longitudinal_plan_send)
|
||||
|
||||
def update_frogpilot_params(self, params):
|
||||
@@ -163,3 +204,8 @@ class FrogPilotPlanner:
|
||||
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
|
||||
|
||||
self.speed_limit_controller = params.get_bool("SpeedLimitController")
|
||||
if self.speed_limit_controller:
|
||||
self.speed_limit_controller_override = params.get_int("SLCOverride")
|
||||
SpeedLimitController.update_frogpilot_params()
|
||||
|
||||
119
selfdrive/frogpilot/functions/speed_limit_controller.py
Normal file
119
selfdrive/frogpilot/functions/speed_limit_controller.py
Normal file
@@ -0,0 +1,119 @@
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.params import Params
|
||||
import json
|
||||
|
||||
params = Params()
|
||||
params_memory = Params("/dev/shm/params")
|
||||
|
||||
class SpeedLimitController:
|
||||
car_speed_limit: float = 0 # m/s
|
||||
lst_speed_limit: float = 0 # m/s
|
||||
map_speed_limit: float = 0 # m/s
|
||||
nav_speed_limit: float = 0 # m/s
|
||||
|
||||
def __init__(self) -> None:
|
||||
self.update_frogpilot_params()
|
||||
self.write_car_state()
|
||||
self.write_map_state()
|
||||
self.write_nav_state()
|
||||
|
||||
def update_current_max_velocity(self, max_v: float, load_state: bool = True, write_state: bool = True) -> None:
|
||||
if load_state:
|
||||
self.load_state()
|
||||
|
||||
@property
|
||||
def offset(self) -> float:
|
||||
if self.speed_limit < 14:
|
||||
return self.offset1
|
||||
elif self.speed_limit < 24:
|
||||
return self.offset2
|
||||
elif self.speed_limit < 29:
|
||||
return self.offset3
|
||||
else:
|
||||
return self.offset4
|
||||
|
||||
@property
|
||||
def speed_limit(self) -> float:
|
||||
params_memory.put_bool("SLCExperimentalMode", False)
|
||||
|
||||
limits = [self.car_speed_limit, self.map_speed_limit, self.nav_speed_limit]
|
||||
filtered_limits = [limit for limit in limits if limit > 0]
|
||||
|
||||
if self.highest and filtered_limits:
|
||||
return max(filtered_limits)
|
||||
if self.lowest and filtered_limits:
|
||||
return min(filtered_limits)
|
||||
|
||||
priority_orders = [
|
||||
["nav", "car", "map"],
|
||||
["nav", "map", "car"],
|
||||
["nav", "map"],
|
||||
["nav", "car"],
|
||||
["nav"],
|
||||
["map", "car", "nav"],
|
||||
["map", "nav", "car"],
|
||||
["map", "nav"],
|
||||
["map", "car"],
|
||||
["map"],
|
||||
["car", "nav", "map"],
|
||||
["car", "map", "nav"],
|
||||
["car", "map"],
|
||||
["car", "nav"],
|
||||
["car"]
|
||||
]
|
||||
|
||||
if self.speed_limit_priority < len(priority_orders):
|
||||
for source in priority_orders[self.speed_limit_priority]:
|
||||
limit = getattr(self, f"{source}_speed_limit", 0)
|
||||
if limit > 0:
|
||||
self.prv_speed_limit = limit
|
||||
return limit
|
||||
|
||||
if self.use_experimental_mode:
|
||||
params_memory.put_bool("SLCExperimentalMode", True)
|
||||
return 0
|
||||
elif self.use_previous_limit:
|
||||
if self.lst_speed_limit != self.prv_speed_limit:
|
||||
params.put_int("PreviousSpeedLimit", self.prv_speed_limit * 100)
|
||||
self.lst_speed_limit = self.prv_speed_limit
|
||||
return self.prv_speed_limit
|
||||
|
||||
return 0
|
||||
|
||||
@property
|
||||
def desired_speed_limit(self):
|
||||
return self.speed_limit + self.offset if self.speed_limit else 0
|
||||
|
||||
def load_state(self):
|
||||
self.car_speed_limit = json.loads(params_memory.get("CarSpeedLimit"))
|
||||
self.map_speed_limit = json.loads(params_memory.get("MapSpeedLimit"))
|
||||
self.nav_speed_limit = json.loads(params_memory.get("NavSpeedLimit"))
|
||||
|
||||
def write_car_state(self):
|
||||
params_memory.put("CarSpeedLimit", json.dumps(self.car_speed_limit))
|
||||
|
||||
def write_map_state(self):
|
||||
params_memory.put("MapSpeedLimit", json.dumps(self.map_speed_limit))
|
||||
|
||||
def write_nav_state(self):
|
||||
params_memory.put("NavSpeedLimit", json.dumps(self.nav_speed_limit))
|
||||
|
||||
def update_frogpilot_params(self):
|
||||
conversion = CV.KPH_TO_MS if params.get_bool("IsMetric") else CV.MPH_TO_MS
|
||||
|
||||
self.offset1 = params.get_int("Offset1") * conversion
|
||||
self.offset2 = params.get_int("Offset2") * conversion
|
||||
self.offset3 = params.get_int("Offset3") * conversion
|
||||
self.offset4 = params.get_int("Offset4") * conversion
|
||||
|
||||
self.speed_limit_priority = params.get_int("SLCPriority")
|
||||
self.highest = self.speed_limit_priority == 15
|
||||
self.lowest = self.speed_limit_priority == 16
|
||||
|
||||
slc_fallback = params.get_int("SLCFallback")
|
||||
self.use_experimental_mode = slc_fallback == 1
|
||||
self.use_previous_limit = slc_fallback == 2
|
||||
|
||||
self.prv_speed_limit = params.get_int("PreviousSpeedLimit") / 100
|
||||
|
||||
SpeedLimitController = SpeedLimitController()
|
||||
@@ -41,6 +41,15 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
|
||||
{"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.", ""},
|
||||
{"PauseLateralOnSignal", "Pause Lateral On Turn Signal", "Temporarily disable lateral control during turn signal use.", ""},
|
||||
|
||||
{"SpeedLimitController", "Speed Limit Controller", "Automatically adjust vehicle speed to match speed limits using 'Open Street Map's, 'Navigate On openpilot', or your car's dashboard (TSS2 Toyotas only).", "../assets/offroad/icon_speed_limit.png"},
|
||||
{"Offset1", "Speed Limit Offset (0-34 mph)", "Speed limit offset for speed limits between 0-34 mph.", ""},
|
||||
{"Offset2", "Speed Limit Offset (35-54 mph)", "Speed limit offset for speed limits between 35-54 mph.", ""},
|
||||
{"Offset3", "Speed Limit Offset (55-64 mph)", "Speed limit offset for speed limits between 55-64 mph.", ""},
|
||||
{"Offset4", "Speed Limit Offset (65-99 mph)", "Speed limit offset for speed limits between 65-99 mph.", ""},
|
||||
{"SLCFallback", "Fallback Method", "Choose your fallback method for when there are no speed limits currently being obtained from Navigation, OSM, and the car's dashboard.", ""},
|
||||
{"SLCOverride", "Override Method", "Choose your preferred method to override the current speed limit.", ""},
|
||||
{"SLCPriority", "Priority Order", "Determine the priority order for what speed limits to use.", ""},
|
||||
};
|
||||
|
||||
for (const auto &[param, title, desc, icon] : controlToggles) {
|
||||
@@ -223,6 +232,91 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
|
||||
}
|
||||
toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 10, laneChangeTimeLabels, this, false);
|
||||
|
||||
} else if (param == "SpeedLimitController") {
|
||||
FrogPilotParamManageControl *speedLimitControllerToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
|
||||
QObject::connect(speedLimitControllerToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
|
||||
parentToggleClicked();
|
||||
for (auto &[key, toggle] : toggles) {
|
||||
toggle->setVisible(speedLimitControllerKeys.find(key.c_str()) != speedLimitControllerKeys.end());
|
||||
}
|
||||
slscPriorityButton->setVisible(true);
|
||||
});
|
||||
toggle = speedLimitControllerToggle;
|
||||
} else if (param == "Offset1" || param == "Offset2" || param == "Offset3" || param == "Offset4") {
|
||||
toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 99, std::map<int, QString>(), this, false, " mph");
|
||||
} else if (param == "SLCFallback") {
|
||||
std::vector<QString> fallbackOptions{tr("None"), tr("Experimental Mode"), tr("Previous Limit")};
|
||||
FrogPilotButtonParamControl *fallbackSelection = new FrogPilotButtonParamControl(param, title, desc, icon, fallbackOptions);
|
||||
toggle = fallbackSelection;
|
||||
} else if (param == "SLCOverride") {
|
||||
std::vector<QString> overrideOptions{tr("None"), tr("Manual Set Speed"), tr("Max Set Speed")};
|
||||
FrogPilotButtonParamControl *overrideSelection = new FrogPilotButtonParamControl(param, title, desc, icon, overrideOptions);
|
||||
toggle = overrideSelection;
|
||||
} else if (param == "SLCPriority") {
|
||||
const QStringList priorities {
|
||||
"Navigation, Dashboard, Offline Maps",
|
||||
"Navigation, Offline Maps, Dashboard",
|
||||
"Navigation, Offline Maps",
|
||||
"Navigation, Dashboard",
|
||||
"Navigation",
|
||||
"Offline Maps, Dashboard, Navigation",
|
||||
"Offline Maps, Navigation, Dashboard",
|
||||
"Offline Maps, Navigation",
|
||||
"Offline Maps, Dashboard",
|
||||
"Offline Maps",
|
||||
"Dashboard, Navigation, Offline Maps",
|
||||
"Dashboard, Offline Maps, Navigation",
|
||||
"Dashboard, Offline Maps",
|
||||
"Dashboard, Navigation",
|
||||
"Dashboard",
|
||||
"Highest",
|
||||
"Lowest",
|
||||
"",
|
||||
};
|
||||
|
||||
slscPriorityButton = new ButtonControl(title, tr("SELECT"), desc);
|
||||
QObject::connect(slscPriorityButton, &ButtonControl::clicked, this, [this, priorities]() {
|
||||
QStringList availablePriorities = {"Dashboard", "Navigation", "Offline Maps", "Highest", "Lowest", "None"};
|
||||
QStringList selectedPriorities;
|
||||
int priorityValue = -1;
|
||||
|
||||
QStringList priorityPrompts = {tr("Select your primary priority"), tr("Select your secondary priority"), tr("Select your tertiary priority")};
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
QString selection = MultiOptionDialog::getSelection(priorityPrompts[i], availablePriorities, "", this);
|
||||
if (selection.isEmpty()) break;
|
||||
|
||||
if (selection == "None") {
|
||||
priorityValue = 17;
|
||||
break;
|
||||
} else if (selection == "Highest") {
|
||||
priorityValue = 15;
|
||||
break;
|
||||
} else if (selection == "Lowest") {
|
||||
priorityValue = 16;
|
||||
break;
|
||||
} else {
|
||||
selectedPriorities.append(selection);
|
||||
availablePriorities.removeAll(selection);
|
||||
availablePriorities.removeAll("Highest");
|
||||
availablePriorities.removeAll("Lowest");
|
||||
}
|
||||
}
|
||||
|
||||
if (priorityValue == -1 && !selectedPriorities.isEmpty()) {
|
||||
QString priorityString = selectedPriorities.join(", ");
|
||||
priorityValue = priorities.indexOf(priorityString);
|
||||
}
|
||||
|
||||
if (priorityValue != -1) {
|
||||
slscPriorityButton->setValue(priorities[priorityValue]);
|
||||
params.putInt("SLCPriority", priorityValue);
|
||||
updateToggles();
|
||||
}
|
||||
});
|
||||
slscPriorityButton->setValue(priorities[params.getInt("SLCPriority")]);
|
||||
addItem(slscPriorityButton);
|
||||
|
||||
} else {
|
||||
toggle = new ParamControl(param, title, desc, icon, this);
|
||||
}
|
||||
@@ -261,7 +355,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
|
||||
laneChangeKeys = {"LaneChangeTime", "LaneDetection", "OneLaneChange", "PauseLateralOnSignal"};
|
||||
lateralTuneKeys = {"AverageCurvature", "NNFF"};
|
||||
longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance"};
|
||||
speedLimitControllerKeys = {};
|
||||
speedLimitControllerKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "SLCFallback", "SLCOverride", "SLCPriority"};
|
||||
visionTurnControlKeys = {};
|
||||
|
||||
QObject::connect(device(), &Device::interactiveTimeout, this, &FrogPilotControlsPanel::hideSubToggles);
|
||||
@@ -289,17 +383,57 @@ 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("Offset1", std::nearbyint(params.getInt("Offset1") * speedConversion));
|
||||
params.putInt("Offset2", std::nearbyint(params.getInt("Offset2") * speedConversion));
|
||||
params.putInt("Offset3", std::nearbyint(params.getInt("Offset3") * speedConversion));
|
||||
params.putInt("Offset4", std::nearbyint(params.getInt("Offset4") * speedConversion));
|
||||
params.putInt("StoppingDistance", std::nearbyint(params.getInt("StoppingDistance") * distanceConversion));
|
||||
}
|
||||
|
||||
FrogPilotParamValueControl *offset1Toggle = static_cast<FrogPilotParamValueControl*>(toggles["Offset1"]);
|
||||
FrogPilotParamValueControl *offset2Toggle = static_cast<FrogPilotParamValueControl*>(toggles["Offset2"]);
|
||||
FrogPilotParamValueControl *offset3Toggle = static_cast<FrogPilotParamValueControl*>(toggles["Offset3"]);
|
||||
FrogPilotParamValueControl *offset4Toggle = static_cast<FrogPilotParamValueControl*>(toggles["Offset4"]);
|
||||
FrogPilotParamValueControl *stoppingDistanceToggle = static_cast<FrogPilotParamValueControl*>(toggles["StoppingDistance"]);
|
||||
|
||||
if (isMetric) {
|
||||
offset1Toggle->setTitle("Speed Limit Offset (0-34 kph)");
|
||||
offset2Toggle->setTitle("Speed Limit Offset (35-54 kph)");
|
||||
offset3Toggle->setTitle("Speed Limit Offset (55-64 kph)");
|
||||
offset4Toggle->setTitle("Speed Limit Offset (65-99 kph)");
|
||||
|
||||
offset1Toggle->setDescription("Set speed limit offset for limits between 0-34 kph.");
|
||||
offset2Toggle->setDescription("Set speed limit offset for limits between 35-54 kph.");
|
||||
offset3Toggle->setDescription("Set speed limit offset for limits between 55-64 kph.");
|
||||
offset4Toggle->setDescription("Set speed limit offset for limits between 65-99 kph.");
|
||||
|
||||
offset1Toggle->updateControl(0, 99, " kph");
|
||||
offset2Toggle->updateControl(0, 99, " kph");
|
||||
offset3Toggle->updateControl(0, 99, " kph");
|
||||
offset4Toggle->updateControl(0, 99, " kph");
|
||||
stoppingDistanceToggle->updateControl(0, 5, " meters");
|
||||
} else {
|
||||
offset1Toggle->setTitle("Speed Limit Offset (0-34 mph)");
|
||||
offset2Toggle->setTitle("Speed Limit Offset (35-54 mph)");
|
||||
offset3Toggle->setTitle("Speed Limit Offset (55-64 mph)");
|
||||
offset4Toggle->setTitle("Speed Limit Offset (65-99 mph)");
|
||||
|
||||
offset1Toggle->setDescription("Set speed limit offset for limits between 0-34 mph.");
|
||||
offset2Toggle->setDescription("Set speed limit offset for limits between 35-54 mph.");
|
||||
offset3Toggle->setDescription("Set speed limit offset for limits between 55-64 mph.");
|
||||
offset4Toggle->setDescription("Set speed limit offset for limits between 65-99 mph.");
|
||||
|
||||
offset1Toggle->updateControl(0, 99, " mph");
|
||||
offset2Toggle->updateControl(0, 99, " mph");
|
||||
offset3Toggle->updateControl(0, 99, " mph");
|
||||
offset4Toggle->updateControl(0, 99, " mph");
|
||||
stoppingDistanceToggle->updateControl(0, 10, " feet");
|
||||
}
|
||||
|
||||
offset1Toggle->refresh();
|
||||
offset2Toggle->refresh();
|
||||
offset3Toggle->refresh();
|
||||
offset4Toggle->refresh();
|
||||
stoppingDistanceToggle->refresh();
|
||||
|
||||
previousIsMetric = isMetric;
|
||||
@@ -310,6 +444,7 @@ void FrogPilotControlsPanel::parentToggleClicked() {
|
||||
conditionalSpeedsImperial->setVisible(false);
|
||||
conditionalSpeedsMetric->setVisible(false);
|
||||
modelSelectorButton->setVisible(false);
|
||||
slscPriorityButton->setVisible(false);
|
||||
standardProfile->setVisible(false);
|
||||
relaxedProfile->setVisible(false);
|
||||
|
||||
@@ -321,6 +456,7 @@ void FrogPilotControlsPanel::hideSubToggles() {
|
||||
conditionalSpeedsImperial->setVisible(false);
|
||||
conditionalSpeedsMetric->setVisible(false);
|
||||
modelSelectorButton->setVisible(true);
|
||||
slscPriorityButton->setVisible(false);
|
||||
standardProfile->setVisible(false);
|
||||
relaxedProfile->setVisible(false);
|
||||
|
||||
|
||||
@@ -22,6 +22,8 @@ private:
|
||||
void updateMetric();
|
||||
void updateToggles();
|
||||
|
||||
ButtonControl *slscPriorityButton;
|
||||
|
||||
FrogPilotButtonIconControl *modelSelectorButton;
|
||||
|
||||
FrogPilotDualParamControl *aggressiveProfile;
|
||||
|
||||
Reference in New Issue
Block a user