brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Traffic Mode

Browse Source 
Added toggle to change the longitudinal behavior to be more focused on driving in traffic.
This commit is contained in:
FrogAi
2024-03-15 19:56:43 -07:00
parent 3e5c66abeb
commit bed144260f
13 changed files with 64 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
common/params.cc
View File

@@ -413,6 +413,8 @@ std::unordered_map<std::string, uint32_t> keys = {
{"StockTune", PERSISTENT},
{"StoppingDistance", PERSISTENT},
{"TetheringEnabled", PERSISTENT},
{"TrafficMode", PERSISTENT},
{"TrafficModeActive", PERSISTENT},
{"UnlimitedLength", PERSISTENT},
{"Updated", PERSISTENT},
{"UpdateSchedule", PERSISTENT},

1
selfdrive/car/toyota/carstate.py
View File

@@ -226,6 +226,7 @@ class CarState(CarStateBase):
self.fpf.update_traffic_mode()
# Revert the previous changes to Experimental Mode
if frogpilot_variables.experimental_mode_via_distance:
if frogpilot_variables.conditional_experimental_mode:
self.fpf.update_cestatus_distance()
else:

1
selfdrive/controls/controlsd.py
View File

@@ -1240,6 +1240,7 @@ class Controls:
longitudinal_tune = self.params.get_bool("LongitudinalTune")
self.frogpilot_variables.sport_plus = longitudinal_tune and self.params.get_int("AccelerationProfile") == 3
self.frogpilot_variables.traffic_mode = longitudinal_tune and self.params.get_bool("TrafficMode")
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

12
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View File

@@ -327,10 +327,10 @@ class LongitudinalMpc:
lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
return lead_xv
def process_lead(self, lead, increased_stopping_distance):
def process_lead(self, lead, frogpilot_planner):
v_ego = self.x0[1]
if lead is not None and lead.status:
x_lead = lead.dRel - increased_stopping_distance
x_lead = lead.dRel - (frogpilot_planner.increased_stopping_distance if not frogpilot_planner.traffic_mode_active else 0)
v_lead = lead.vLead
a_lead = lead.aLeadK
a_lead_tau = lead.aLeadTau
@@ -357,13 +357,17 @@ class LongitudinalMpc:
self.max_a = max_a
def update(self, radarstate, v_cruise, x, v, a, j, frogpilot_planner, personality=log.LongitudinalPersonality.standard):
if frogpilot_planner.traffic_mode_active:
t_follow = frogpilot_planner.traffic_mode_t_follow
else:
t_follow = get_T_FOLLOW(frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_follow, frogpilot_planner.standard_follow, frogpilot_planner.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, frogpilot_planner.increased_stopping_distance)
lead_xv_1 = self.process_lead(radarstate.leadTwo, frogpilot_planner.increased_stopping_distance)
lead_xv_0 = self.process_lead(radarstate.leadOne, frogpilot_planner)
lead_xv_1 = self.process_lead(radarstate.leadTwo, frogpilot_planner)
# Offset by FrogAi for FrogPilot for a more natural takeoff with a lead
if frogpilot_planner.aggressive_acceleration:

5
selfdrive/frogpilot/functions/frogpilot_functions.py
View File

@@ -113,6 +113,11 @@ class FrogPilotFunctions:
# Invert the value of "ExperimentalMode"
self.params.put_bool("ExperimentalMode", not experimental_mode)
def update_traffic_mode(self):
traffic_mode = self.params_memory.get_bool("TrafficModeActive")
# Invert the value of "TrafficModeActive"
self.params_memory.put_bool("TrafficModeActive", not traffic_mode)
@staticmethod
def road_curvature(modelData, v_ego):
predicted_velocities = np.array(modelData.velocity.x)

13
selfdrive/frogpilot/functions/frogpilot_planner.py
View File

@@ -2,18 +2,20 @@ import cereal.messaging as messaging
import numpy as np
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import clip
from openpilot.common.numpy_fast import clip, interp
from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel
from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED, FrogPilotFunctions
from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, FrogPilotFunctions
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
TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT]
TRAFFIC_MODE_T_FOLLOW = [.50, 1.]
class FrogPilotPlanner:
def __init__(self, CP, params, params_memory):
@@ -26,6 +28,7 @@ class FrogPilotPlanner:
self.mtsc = MapTurnSpeedController()
self.override_slc = False
self.traffic_mode_active = False
self.overridden_speed = 0
self.mtsc_target = 0
@@ -84,6 +87,11 @@ class FrogPilotPlanner:
# Update the current road curvature
self.road_curvature = self.fpf.road_curvature(modelData, v_ego)
# Update the current state of "Traffic Mode"
self.traffic_mode_active = self.traffic_mode and self.params_memory.get_bool("TrafficModeActive")
if self.traffic_mode_active:
self.traffic_mode_t_follow = interp(v_ego, TRAFFIC_MODE_BP, TRAFFIC_MODE_T_FOLLOW)
# Update the desired stopping distance
self.stop_distance = STOP_DISTANCE
@@ -209,6 +217,7 @@ class FrogPilotPlanner:
self.aggressive_acceleration = longitudinal_tune and params.get_bool("AggressiveAcceleration")
self.increased_stopping_distance = params.get_int("StoppingDistance") * (1 if self.is_metric else CV.FOOT_TO_METER) if longitudinal_tune else 0
self.smoother_braking = longitudinal_tune and params.get_bool("SmoothBraking")
self.traffic_mode = longitudinal_tune and params.get_bool("TrafficMode")
self.map_turn_speed_controller = params.get_bool("MTSCEnabled")
if self.map_turn_speed_controller:

