diff --git a/cereal/custom.capnp b/cereal/custom.capnp index fdb89da84..5468e0f39 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -172,6 +172,8 @@ struct OnroadEventSP @0xda96579883444c35 { experimentalModeSwitched @14; wrongCarModeAlertOnly @15; pedalPressedAlertOnly @16; + laneTurnLeft @17; + laneTurnRight @18; } } @@ -258,9 +260,16 @@ struct LiveMapDataSP @0xf416ec09499d9d19 { roadName @5 :Text; } -struct CustomReserved9 @0xa1680744031fdb2d { +struct ModelDataV2SP @0xa1680744031fdb2d { + laneTurnDirection @0 :TurnDirection; } + enum TurnDirection { + none @0; + turnLeft @1; + turnRight @2; + } + struct CustomReserved10 @0xcb9fd56c7057593a { } diff --git a/cereal/log.capnp b/cereal/log.capnp index 020176bad..874b2b6eb 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2633,7 +2633,7 @@ struct Event { backupManagerSP @113 :Custom.BackupManagerSP; carStateSP @114 :Custom.CarStateSP; liveMapDataSP @115 :Custom.LiveMapDataSP; - customReserved9 @116 :Custom.CustomReserved9; + modelDataV2SP @116 :Custom.ModelDataV2SP; customReserved10 @136 :Custom.CustomReserved10; customReserved11 @137 :Custom.CustomReserved11; customReserved12 @138 :Custom.CustomReserved12; diff --git a/cereal/services.py b/cereal/services.py index 373e865e3..b48290bc7 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -88,6 +88,7 @@ _services: dict[str, tuple] = { "carControlSP": (True, 100., 10), "carStateSP": (True, 100., 10), "liveMapDataSP": (True, 1., 1), + "modelDataV2SP": (True, 20.), # debug "uiDebug": (True, 0., 1), diff --git a/common/params_keys.h b/common/params_keys.h index eb388c90a..425b83700 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -204,10 +204,12 @@ inline static std::unordered_map keys = { {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}}, {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}}, - // model panel params + // sunnypilot model params {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}}, {"LagdToggleDelay", {PERSISTENT | BACKUP, FLOAT, "0.2"}}, {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}}, + {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}}, // mapd {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}}, diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 2adfa65f6..93088182f 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -1,7 +1,8 @@ -from cereal import log +from cereal import log, custom from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode +from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection @@ -30,6 +31,12 @@ DESIRES = { }, } +TURN_DESIRES = { + custom.TurnDirection.none: log.Desire.none, + custom.TurnDirection.turnLeft: log.Desire.turnLeft, + custom.TurnDirection.turnRight: log.Desire.turnRight, +} + class DesireHelper: def __init__(self): @@ -41,13 +48,21 @@ class DesireHelper: self.prev_one_blinker = False self.desire = log.Desire.none self.alc = AutoLaneChangeController(self) + self.lane_turn_controller = LaneTurnController(self) + self.lane_turn_direction = custom.TurnDirection.none def update(self, carstate, lateral_active, lane_change_prob): self.alc.update_params() + self.lane_turn_controller.update_params() v_ego = carstate.vEgo one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN + # Lane turn controller update + self.lane_turn_controller.update_lane_turn(blindspot_left=carstate.leftBlindspot, blindspot_right=carstate.rightBlindspot, + left_blinker=carstate.leftBlinker, right_blinker=carstate.rightBlinker, v_ego=v_ego) + self.lane_turn_direction = self.lane_turn_controller.get_turn_direction() + if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX or self.alc.lane_change_set_timer == AutoLaneChangeMode.OFF: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none @@ -106,7 +121,10 @@ class DesireHelper: self.prev_one_blinker = one_blinker - self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] + if self.lane_turn_direction != custom.TurnDirection.none: + self.desire = TURN_DESIRES[self.lane_turn_direction] + else: + self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] # Send keep pulse once per second during LaneChangeStart.preLaneChange if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting): diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index caf342e88..5f9ccf507 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -216,7 +216,7 @@ def main(demo=False): cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})") # messaging - pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"]) + pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"]) sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"]) publish_state = PublishState() @@ -333,6 +333,7 @@ def main(demo=False): modelv2_send = messaging.new_message('modelV2') drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') + mdv2sp_send = messaging.new_message('modelDataV2SP') action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) prev_action = action @@ -347,6 +348,7 @@ def main(demo=False): DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction + mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction @@ -354,6 +356,7 @@ def main(demo=False): pm.send('modelV2', modelv2_send) pm.send('drivingModelData', drivingdata_send) pm.send('cameraOdometry', posenet_send) + pm.send('modelDataV2SP', mdv2sp_send) last_vipc_frame_id = meta_main.frame_id diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 249621d6f..c56c7fe65 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -86,7 +86,7 @@ class SelfdriveD(CruiseHelper): # TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches self.car_state_sock = messaging.sub_sock('carState', timeout=20) - ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ['modelDataV2SP'] if SIMULATION: ignore += ['driverCameraState', 'managerState'] if REPLAY: @@ -95,7 +95,8 @@ class SelfdriveD(CruiseHelper): self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', - 'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback'] + \ + 'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback', + 'modelDataV2SP'] + \ self.camera_packets + self.sensor_packets + self.gps_packets, ignore_alive=ignore, ignore_avg_freq=ignore, ignore_valid=ignore, frequency=int(1/DT_CTRL)) @@ -300,6 +301,13 @@ class SelfdriveD(CruiseHelper): LaneChangeState.laneChangeFinishing): self.events.add(EventName.laneChange) + # Handle lane turn + lane_turn_direction = self.sm['modelDataV2SP'].laneTurnDirection + if lane_turn_direction == custom.TurnDirection.turnLeft: + self.events_sp.add(custom.OnroadEventSP.EventName.laneTurnLeft) + elif lane_turn_direction == custom.TurnDirection.turnRight: + self.events_sp.add(custom.OnroadEventSP.EventName.laneTurnRight) + for i, pandaState in enumerate(self.sm['pandaStates']): # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput if i < len(self.CP.safetyConfigs): diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc index baba7d3a1..02a01a4b6 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc @@ -48,6 +48,25 @@ static const QString progressStyleError = progressStyleActive + " background-color: transparent;" "}"; +std::optional safeParamEventLoad(Params& params, const std::string& paramName) { + std::string raw = params.get(paramName); + if (raw.empty()) { + return std::nullopt; + } + + try { + AlignedBuffer alignedBuf; + auto buf = alignedBuf.align(raw.data(), raw.size()); + + capnp::FlatArrayMessageReader msg(kj::ArrayPtr(buf.begin(), buf.size())); + return msg.getRoot(); + } + catch (const kj::Exception& e) { + qInfo() << "Invalid param" << QString::fromStdString(paramName) << ":" << e.getDescription().cStr(); + return std::nullopt; + } +} + ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) { QVBoxLayout *main_layout = new QVBoxLayout(this); main_layout->setContentsMargins(50, 20, 50, 20); @@ -101,9 +120,32 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) { QString policyType = tr("Policy Model"); policyFrame = createModelDetailFrame(this, policyType, policyProgressBar); list->addItem(policyFrame); - list->addItem(horizontal_line()); + // Lane Turn Desire toggle + lane_turn_desire_toggle = new ParamControlSP("LaneTurnDesire", tr("Use Lane Turn Desires"), + "If you’re driving at 20 mph (32 km/h) or below and have your blinker on, " + "the car will plan a turn in that direction at the nearest drivable path. " + "This prevents situations (like at red lights) where the car might plan the wrong turn direction.", + "../assets/offroad/icon_shell.png"); + list->addItem(lane_turn_desire_toggle); + + // Lane Turn Value control + int max_value_mph = 20; + bool is_metric_initial = params.getBool("IsMetric"); + const float K = 1.609344f; + int per_value_change_scaled = is_metric_initial ? static_cast(std::round((1.0f / K) * 100.0f)) : 100; // 100 -> 1 mph + lane_turn_value_control = new OptionControlSP("LaneTurnValue", tr("Adjust Lane Turn Speed"), + tr("Set the maximum speed for lane turn desires. Default is 19 %1.").arg(is_metric_initial ? "km/h" : "mph"), + "", {5 * 100, max_value_mph * 100}, per_value_change_scaled, false, nullptr, true, true); + lane_turn_value_control->showDescription(); + list->addItem(lane_turn_value_control); + + // Show based on toggle + refreshLaneTurnValueControl(); + connect(lane_turn_desire_toggle, &ParamControlSP::toggleFlipped, this, &ModelsPanel::refreshLaneTurnValueControl); + connect(lane_turn_value_control, &OptionControlSP::updateLabels, this, &ModelsPanel::refreshLaneTurnValueControl); + // LiveDelay toggle lagd_toggle_control = new ParamControlSP("LagdToggle", tr("Live Learning Steer Delay"), "", "../assets/offroad/icon_shell.png"); lagd_toggle_control->showDescription(); @@ -111,17 +153,10 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) { // Software delay control int liveDelayMaxInt = 30; - std::string liveDelayBytes = params.get("LiveDelay"); - if (!liveDelayBytes.empty()) { - capnp::FlatArrayMessageReader msg(kj::ArrayPtr( - reinterpret_cast(liveDelayBytes.data()), - liveDelayBytes.size() / sizeof(capnp::word))); - auto event = msg.getRoot(); - if (event.hasLiveDelay()) { - auto liveDelay = event.getLiveDelay(); - float lateralDelay = liveDelay.getLateralDelay(); - liveDelayMaxInt = static_cast(lateralDelay * 100.0f) + 20; - } + if (const auto event = safeParamEventLoad(params, "LiveDelay"); event && event->hasLiveDelay()) { + auto liveDelay = event->getLiveDelay(); + float lateralDelay = liveDelay.getLateralDelay(); + liveDelayMaxInt = static_cast(lateralDelay * 100.0f) + 20; } delay_control = new OptionControlSP("LagdToggleDelay", tr("Adjust Software Delay"), tr("Adjust the software delay when Live Learning Steer Delay is toggled off." @@ -159,6 +194,19 @@ QFrame* ModelsPanel::createModelDetailFrame(QWidget *parent, QString &typeName, return frame; } +void ModelsPanel::refreshLaneTurnValueControl() { + if (!lane_turn_value_control) return; + float stored_mph = QString::fromStdString(params.get("LaneTurnValue")).toFloat(); + bool is_metric = params.getBool("IsMetric"); + QString unit = is_metric ? "km/h" : "mph"; + float display_value = stored_mph; + if (is_metric) { + display_value = stored_mph * 1.609344f; + } + lane_turn_value_control->setLabel(QString::number(static_cast(std::round(display_value))) + " " + unit); + lane_turn_value_control->setVisible(params.getBool("LaneTurnDesire")); +} + /** * @brief Updates the UI with bundle download progress information * Reads status from modelManagerSP cereal message and displays status for all models @@ -401,18 +449,11 @@ void ModelsPanel::updateLabels() { "Disable to use a fixed steering response time. Keeping this on provides the stock openpilot experience."); bool lagdEnabled = params.getBool("LagdToggle"); if (lagdEnabled) { - std::string liveDelayBytes = params.get("LiveDelay"); - if (!liveDelayBytes.empty()) { - capnp::FlatArrayMessageReader msg(kj::ArrayPtr( - reinterpret_cast(liveDelayBytes.data()), - liveDelayBytes.size() / sizeof(capnp::word))); - auto event = msg.getRoot(); - if (event.hasLiveDelay()) { - auto liveDelay = event.getLiveDelay(); - float lateralDelay = liveDelay.getLateralDelay(); - desc += QString("

%1 %2 s") + if (const auto event = safeParamEventLoad(params, "LiveDelay"); event && event->hasLiveDelay()) { + auto liveDelay = event->getLiveDelay(); + float lateralDelay = liveDelay.getLateralDelay(); + desc += QString("

%1 %2 s") .arg(tr("Live Steer Delay:")).arg(QString::number(lateralDelay, 'f', 3)); - } } } else { std::string carParamsBytes = params.get("CarParamsPersistent"); @@ -439,6 +480,9 @@ void ModelsPanel::updateLabels() { delay_control->setLabel(QString::number(value, 'f', 2) + "s"); } + // Update lane turn desire label and visibility + refreshLaneTurnValueControl(); + clearModelCacheBtn->setValue(QString::number(calculateCacheSize(), 'f', 2) + " MB"); } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h index 93edc4de1..1906ebd2a 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h @@ -37,6 +37,7 @@ private: void updateLabels(); void handleCurrentModelLblBtnClicked(); void handleBundleDownloadProgress(); + void refreshLaneTurnValueControl(); void showResetParamsDialog(); QProgressBar* createProgressBar(QWidget *parent); QFrame* createModelDetailFrame(QWidget *parent, QString &typeName, QProgressBar *progressBar); @@ -81,5 +82,6 @@ private: Params params; ButtonControlSP *clearModelCacheBtn; ButtonControlSP *refreshAvailableModelsBtn; - + ParamControlSP *lane_turn_desire_toggle; + OptionControlSP *lane_turn_value_control; }; diff --git a/sunnypilot/modeld/modeld.py b/sunnypilot/modeld/modeld.py index b94e166bc..3d11ed23f 100755 --- a/sunnypilot/modeld/modeld.py +++ b/sunnypilot/modeld/modeld.py @@ -177,7 +177,7 @@ def main(demo=False): cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})") # messaging - pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"]) + pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"]) sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"]) publish_state = PublishState() @@ -304,6 +304,7 @@ def main(demo=False): modelv2_send = messaging.new_message('modelV2') drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') + mdv2sp_send = messaging.new_message('modelDataV2SP') action = model.get_action_from_model(model_output, prev_action, long_delay + DT_MDL) fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen, @@ -316,6 +317,7 @@ def main(demo=False): DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction + mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction @@ -323,6 +325,7 @@ def main(demo=False): pm.send('modelV2', modelv2_send) pm.send('drivingModelData', drivingdata_send) pm.send('cameraOdometry', posenet_send) + pm.send('modelDataV2SP', mdv2sp_send) last_vipc_frame_id = meta_main.frame_id diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index 1a420e6f1..bf89bc98d 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -202,7 +202,7 @@ def main(demo=False): cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})") # messaging - pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"]) + pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"]) sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"]) publish_state = PublishState() @@ -322,6 +322,7 @@ def main(demo=False): modelv2_send = messaging.new_message('modelV2') drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') + mdv2sp_send = messaging.new_message('modelDataV2SP') action = model.get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego) prev_action = action @@ -336,6 +337,7 @@ def main(demo=False): DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction + mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction @@ -343,6 +345,7 @@ def main(demo=False): pm.send('modelV2', modelv2_send) pm.send('drivingModelData', drivingdata_send) pm.send('cameraOdometry', posenet_send) + pm.send('modelDataV2SP', mdv2sp_send) last_vipc_frame_id = meta_main.frame_id diff --git a/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py b/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py new file mode 100644 index 000000000..00ce026ab --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py @@ -0,0 +1,45 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from cereal import custom + +from openpilot.common.constants import CV +from openpilot.common.params import Params + +LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS + + +class LaneTurnController: + def __init__(self, desire_helper): + self.DH = desire_helper + self.turn_direction = custom.TurnDirection.none + self.params = Params() + self.lane_turn_value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS + self.param_read_counter = 0 + self.enabled = self.params.get_bool("LaneTurnDesire") + + def read_params(self): + self.enabled = self.params.get_bool("LaneTurnDesire") + value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS + self.lane_turn_value = min(float(LANE_CHANGE_SPEED_MIN), value) + + def update_params(self) -> None: + if self.param_read_counter % 50 == 0: + self.read_params() + self.param_read_counter += 1 + + def update_lane_turn(self, blindspot_left: bool, blindspot_right: bool, left_blinker: bool, right_blinker: bool, v_ego: float) -> None: + if left_blinker and not right_blinker and v_ego < self.lane_turn_value and not blindspot_left: + self.turn_direction = custom.TurnDirection.turnLeft + elif right_blinker and not left_blinker and v_ego < self.lane_turn_value and not blindspot_right: + self.turn_direction = custom.TurnDirection.turnRight + else: + self.turn_direction = custom.TurnDirection.none + + def get_turn_direction(self): + if not self.enabled: + return custom.TurnDirection.none + return self.turn_direction diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py b/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py new file mode 100644 index 000000000..5633ed6ef --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py @@ -0,0 +1,113 @@ +import pytest +from cereal import log +from openpilot.common.params import Params + +from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper +from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN +from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode + + +class TurnDirection: + none = 0 + turnLeft = 1 + turnRight = 2 + + +@pytest.mark.parametrize("left_blinker,right_blinker,v_ego,blindspot_left,blindspot_right,expected", [ + (True, False, 5, False, False, TurnDirection.turnLeft), + (False, True, 6, False, False, TurnDirection.turnRight), + (True, False, 9, False, False, TurnDirection.none), + (True, False, 7, True, False, TurnDirection.none), + (False, True, 6, False, True, TurnDirection.none), + (False, False, 5, False, False, TurnDirection.none), + (True, True, 5, False, False, TurnDirection.none), +]) +def test_lane_turn_desire_conditions(left_blinker, right_blinker, v_ego, blindspot_left, blindspot_right, expected): + dh = DesireHelper() + controller = LaneTurnController(dh) + controller.enabled = True + controller.lane_turn_value = LANE_CHANGE_SPEED_MIN + controller.turn_direction = TurnDirection.none + controller.update_lane_turn(blindspot_left, blindspot_right, left_blinker, right_blinker, v_ego) + assert controller.get_turn_direction() == expected + + +def test_lane_turn_desire_disabled(): + dh = DesireHelper() + controller = LaneTurnController(dh) + controller.enabled = False + controller.lane_turn_value = LANE_CHANGE_SPEED_MIN + controller.turn_direction = TurnDirection.none + controller.update_lane_turn(False, False, True, False, 7) + assert controller.get_turn_direction() == TurnDirection.none + + +def test_lane_turn_overrides_lane_change(): + dh = DesireHelper() + controller = LaneTurnController(dh) + controller.enabled = True + controller.lane_turn_value = LANE_CHANGE_SPEED_MIN + controller.turn_direction = TurnDirection.none + # left turn desire + controller.update_lane_turn(False, False, True, False, 5) + assert controller.get_turn_direction() == TurnDirection.turnLeft + # right turn desire + controller.update_lane_turn(False, False, False, True, 6) + assert controller.get_turn_direction() == TurnDirection.turnRight + # no turn + controller.update_lane_turn(False, False, False, False, 7) + assert controller.get_turn_direction() == TurnDirection.none + + +@pytest.mark.parametrize("v_ego,expected", [ + (8.93, TurnDirection.turnLeft), # just below threshold + (8.96, TurnDirection.none), # above threshold + (8.95, TurnDirection.none), # just above threshold +]) +def test_lane_turn_desire_speed_boundary(v_ego, expected): + dh = DesireHelper() + controller = LaneTurnController(dh) + controller.enabled = True + controller.lane_turn_value = LANE_CHANGE_SPEED_MIN + controller.turn_direction = TurnDirection.none + controller.update_lane_turn(False, True, True, False, v_ego) + assert controller.get_turn_direction() == expected + + +class DummyCarState: + def __init__(self, vEgo=0, leftBlinker=False, rightBlinker=False, leftBlindspot=False, rightBlindspot=False, + steeringPressed=False, steeringTorque=0, brakePressed=False): + self.vEgo = vEgo + self.leftBlinker = leftBlinker + self.rightBlinker = rightBlinker + self.leftBlindspot = leftBlindspot + self.rightBlindspot = rightBlindspot + self.steeringPressed = steeringPressed + self.steeringTorque = steeringTorque + self.brakePressed = brakePressed + +@pytest.fixture +def set_lane_turn_params(): + params = Params() + params.put("LaneTurnDesire", True) + params.put("LaneTurnValue", 20.0) + +@pytest.mark.parametrize("carstate, lateral_active, lane_change_prob, expected_desire", [ + # Lane turn desire overrides lane change desire + (DummyCarState(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False), True, 1.0, log.Desire.turnLeft), + (DummyCarState(vEgo=7, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False), True, 1.0, log.Desire.turnRight), + # Lane change desire only (no turn desires) + (DummyCarState(vEgo=9, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False, + steeringPressed=True, steeringTorque=1), True, 1.0, log.Desire.laneChangeLeft), + (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False, + steeringPressed=True, steeringTorque=-1), True, 1.0, log.Desire.laneChangeRight), + # No desire (inactive) + (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=False), False, 1.0, log.Desire.none), + (DummyCarState(vEgo=4, leftBlinker=False, rightBlinker=False), True, 1.0, log.Desire.none), # No blinkers? no desire! +]) +def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params): + dh = DesireHelper() + dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE + for _ in range(10): + dh.update(carstate, lateral_active, lane_change_prob) + assert dh.desire == expected_desire # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers diff --git a/sunnypilot/selfdrive/selfdrived/events.py b/sunnypilot/selfdrive/selfdrived/events.py index d81345a14..7c723397f 100644 --- a/sunnypilot/selfdrive/selfdrived/events.py +++ b/sunnypilot/selfdrive/selfdrived/events.py @@ -132,6 +132,21 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = { EventNameSP.pedalPressedAlertOnly: { ET.WARNING: NoEntryAlert("Pedal Pressed") - } + }, + EventNameSP.laneTurnLeft: { + ET.WARNING: Alert( + "Turning Left", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.), + }, + + EventNameSP.laneTurnRight: { + ET.WARNING: Alert( + "Turning Right", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.), + } }