From ddf63701e8d4cd16045efe5638ca089ac9dc608c Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 19 Sep 2025 18:32:20 -0400 Subject: [PATCH] Speed Limit: Resolver (#1256) * init * some fixes * move * more * old navd helpers * bring back cereal * fix linting * more * add to cereal first * sp events * lint * implement in long plan * fixme-sp * refactor state machine * wrong state * start refactor controller * some type hints * init these * enable debug print * ui? ui! * print them out * fix spinner import * fix path * let's use gps chips directly for now * service missing * publish events * no nav for now * need to sub * no car state speed yet * missed event * Car: `CarStateSP` * fix tests * bring back car state speed limit * fix * use old controller for now * fix * fix source * type hints * none for now * formatting * more * create directory if does not exist * mypy my bt * policy param catch exceptions * handle all params with exceptions * more * single method * define types in init * rename * simpler op enabled check * more mypy stuff * rename * no need for brake pressed * don't reset if gas pressed * type hint all * type hint all * back to upstream * in another pr * no longer need data type * qlog * slc in another pr * use horizontal accuracy * use horizontal accuracy * set core affinity for all realtime processes * unused * sort * unused * type hint and slight cleanup * from old implementation * use directly * combine pm * slight more cleanup * type hints * even more type hint * Revert "slc in another pr" This reverts commit 3a6987e6 * Revert "in another pr" This reverts commit a29bccff12b9ed9d64efe7a5722bf614214002c4. * rebump * no need to check alive * use it directly * fix test * refactor * use gps data directly * quote...? * lint * fix tests * use CC.longActive * user confirm in another PR * rename * fix import * params fix * no more * fix * drop new state machine for now * more fixes * internalize output * unused * rearrange * auto draft * rename * this * no * no need * use existing * wrong cruise speed * fix * not used for now * Revert "not used for now" This reverts commit f0083d6241380e84ed259c402941fa2c78e03fdf. * some * use frames instead * split speed limit resolver out of slc * no need to pass sm * fix params * test init * use frame instead of time * track session * some tests * too limiting * bump * always reset state * end session if long_active but slc inactive at any given time * off * no warning in this PR * no speed factor engage type yet * wide open * no * introduce disabled, no longer transitions at inactive * fix tests * no more tempinactive * clean * rename * offset default > off * new tests, fixes controller * more tests * not really needed yet * lint * fix * some more tests * wrap * more * more * use vCruiseCluster for set speed * init better * finish it up * no * typo * one method state machine * refactor preactive timeout check * refactor new session check * directly return statuses * comments * v_target * refactor speed limit resolver * turn off debug * more resolver refactor * no longer needed * lint * more lint * fix * move around * fix events * update event * already happens while in enabled * add carstateSP * less * Speed Limit Control -> Speed Limit Assist * in another PR * more rename * overriding state * fix * make sure to return the correct type * just slr in this one * more * update * redundant * fix * fix * lint * fix * fix * match toggle * fix priority checks * fix combined source for picking 0 limit * no need to wrap * add speed limit offset to resolver * add speed limit offset * make sure it displays distance when higher * Revert "make sure it displays distance when higher" This reverts commit 15c6834d4e68e2ba8f717c9442ec8d4d269d2b83. * some rename * translations * unused for now * more * lint --------- Co-authored-by: nayan Co-authored-by: DevTekVE --- cereal/custom.capnp | 19 +++ common/params_keys.h | 5 + selfdrive/controls/plannerd.py | 6 +- .../test/longitudinal_maneuvers/plant.py | 8 +- selfdrive/ui/sunnypilot/SConscript | 2 + .../longitudinal/speed_limit/helpers.h | 35 ++++ .../speed_limit/speed_limit_policy.cc | 53 ++++++ .../speed_limit/speed_limit_policy.h | 55 +++++++ .../speed_limit/speed_limit_settings.cc | 112 +++++++++++++ .../speed_limit/speed_limit_settings.h | 53 ++++++ .../qt/offroad/settings/longitudinal_panel.cc | 29 ++++ .../qt/offroad/settings/longitudinal_panel.h | 3 + sunnypilot/mapd/live_map_data/debug.py | 11 +- .../controls/lib/longitudinal_planner.py | 13 ++ .../smart_cruise_control/vision_controller.py | 4 +- .../controls/lib/speed_limit/__init__.py | 8 + .../controls/lib/speed_limit/common.py | 21 +++ .../lib/speed_limit/speed_limit_resolver.py | 152 ++++++++++++++++++ .../lib/speed_limit/tests/__init__.py | 0 .../tests/test_speed_limit_resolver.py | 144 +++++++++++++++++ 20 files changed, 723 insertions(+), 10 deletions(-) create mode 100644 selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h create mode 100644 selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc create mode 100644 selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h create mode 100644 selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc create mode 100644 selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h create mode 100644 sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py create mode 100644 sunnypilot/selfdrive/controls/lib/speed_limit/common.py create mode 100644 sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py create mode 100644 sunnypilot/selfdrive/controls/lib/speed_limit/tests/__init__.py create mode 100644 sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_resolver.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index c4b546402..652783809 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -145,6 +145,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { dec @0 :DynamicExperimentalControl; longitudinalPlanSource @1 :LongitudinalPlanSource; smartCruiseControl @2 :SmartCruiseControl; + speedLimit @3 :SpeedLimit; struct DynamicExperimentalControl { state @0 :DynamicExperimentalControlState; @@ -180,6 +181,23 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { } } + struct SpeedLimit { + resolver @0 :Resolver; + + struct Resolver { + speedLimit @0 :Float32; + distToSpeedLimit @1 :Float32; + source @2 :Source; + speedLimitOffset @3 :Float32; + } + + enum Source { + none @0; + car @1; + map @2; + } + } + enum LongitudinalPlanSource { cruise @0; sccVision @1; @@ -316,6 +334,7 @@ struct BackupManagerSP @0xf98d843bfd7004a3 { } struct CarStateSP @0xb86e6369214c01c8 { + speedLimit @0 :Float32; } struct LiveMapDataSP @0xf416ec09499d9d19 { diff --git a/common/params_keys.h b/common/params_keys.h index 80d12c6b9..a9408037a 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -225,4 +225,9 @@ inline static std::unordered_map keys = { {"OsmStateTitle", {PERSISTENT, STRING}}, {"OsmWayTest", {PERSISTENT, STRING}}, {"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}}, + + // Speed Limit + {"SpeedLimitOffsetType", {PERSISTENT | BACKUP, INT, "0"}}, + {"SpeedLimitPolicy", {PERSISTENT | BACKUP, INT, "3"}}, + {"SpeedLimitValueOffset", {PERSISTENT | BACKUP, INT, "0"}}, }; diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 60036c308..8994fb7c0 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 from cereal import car +from openpilot.common.gps import get_gps_location_service from openpilot.common.params import Params from openpilot.common.realtime import Priority, config_realtime_process from openpilot.common.swaglog import cloudlog @@ -16,10 +17,13 @@ def main(): CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) cloudlog.info("plannerd got CarParams: %s", CP.brand) + gps_location_service = get_gps_location_service(params) + ldw = LaneDepartureWarning() longitudinal_planner = LongitudinalPlanner(CP) pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP']) - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'], + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState', + 'liveMapDataSP', 'carStateSP', gps_location_service], poll='modelV2') while True: diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index b8c6adb43..e46647243 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -67,6 +67,9 @@ class Plant: lp = messaging.new_message('liveParameters') car_control = messaging.new_message('carControl') model = messaging.new_message('modelV2') + car_state_sp = messaging.new_message('carStateSP') + live_map_data_sp = messaging.new_message('liveMapDataSP') + gps_data = messaging.new_message('gpsLocation') a_lead = (v_lead - self.v_lead_prev)/self.ts self.v_lead_prev = v_lead @@ -133,7 +136,10 @@ class Plant: 'controlsState': control.controlsState, 'selfdriveState': ss.selfdriveState, 'liveParameters': lp.liveParameters, - 'modelV2': model.modelV2} + 'modelV2': model.modelV2, + 'carStateSP': car_state_sp.carStateSP, + 'liveMapDataSP': live_map_data_sp.liveMapDataSP, + 'gpsLocation': gps_data.gpsLocation} self.planner.update(sm) self.acceleration = self.planner.output_a_target self.speed = self.speed + self.acceleration * self.ts diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript index 807bf0247..5c2680b29 100644 --- a/selfdrive/ui/sunnypilot/SConscript +++ b/selfdrive/ui/sunnypilot/SConscript @@ -54,6 +54,8 @@ lateral_panel_qt_src = [ longitudinal_panel_qt_src = [ "sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.cc", + "sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc", + "sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc", ] network_src = [ diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h new file mode 100644 index 000000000..84979a53a --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h @@ -0,0 +1,35 @@ +/* + * 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. + */ +#pragma once + +enum class SpeedLimitOffsetType { + NONE, + FIXED, + PERCENT, +}; + +inline const QString SpeedLimitOffsetTypeTexts[]{ + QObject::tr("None"), + QObject::tr("Fixed"), + QObject::tr("Percent"), +}; + +enum class SpeedLimitSourcePolicy { + CAR_ONLY, + MAP_ONLY, + CAR_FIRST, + MAP_FIRST, + COMBINED, +}; + +inline const QString SpeedLimitSourcePolicyTexts[]{ + QObject::tr("Car\nOnly"), + QObject::tr("Map\nOnly"), + QObject::tr("Car\nFirst"), + QObject::tr("Map\nFirst"), + QObject::tr("Combined\nData") +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc new file mode 100644 index 000000000..46f361a15 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc @@ -0,0 +1,53 @@ +/* + * 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. + */ + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h" + +SpeedLimitPolicy::SpeedLimitPolicy(QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(0, 0, 0, 0); + main_layout->setSpacing(0); + + // Back button + PanelBackButton *back = new PanelBackButton(tr("Back")); + connect(back, &QPushButton::clicked, [=]() { emit backPress(); }); + main_layout->addWidget(back, 0, Qt::AlignLeft); + + main_layout->addSpacing(10); + + ListWidgetSP *list = new ListWidgetSP(this); + + std::vector speed_limit_policy_texts{ + SpeedLimitSourcePolicyTexts[static_cast(SpeedLimitSourcePolicy::CAR_ONLY)], + SpeedLimitSourcePolicyTexts[static_cast(SpeedLimitSourcePolicy::MAP_ONLY)], + SpeedLimitSourcePolicyTexts[static_cast(SpeedLimitSourcePolicy::CAR_FIRST)], + SpeedLimitSourcePolicyTexts[static_cast(SpeedLimitSourcePolicy::MAP_FIRST)], + SpeedLimitSourcePolicyTexts[static_cast(SpeedLimitSourcePolicy::COMBINED)] + }; + speed_limit_policy = new ButtonParamControlSP( + "SpeedLimitPolicy", + tr("Speed Limit Source"), + "", + "", + speed_limit_policy_texts, + 250); + list->addItem(speed_limit_policy); + connect(speed_limit_policy, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitPolicy::refresh); + + refresh(); + main_layout->addWidget(list); +}; + +void SpeedLimitPolicy::refresh() { + SpeedLimitSourcePolicy policy_param = static_cast(std::atoi(params.get("SpeedLimitPolicy").c_str())); + speed_limit_policy->setDescription(sourceDescription(policy_param)); +} + +void SpeedLimitPolicy::showEvent(QShowEvent *event) { + refresh(); + speed_limit_policy->showDescription(); +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h new file mode 100644 index 000000000..adeb115db --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h @@ -0,0 +1,55 @@ +/* + * 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. + */ + +#pragma once + +#include "selfdrive/ui/sunnypilot/ui.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" + +class SpeedLimitPolicy : public QWidget { + Q_OBJECT + +public: + explicit SpeedLimitPolicy(QWidget *parent = nullptr); + void refresh(); + void showEvent(QShowEvent *event) override; + +signals: + void backPress(); + +private: + Params params; + ButtonParamControlSP *speed_limit_policy; + + static QString sourceDescription(SpeedLimitSourcePolicy type = SpeedLimitSourcePolicy::CAR_ONLY) { + QString car_only = tr("⦿ Car Only: Use Speed Limit data only from Car"); + QString map_only = tr("⦿ Map Only: Use Speed Limit data only from OpenStreetMaps"); + QString car_first = tr("⦿ Car First: Use Speed Limit data from Car if available, else use from OpenStreetMaps"); + QString map_first = tr("⦿ Map First: Use Speed Limit data from OpenStreetMaps if available, else use from Car"); + QString combined = tr("⦿ Combined: Use combined Speed Limit data from Car & OpenStreetMaps"); + + if (type == SpeedLimitSourcePolicy::CAR_ONLY) { + car_only = "" + car_only + ""; + } else if (type == SpeedLimitSourcePolicy::MAP_ONLY) { + map_only = "" + map_only + ""; + } else if (type == SpeedLimitSourcePolicy::CAR_FIRST) { + car_first = "" + car_first + ""; + } else if (type == SpeedLimitSourcePolicy::MAP_FIRST) { + map_first = "" + map_first + ""; + } else { + combined = "" + combined + ""; + } + + return QString("%1
%2
%3
%4
%5") + .arg(car_only) + .arg(map_only) + .arg(car_first) + .arg(map_first) + .arg(combined); + } +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc new file mode 100644 index 000000000..7508ea134 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc @@ -0,0 +1,112 @@ +/* + * 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. + */ + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h" + +SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent) { + subPanelFrame = new QFrame(); + QVBoxLayout *subPanelLayout = new QVBoxLayout(subPanelFrame); + subPanelLayout->setContentsMargins(0, 0, 0, 0); + subPanelLayout->setSpacing(0); + + // Back button + PanelBackButton *back = new PanelBackButton(tr("Back")); + connect(back, &QPushButton::clicked, [=]() { emit backPress(); }); + subPanelLayout->addWidget(back, 0, Qt::AlignLeft); + + subPanelLayout->addSpacing(20); + + ListWidgetSP *list = new ListWidgetSP(this); + + auto *speedLimitBtnFrame = new QFrame(this); + auto *speedLimitBtnFrameLayout = new QGridLayout(); + speedLimitBtnFrame->setLayout(speedLimitBtnFrameLayout); + speedLimitBtnFrameLayout->setContentsMargins(0, 40, 0, 40); + speedLimitBtnFrameLayout->setSpacing(0); + + speedLimitPolicyScreen = new SpeedLimitPolicy(this); + + speedLimitSource = new PushButtonSP(tr("Customize Source")); + connect(speedLimitSource, &QPushButton::clicked, [&]() { + setCurrentWidget(speedLimitPolicyScreen); + speedLimitPolicyScreen->refresh(); + }); + connect(speedLimitPolicyScreen, &SpeedLimitPolicy::backPress, [&]() { + setCurrentWidget(subPanelFrame); + showEvent(new QShowEvent()); + }); + + speedLimitSource->setFixedWidth(720); + speedLimitBtnFrameLayout->addWidget(speedLimitSource, 0, 0, Qt::AlignLeft); + list->addItem(speedLimitBtnFrame); + + QFrame *offsetFrame = new QFrame(this); + QVBoxLayout *offsetLayout = new QVBoxLayout(offsetFrame); + + std::vector speed_limit_offset_texts{ + SpeedLimitOffsetTypeTexts[static_cast(SpeedLimitOffsetType::NONE)], + SpeedLimitOffsetTypeTexts[static_cast(SpeedLimitOffsetType::FIXED)], + SpeedLimitOffsetTypeTexts[static_cast(SpeedLimitOffsetType::PERCENT)] + }; + speed_limit_offset_settings = new ButtonParamControlSP( + "SpeedLimitOffsetType", + tr("Speed Limit Offset"), + "", + "", + speed_limit_offset_texts, + 500); + + offsetLayout->addWidget(speed_limit_offset_settings); + + speed_limit_offset = new OptionControlSP( + "SpeedLimitValueOffset", + "", + "", + "", + {-30, 30} + ); + offsetLayout->addWidget(speed_limit_offset); + + list->addItem(offsetFrame); + + connect(speed_limit_offset, &OptionControlSP::updateLabels, this, &SpeedLimitSettings::refresh); + connect(speed_limit_offset_settings, &ButtonParamControlSP::showDescriptionEvent, speed_limit_offset, &OptionControlSP::showDescription); + connect(speed_limit_offset_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitSettings::refresh); + + refresh(); + subPanelLayout->addWidget(list); + addWidget(subPanelFrame); + addWidget(speedLimitPolicyScreen); + setCurrentWidget(subPanelFrame); +} + +void SpeedLimitSettings::refresh() { + bool is_metric_param = params.getBool("IsMetric"); + SpeedLimitOffsetType offset_type_param = static_cast(std::atoi(params.get("SpeedLimitOffsetType").c_str())); + QString offsetLabel = QString::fromStdString(params.get("SpeedLimitValueOffset")); + + speed_limit_offset->setDescription(offsetDescription(offset_type_param)); + + if (offset_type_param == SpeedLimitOffsetType::PERCENT) { + offsetLabel += "%"; + } else if (offset_type_param == SpeedLimitOffsetType::FIXED) { + offsetLabel += QString(" %1").arg(is_metric_param ? "km/h" : "mph"); + } + + if (offset_type_param == SpeedLimitOffsetType::NONE) { + speed_limit_offset->setVisible(false); + } else { + speed_limit_offset->setVisible(true); + speed_limit_offset->setLabel(offsetLabel); + speed_limit_offset->showDescription(); + } +} + +void SpeedLimitSettings::showEvent(QShowEvent *event) { + refresh(); + speed_limit_offset->showDescription(); +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h new file mode 100644 index 000000000..40ffead70 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h @@ -0,0 +1,53 @@ +/* + * 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. + */ + +#pragma once + +#include "selfdrive/ui/sunnypilot/ui.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" + +class SpeedLimitSettings : public QStackedWidget { + Q_OBJECT + +public: + SpeedLimitSettings(QWidget *parent = nullptr); + void refresh(); + void showEvent(QShowEvent *event) override; + +signals: + void backPress(); + +private: + Params params; + QFrame *subPanelFrame; + PushButtonSP *speedLimitSource; + SpeedLimitPolicy *speedLimitPolicyScreen; + ButtonParamControlSP *speed_limit_offset_settings; + OptionControlSP *speed_limit_offset; + + static QString offsetDescription(SpeedLimitOffsetType type = SpeedLimitOffsetType::NONE) { + QString none_str = tr("⦿ None: No Offset"); + QString fixed_str = tr("⦿ Fixed: Adds a fixed offset [Speed Limit + Offset]"); + QString percent_str = tr("⦿ Percent: Adds a percent offset [Speed Limit + (Offset % Speed Limit)]"); + + if (type == SpeedLimitOffsetType::FIXED) { + fixed_str = "" + fixed_str + ""; + } else if (type == SpeedLimitOffsetType::PERCENT) { + percent_str = "" + percent_str + ""; + } else { + none_str = "" + none_str + ""; + } + + return QString("%1
%2
%3") + .arg(none_str) + .arg(fixed_str) + .arg(percent_str); + } +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc index df4a6077b..205f64a0b 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc @@ -8,6 +8,21 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h" LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) { + setStyleSheet(R"( + #back_btn { + font-size: 50px; + margin: 0px; + padding: 15px; + border-width: 0; + border-radius: 30px; + color: #dddddd; + background-color: #393939; + } + #back_btn:pressed { + background-color: #4a4a4a; + } + )"); + main_layout = new QStackedLayout(this); ListWidget *list = new ListWidget(this, false); @@ -40,7 +55,21 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) { QObject::connect(uiState(), &UIState::offroadTransition, this, &LongitudinalPanel::refresh); + speedLimitSettings = new PushButtonSP(tr("Speed Limit"), 750, this); + connect(speedLimitSettings, &QPushButton::clicked, [&]() { + cruisePanelScroller->setLastScrollPosition(); + main_layout->setCurrentWidget(speedLimitScreen); + }); + list->addItem(speedLimitSettings); + + speedLimitScreen = new SpeedLimitSettings(this); + connect(speedLimitScreen, &SpeedLimitSettings::backPress, [=]() { + cruisePanelScroller->restoreScrollPosition(); + main_layout->setCurrentWidget(cruisePanelScreen); + }); + main_layout->addWidget(cruisePanelScreen); + main_layout->addWidget(speedLimitScreen); main_layout->setCurrentWidget(cruisePanelScreen); refresh(offroad); } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h index af8982259..846415065 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h @@ -8,6 +8,7 @@ #pragma once #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" @@ -32,4 +33,6 @@ private: CustomAccIncrement *customAccIncrement = nullptr; ParamControl *SmartCruiseControlVision; ParamControl *intelligentCruiseButtonManagement = nullptr; + SpeedLimitSettings *speedLimitScreen; + PushButtonSP *speedLimitSettings; }; diff --git a/sunnypilot/mapd/live_map_data/debug.py b/sunnypilot/mapd/live_map_data/debug.py index da9f4d777..794f2ef8f 100644 --- a/sunnypilot/mapd/live_map_data/debug.py +++ b/sunnypilot/mapd/live_map_data/debug.py @@ -37,15 +37,14 @@ def live_map_data_sp_thread(): def live_map_data_sp_thread_debug(gps_location_service): - _sub_master = messaging.SubMaster(['carState', 'livePose', 'liveMapDataSP', 'longitudinalPlanSP', gps_location_service]) + _sub_master = messaging.SubMaster(['carState', 'livePose', 'liveMapDataSP', 'longitudinalPlanSP', 'carStateSP', gps_location_service]) _sub_master.update() v_ego = _sub_master['carState'].vEgo - long_spl = _sub_master['longitudinalPlanSP'].speedLimit - _policy = Policy.car_state_priority - _resolver = SpeedLimitResolver(_policy) - _speed_limit, _distance, _source = _resolver.resolve(v_ego, long_spl, _sub_master) - print(_speed_limit, _distance, _source, " <-> ", long_spl) + _resolver = SpeedLimitResolver() + _resolver.policy = Policy.car_state_priority + _resolver.update(v_ego, _sub_master) + print(_resolver.speed_limit, _resolver.distance, _resolver.source) def main(): diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index f2358a264..b4f4fe6d7 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -9,6 +9,7 @@ from cereal import messaging, custom from opendbc.car import structs from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver from openpilot.sunnypilot.models.helpers import get_active_bundle DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState @@ -19,6 +20,7 @@ class LongitudinalPlannerSP: def __init__(self, CP: structs.CarParams, mpc): self.dec = DynamicExperimentalController(CP, mpc) self.scc = SmartCruiseControl() + self.resolver = SpeedLimitResolver() self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None self.source = Source.cruise @@ -36,6 +38,9 @@ class LongitudinalPlannerSP: def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]: self.scc.update(sm, v_ego, a_ego, v_cruise) + # Speed Limit Resolver + self.resolver.update(v_ego, sm) + targets = { Source.cruise: (v_cruise, a_ego), Source.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target) @@ -75,4 +80,12 @@ class LongitudinalPlannerSP: sccVision.enabled = self.scc.vision.is_enabled sccVision.active = self.scc.vision.is_active + # Speed Limit + speedLimit = longitudinalPlanSP.speedLimit + resolver = speedLimit.resolver + resolver.speedLimit = float(self.resolver.speed_limit) + resolver.speedLimitOffset = float(self.resolver.speed_limit_offset) + resolver.distToSpeedLimit = float(self.resolver.distance) + resolver.source = self.resolver.source + pm.send('longitudinalPlanSP', plan_sp_send) diff --git a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py index f12a00f23..491b17903 100644 --- a/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py +++ b/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py @@ -102,7 +102,7 @@ class SmartCruiseControlVision: self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5 def _update_state_machine(self) -> tuple[bool, bool]: - # ENABLED, ENTERING, TURNING, LEAVING + # ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING if self.state != VisionState.disabled: # longitudinal and feature disable always have priority in a non-disabled state if not self.long_enabled or not self.enabled: @@ -163,7 +163,7 @@ class SmartCruiseControlVision: return enabled, active def _update_solution(self) -> float: - # DISABLED, ENABLED + # DISABLED, ENABLED, OVERRIDING if self.state not in ACTIVE_STATES: # when not overshooting, calculate v_turn as the speed at the prediction horizon when following # the smooth deceleration. diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py new file mode 100644 index 000000000..af14a87f0 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.py @@ -0,0 +1,8 @@ +""" +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. +""" +LIMIT_ADAPT_ACC = -1. # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits. +LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers. diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/common.py b/sunnypilot/selfdrive/controls/lib/speed_limit/common.py new file mode 100644 index 000000000..7d219ac3b --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/common.py @@ -0,0 +1,21 @@ +""" +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 enum import IntEnum + + +class Policy(IntEnum): + car_state_only = 0 + map_data_only = 1 + car_state_priority = 2 + map_data_priority = 3 + combined = 4 + + +class OffsetType(IntEnum): + off = 0 + fixed = 1 + percentage = 2 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py new file mode 100644 index 000000000..2a74a01d1 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_resolver.py @@ -0,0 +1,152 @@ +""" +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. +""" +import time + +import cereal.messaging as messaging +from cereal import custom +from openpilot.common.constants import CV +from openpilot.common.gps import get_gps_location_service +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy, OffsetType + +SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source + +ALL_SOURCES = tuple(SpeedLimitSource.schema.enumerants.values()) + + +class SpeedLimitResolver: + limit_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float] + distance_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float] + v_ego: float + speed_limit: float + distance: float + source: custom.LongitudinalPlanSP.SpeedLimit.Source + speed_limit_offset: float + + def __init__(self): + self.params = Params() + self.frame = -1 + + self._gps_location_service = get_gps_location_service(self.params) + self.limit_solutions = {} # Store for speed limit solutions from different sources + self.distance_solutions = {} # Store for distance to current speed limit start for different sources + + self.policy = self.params.get("SpeedLimitPolicy", return_default=True) + self._policy_to_sources_map = { + Policy.car_state_only: [SpeedLimitSource.car], + Policy.map_data_only: [SpeedLimitSource.map], + Policy.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map], + Policy.map_data_priority: [SpeedLimitSource.map, SpeedLimitSource.car], + Policy.combined: [SpeedLimitSource.car, SpeedLimitSource.map], + } + self.source = SpeedLimitSource.none + for source in ALL_SOURCES: + self._reset_limit_sources(source) + + self.is_metric = self.params.get_bool("IsMetric") + self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True) + self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True) + + def update_params(self): + if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: + self.policy = self.params.get("SpeedLimitPolicy", return_default=True) + self.is_metric = self.params.get_bool("IsMetric") + self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True) + self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True) + + def _get_speed_limit_offset(self) -> float: + if self.offset_type == OffsetType.off: + return 0 + elif self.offset_type == OffsetType.fixed: + return float(self.offset_value * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS)) + elif self.offset_type == OffsetType.percentage: + return float(self.offset_value * 0.01 * self.speed_limit) + else: + raise NotImplementedError("Offset not supported") + + def _reset_limit_sources(self, source: custom.LongitudinalPlanSP.SpeedLimit.Source) -> None: + self.limit_solutions[source] = 0. + self.distance_solutions[source] = 0. + + def _get_from_car_state(self, sm: messaging.SubMaster) -> None: + self._reset_limit_sources(SpeedLimitSource.car) + self.limit_solutions[SpeedLimitSource.car] = sm['carStateSP'].speedLimit + self.distance_solutions[SpeedLimitSource.car] = 0. + + def _get_from_map_data(self, sm: messaging.SubMaster) -> None: + self._reset_limit_sources(SpeedLimitSource.map) + self._process_map_data(sm) + + def _process_map_data(self, sm: messaging.SubMaster) -> None: + gps_data = sm[self._gps_location_service] + map_data = sm['liveMapDataSP'] + + gps_fix_age = time.monotonic() - gps_data.unixTimestampMillis * 1e-3 + if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE: + return + + speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0. + next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0. + + self._calculate_map_data_limits(sm, speed_limit, next_speed_limit) + + def _calculate_map_data_limits(self, sm: messaging.SubMaster, speed_limit: float, next_speed_limit: float) -> None: + gps_data = sm[self._gps_location_service] + map_data = sm['liveMapDataSP'] + + distance_since_fix = self.v_ego * (time.monotonic() - gps_data.unixTimestampMillis * 1e-3) + distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix) + + self.limit_solutions[SpeedLimitSource.map] = speed_limit + self.distance_solutions[SpeedLimitSource.map] = 0. + + if 0. < next_speed_limit < self.v_ego: + adapt_time = (next_speed_limit - self.v_ego) / LIMIT_ADAPT_ACC + adapt_distance = self.v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2 + + if distance_to_speed_limit_ahead <= adapt_distance: + self.limit_solutions[SpeedLimitSource.map] = next_speed_limit + self.distance_solutions[SpeedLimitSource.map] = distance_to_speed_limit_ahead + + def _get_source_solution_according_to_policy(self) -> custom.LongitudinalPlanSP.SpeedLimit.Source: + sources_for_policy = self._policy_to_sources_map[self.policy] + + if self.policy != Policy.combined: + # They are ordered in the order of preference, so we pick the first that's non-zero + for source in sources_for_policy: + if self.limit_solutions[source] > 0.: + return source + return SpeedLimitSource.none + + sources_with_limits = [(s, limit) for s, limit in [(s, self.limit_solutions[s]) for s in sources_for_policy] if limit > 0.] + if sources_with_limits: + return min(sources_with_limits, key=lambda x: x[1])[0] + + return SpeedLimitSource.none + + def _resolve_limit_sources(self, sm: messaging.SubMaster) -> tuple[float, float, custom.LongitudinalPlanSP.SpeedLimit.Source]: + """Get limit solutions from each data source""" + self._get_from_car_state(sm) + self._get_from_map_data(sm) + + source = self._get_source_solution_according_to_policy() + speed_limit = self.limit_solutions[source] if source else 0. + distance = self.distance_solutions[source] if source else 0. + + return speed_limit, distance, source + + def update(self, v_ego: float, sm: messaging.SubMaster) -> None: + self.v_ego = v_ego + self.update_params() + + self.speed_limit, self.distance, self.source = self._resolve_limit_sources(sm) + self.speed_limit_offset = self._get_speed_limit_offset() + + self.frame += 1 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_resolver.py new file mode 100644 index 000000000..c02831d71 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_resolver.py @@ -0,0 +1,144 @@ +""" +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. +""" +import random +import time + +import pytest +from pytest_mock import MockerFixture + +from cereal import custom +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE + +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy + +SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source + + +def create_mock(properties, mocker: MockerFixture): + mock = mocker.MagicMock() + for _property, value in properties.items(): + setattr(mock, _property, value) + return mock + + +def setup_sm_mock(mocker: MockerFixture): + cruise_speed_limit = random.uniform(0, 120) + live_map_data_limit = random.uniform(0, 120) + + car_state = create_mock({ + 'gasPressed': False, + 'brakePressed': False, + 'standstill': False, + }, mocker) + car_state_sp = create_mock({ + 'speedLimit': cruise_speed_limit, + }, mocker) + live_map_data = create_mock({ + 'speedLimit': live_map_data_limit, + 'speedLimitValid': True, + 'speedLimitAhead': 0., + 'speedLimitAheadValid': 0., + 'speedLimitAheadDistance': 0., + }, mocker) + gps_data = create_mock({ + 'unixTimestampMillis': time.monotonic() * 1e3, + }, mocker) + sm_mock = mocker.MagicMock() + sm_mock.__getitem__.side_effect = lambda key: { + 'carState': car_state, + 'liveMapDataSP': live_map_data, + 'carStateSP': car_state_sp, + 'gpsLocation': gps_data, + }[key] + return sm_mock + + +parametrized_policies = pytest.mark.parametrize( + "policy, sm_key, function_key", [ + (Policy.car_state_only, 'carStateSP', SpeedLimitSource.car), + (Policy.car_state_priority, 'carStateSP', SpeedLimitSource.car), + (Policy.map_data_only, 'liveMapDataSP', SpeedLimitSource.map), + (Policy.map_data_priority, 'liveMapDataSP', SpeedLimitSource.map), + ], + ids=lambda val: val.name if hasattr(val, 'name') else str(val) +) + + +@pytest.mark.parametrize("resolver_class", [SpeedLimitResolver]) +class TestSpeedLimitResolverValidation: + + @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name) + def test_initial_state(self, resolver_class, policy): + resolver = resolver_class() + resolver.policy = policy + for source in ALL_SOURCES: + if source in resolver.limit_solutions: + assert resolver.limit_solutions[source] == 0. + assert resolver.distance_solutions[source] == 0. + + @parametrized_policies + def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture): + resolver = resolver_class() + resolver.policy = policy + sm_mock = setup_sm_mock(mocker) + source_speed_limit = sm_mock[sm_key].speedLimit + + # Assert the resolver + resolver.update(source_speed_limit, sm_mock) + assert resolver.speed_limit == source_speed_limit + assert resolver.source == ALL_SOURCES[function_key] + + def test_resolver_combined(self, resolver_class, mocker: MockerFixture): + resolver = resolver_class() + resolver.policy = Policy.combined + sm_mock = setup_sm_mock(mocker) + socket_to_source = {'carStateSP': SpeedLimitSource.car, 'liveMapDataSP': SpeedLimitSource.map} + minimum_key, minimum_speed_limit = min( + ((key, sm_mock[key].speedLimit) for key in + socket_to_source.keys()), key=lambda x: x[1]) + + # Assert the resolver + resolver.update(minimum_speed_limit, sm_mock) + assert resolver.speed_limit == minimum_speed_limit + assert resolver.source == socket_to_source[minimum_key] + + @parametrized_policies + def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture): + resolver = resolver_class() + resolver.policy = policy + sm_mock = setup_sm_mock(mocker) + source_speed_limit = sm_mock[sm_key].speedLimit + + # Assert the parsing + resolver.update(source_speed_limit, sm_mock) + assert resolver.limit_solutions[ALL_SOURCES[function_key]] == source_speed_limit + assert resolver.distance_solutions[ALL_SOURCES[function_key]] == 0. + + @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name) + def test_resolve_interaction_in_update(self, resolver_class, policy, mocker: MockerFixture): + v_ego = 50 + resolver = resolver_class() + resolver.policy = policy + + sm_mock = setup_sm_mock(mocker) + resolver.update(v_ego, sm_mock) + + # After resolution + assert resolver.speed_limit is not None + assert resolver.distance is not None + assert resolver.source is not None + + @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name) + def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture): + resolver = resolver_class() + resolver.policy = policy + sm_mock = mocker.MagicMock() + sm_mock['gpsLocation'].unixTimestampMillis = (time.monotonic() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3 + resolver._get_from_map_data(sm_mock) + assert resolver.limit_solutions[SpeedLimitSource.map] == 0. + assert resolver.distance_solutions[SpeedLimitSource.map] == 0.