1
selfdrive/frogpilot/ui/control_settings.cc
View File

@@ -41,6 +41,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
{"AggressiveAcceleration", "Aggressive Acceleration With Lead", "Increase acceleration aggressiveness when following a lead vehicle from a stop.", ""},
{"StoppingDistance", "Increase Stop Distance Behind Lead", "Increase the stopping distance for a more comfortable stop from lead vehicles.", ""},
{"SmoothBraking", "Smoother Braking Behind Lead", "Smoothen out the braking behavior when approaching slower vehicles.", ""},
{"TrafficMode", "Traffic Mode", "Hold down the 'distance' button for 2.5 seconds to enable more aggressive driving behavior catered towards stop and go traffic.", ""},
{"Model", "Model Selector", "Choose your preferred openpilot model.", "../assets/offroad/icon_calibration.png"},

2
selfdrive/frogpilot/ui/control_settings.h
View File

@@ -45,7 +45,7 @@ private:
std::set<QString> fireTheBabysitterKeys = {"NoLogging", "MuteOverheated", "NoUploads", "OfflineMode"};
std::set<QString> laneChangeKeys = {"LaneChangeTime", "LaneDetection", "LaneDetectionWidth", "OneLaneChange"};
std::set<QString> lateralTuneKeys = {"ForceAutoTune", "NNFF", "UseLateralJerk"};
std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance"};
std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance", "TrafficMode"};
std::set<QString> mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit"};
std::set<QString> qolKeys = {"DisableOnroadUploads", "HigherBitrate", "NavChill", "PauseLateralOnSignal", "ReverseCruise", "SetSpeedOffset"};
std::set<QString> speedLimitControllerKeys = {"SLCControls", "SLCQOL", "SLCVisuals"};

2
selfdrive/manager/manager.py
View File

@@ -260,6 +260,7 @@ def manager_init() -> None:
("SteerRatio", "0"),
("StockTune", "0"),
("StoppingDistance", "0"),
("TrafficMode", "0"),
("TurnAggressiveness", "100"),
("TurnDesires", "0"),
("UnlimitedLength", "1"),
@@ -390,6 +391,7 @@ def manager_thread() -> None:
params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION)
elif not started and started_prev:
params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION)
params_memory.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION)
# Clear the error log on offroad transition to prevent old errors from hanging around
if os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')):

11
selfdrive/ui/qt/onroad.cc
View File

@@ -493,6 +493,7 @@ void ExperimentalButton::paintEvent(QPaintEvent *event) {
(scene.always_on_lateral_active ? QColor(10, 186, 181, 255) :
(scene.conditional_status == 1 ? QColor(255, 246, 0, 255) :
(experimental_mode ? QColor(218, 111, 37, 241) :
(scene.traffic_mode && scene.traffic_mode_active ? QColor(201, 34, 49, 255) :
(scene.navigate_on_openpilot ? QColor(49, 161, 238, 255) : QColor(0, 0, 0, 166)))))) :
QColor(0, 0, 0, 166);
@@ -658,6 +659,8 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
), 6));
} else if (scene.reverse_cruise) {
p.setPen(QPen(QColor(0, 150, 255), 6));
} else if (trafficModeActive) {
p.setPen(QPen(redColor(255), 6));
} else {
p.setPen(QPen(whiteColor(75), 6));
}
@@ -906,6 +909,10 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
pe.setColorAt(0.0, QColor::fromHslF(25 / 360., 0.71, 0.50, 1.0));
pe.setColorAt(0.5, QColor::fromHslF(25 / 360., 0.71, 0.50, 0.5));
pe.setColorAt(1.0, QColor::fromHslF(25 / 360., 0.71, 0.50, 0.1));
} else if (trafficModeActive) {
pe.setColorAt(0.0, QColor::fromHslF(355 / 360., 0.71, 0.46, 1.0));
pe.setColorAt(0.5, QColor::fromHslF(355 / 360., 0.71, 0.46, 0.5));
pe.setColorAt(1.0, QColor::fromHslF(355 / 360., 0.71, 0.46, 0.1));
} else if (scene.navigate_on_openpilot) {
pe.setColorAt(0.0, QColor::fromHslF(205 / 360., 0.85, 0.56, 1.0));
pe.setColorAt(0.5, QColor::fromHslF(205 / 360., 0.85, 0.56, 0.5));
@@ -1347,6 +1354,8 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH);
useViennaSLCSign = scene.use_vienna_slc_sign;
trafficModeActive = scene.traffic_mode && scene.traffic_mode_active;
turnSignalLeft = scene.turn_signal_left;
turnSignalRight = scene.turn_signal_right;
@@ -1602,7 +1611,7 @@ void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) {
double currentAcceleration = std::round(scene.acceleration * 100) / 100;
static double maxAcceleration = 0.0;
if (currentAcceleration > maxAcceleration && status == STATUS_ENGAGED) {
if (currentAcceleration > maxAcceleration && (status == STATUS_ENGAGED || status == STATUS_TRAFFIC_MODE_ACTIVE)) {
maxAcceleration = currentAcceleration;
isFiveSecondsPassed = false;
timer.start();

1
selfdrive/ui/qt/onroad.h
View File

@@ -229,6 +229,7 @@ private:
bool showSLCOffset;
bool slcOverridden;
bool speedLimitController;
bool trafficModeActive;
bool turnSignalLeft;
bool turnSignalRight;
bool useViennaSLCSign;

20
selfdrive/ui/ui.cc
View File

@@ -349,6 +349,9 @@ void ui_update_frogpilot_params(UIState *s) {
scene.experimental_mode_via_screen = params.getBool("ExperimentalModeViaScreen") && params.getBool("ExperimentalModeActivation");
scene.fahrenheit = params.getBool("Fahrenheit");
bool longitudinal_tune = params.getBool("LongitudinalTune");
scene.traffic_mode = longitudinal_tune && params.getBool("TrafficMode");
scene.model_ui = params.getBool("ModelUI");
scene.dynamic_path_width = scene.model_ui && params.getBool("DynamicPathWidth");
scene.hide_lead_marker = scene.model_ui && params.getBool("HideLeadMarker");
@@ -401,7 +404,9 @@ void UIState::updateStatus() {
if (state == cereal::ControlsState::OpenpilotState::PRE_ENABLED || state == cereal::ControlsState::OpenpilotState::OVERRIDING) {
status = STATUS_OVERRIDE;
} else if (scene.always_on_lateral_active) {
status = STATUS_LATERAL_ACTIVE;
status = STATUS_ALWAYS_ON_LATERAL_ACTIVE;
} else if (scene.traffic_mode_active) {
status = STATUS_TRAFFIC_MODE_ACTIVE;
} else {
status = controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED;
}
@@ -469,15 +474,10 @@ void UIState::update() {
}
// FrogPilot live variables that need to be constantly checked
if (scene.conditional_experimental) {
scene.conditional_status = paramsMemory.getInt("CEStatus");
}
if (scene.holiday_themes) {
scene.current_holiday_theme = paramsMemory.getInt("CurrentHolidayTheme");
}
if (scene.random_events) {
scene.current_random_event = paramsMemory.getInt("CurrentRandomEvent");
}
scene.conditional_status = scene.conditional_experimental ? paramsMemory.getInt("CEStatus") : 0;
scene.current_holiday_theme = scene.holiday_themes ? paramsMemory.getInt("CurrentHolidayTheme") : 0;
scene.current_random_event = scene.random_events ? paramsMemory.getInt("CurrentRandomEvent") : 0;
scene.traffic_mode_active = scene.conditional_experimental && scene.enabled && paramsMemory.getBool("TrafficModeActive");
}
void UIState::setPrimeType(PrimeType type) {

8
selfdrive/ui/ui.h
View File

@@ -113,7 +113,8 @@ typedef enum UIStatus {
STATUS_ENGAGED,
// FrogPilot statuses
STATUS_LATERAL_ACTIVE,
STATUS_ALWAYS_ON_LATERAL_ACTIVE,
STATUS_TRAFFIC_MODE_ACTIVE,
} UIStatus;
enum PrimeType {
@@ -132,7 +133,8 @@ const QColor bg_colors [] = {
[STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1),
// FrogPilot colors
[STATUS_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1),
[STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1),
[STATUS_TRAFFIC_MODE_ACTIVE] = QColor(0xc9, 0x22, 0x31, 0xf1),
};
static std::map<cereal::ControlsState::AlertStatus, QColor> alert_colors = {
@@ -228,6 +230,8 @@ typedef struct UIScene {
bool standstill;
bool status_changed;
bool tethering_enabled;
bool traffic_mode;
bool traffic_mode_active;
bool turn_signal_left;
bool turn_signal_right;
bool unlimited_road_ui_length;