diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 0791841a5..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; @@ -231,7 +249,7 @@ struct OnroadEventSP @0xda96579883444c35 { struct CarParamsSP @0x80ae746ee2596b11 { flags @0 :UInt32; # flags for car specific quirks in sunnypilot safetyParam @1 : Int16; # flags for sunnypilot's custom safety flags - pcmCruiseSpeed @3 :Bool = true; + pcmCruiseSpeed @3 :Bool; intelligentCruiseButtonManagementAvailable @4 :Bool; neuralNetworkLateralControl @2 :NeuralNetworkLateralControl; @@ -261,7 +279,7 @@ struct CarControlSP @0xa5cd762cd951a455 { valueDEPRECATED @1 :Text; # The data type change may cause issues with backwards compatibility. } - + enum ParamType { string @0; bool @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 8f9d7bece..56d865be7 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -235,4 +235,10 @@ inline static std::unordered_map keys = { {"OsmStateTitle", {PERSISTENT, STRING}}, {"OsmWayTest", {PERSISTENT, STRING}}, {"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}}, + + // Speed Limit + {"SpeedLimitMode", {PERSISTENT | BACKUP, INT, "1"}}, + {"SpeedLimitOffsetType", {PERSISTENT | BACKUP, INT, "0"}}, + {"SpeedLimitPolicy", {PERSISTENT | BACKUP, INT, "3"}}, + {"SpeedLimitValueOffset", {PERSISTENT | BACKUP, INT, "0"}}, }; diff --git a/common/util.h b/common/util.h index f46db4d9f..6d28e3ccb 100644 --- a/common/util.h +++ b/common/util.h @@ -36,6 +36,7 @@ const double MS_TO_KPH = 3.6; const double MS_TO_MPH = MS_TO_KPH * KM_TO_MILE; const double METER_TO_MILE = KM_TO_MILE / 1000.0; const double METER_TO_FOOT = 3.28084; +const double METER_TO_KM = 1. / 1000.0; #define ALIGNED_SIZE(x, align) (((x) + (align)-1) & ~((align)-1)) diff --git a/opendbc_repo b/opendbc_repo index a92c0d5f2..1e1700944 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit a92c0d5f26e0ef9948cb7e70f2a4c2bbb425536c +Subproject commit 1e170094440cfefcd1fcf7390b95f6fea1b2425e diff --git a/selfdrive/car/tests/test_cruise_speed.py b/selfdrive/car/tests/test_cruise_speed.py index 8512651de..61bae6893 100644 --- a/selfdrive/car/tests/test_cruise_speed.py +++ b/selfdrive/car/tests/test_cruise_speed.py @@ -44,12 +44,12 @@ class TestCruiseSpeed: assert simulation_steady_state == pytest.approx(cruise_speed, abs=.01), f'Did not reach {self.speed} m/s' -# TODO: test pcmCruise -@parameterized_class(('pcm_cruise',), [(False,)]) +# TODO: test pcmCruise and pcmCruiseSpeed +@parameterized_class(('pcm_cruise', 'pcm_cruise_speed'), [(False, True)]) class TestVCruiseHelper: def setup_method(self): self.CP = car.CarParams(pcmCruise=self.pcm_cruise) - self.CP_SP = custom.CarParamsSP() + self.CP_SP = custom.CarParamsSP(pcmCruiseSpeed=self.pcm_cruise_speed) self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_SP) self.reset_cruise_speed_state() 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..22cdd8c63 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h @@ -0,0 +1,47 @@ +/* + * 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") +}; + +enum class SpeedLimitMode { + OFF, + INFORMATION, + WARNING, +}; + +inline const QString SpeedLimitModeTexts[]{ + QObject::tr("Off"), + QObject::tr("Information"), + QObject::tr("Warning"), +}; 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..6f4883361 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc @@ -0,0 +1,130 @@ +/* + * 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, false); + + speedLimitPolicyScreen = new SpeedLimitPolicy(this); + + std::vector speed_limit_mode_texts{ + SpeedLimitModeTexts[static_cast(SpeedLimitMode::OFF)], + SpeedLimitModeTexts[static_cast(SpeedLimitMode::INFORMATION)], + SpeedLimitModeTexts[static_cast(SpeedLimitMode::WARNING)], + }; + speed_limit_mode_settings = new ButtonParamControlSP( + "SpeedLimitMode", + tr("Speed Limit Mode"), + "", + "", + speed_limit_mode_texts, + 385); + list->addItem(speed_limit_mode_settings); + + list->addItem(horizontal_line()); + list->addItem(vertical_space()); + + 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); + list->addItem(speedLimitSource); + + list->addItem(vertical_space(0)); + list->addItem(horizontal_line()); + + 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_mode_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitSettings::refresh); + 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"); + SpeedLimitMode speed_limit_mode_param = static_cast(std::atoi(params.get("SpeedLimitMode").c_str())); + SpeedLimitOffsetType offset_type_param = static_cast(std::atoi(params.get("SpeedLimitOffsetType").c_str())); + QString offsetLabel = QString::fromStdString(params.get("SpeedLimitValueOffset")); + + speed_limit_mode_settings->setDescription(modeDescription(speed_limit_mode_param)); + 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(); + } + + speed_limit_mode_settings->showDescription(); + speed_limit_offset->showDescription(); +} + +void SpeedLimitSettings::showEvent(QShowEvent *event) { + refresh(); +} 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..f83f45055 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h @@ -0,0 +1,73 @@ +/* + * 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; + ButtonParamControlSP *speed_limit_mode_settings; + 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); + } + + static QString modeDescription(SpeedLimitMode mode = SpeedLimitMode::OFF) { + QString off_str = tr("⦿ Off: Disables the Speed Limit functions."); + QString info_str = tr("⦿ Information: Displays the current road's speed limit."); + QString warning_str = tr("⦿ Warning: Provides a warning when exceeding the current road's speed limit."); + + if (mode == SpeedLimitMode::WARNING) { + warning_str = "" + warning_str + ""; + } else if (mode == SpeedLimitMode::INFORMATION) { + info_str = "" + info_str + ""; + } else { + off_str = "" + off_str + ""; + } + + return QString("%1
%2
%3") + .arg(off_str) + .arg(info_str) + .arg(warning_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/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc index e19760f2e..ca58282a3 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc @@ -25,21 +25,28 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) { "BlindSpot", tr("Show Blind Spot Warnings"), tr("Enabling this will display warnings when a vehicle is detected in your blind spot as long as your car has BSM supported."), - "../assets/offroad/icon_monitoring.png", + "", false, }, { "RainbowMode", tr("Enable Tesla Rainbow Mode"), RainbowizeWords(tr("A beautiful rainbow effect on the path the model wants to take.")) + "
" + tr("It")+ " " + tr("does not") + " " + tr("affect driving in any way.") + "", - "../assets/offroad/icon_monitoring.png", + "", false, }, { "StandstillTimer", tr("Enable Standstill Timer"), tr("Show a timer on the HUD when the car is at a standstill."), - "../assets/offroad/icon_monitoring.png", + "", + false, + }, + { + "RoadName", + tr("Display Road Name"), + tr("Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name."), + "", false, }, }; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index fb2a69c24..991710bbf 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc @@ -17,7 +17,6 @@ void HudRendererSP::updateState(const UIState &s) { HudRenderer::updateState(s); const SubMaster &sm = *(s.sm); - const bool cs_alive = sm.alive("controlsState"); const auto cs = sm["controlsState"].getControlsState(); const auto car_state = sm["carState"].getCarState(); const auto car_control = sm["carControl"].getCarControl(); @@ -27,6 +26,25 @@ void HudRendererSP::updateState(const UIState &s) { const auto ltp = sm["liveTorqueParameters"].getLiveTorqueParameters(); const auto car_params = sm["carParams"].getCarParams(); const auto lp_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP(); + const auto lmd = sm["liveMapDataSP"].getLiveMapDataSP(); + + float speedConv = is_metric ? MS_TO_KPH : MS_TO_MPH; + speedLimit = lp_sp.getSpeedLimit().getResolver().getSpeedLimit() * speedConv; + speedLimitOffset = lp_sp.getSpeedLimit().getResolver().getSpeedLimitOffset() * speedConv; + speedLimitMode = static_cast(s.scene.speed_limit_mode); + roadName = s.scene.road_name; + if (sm.updated("liveMapDataSP")) { + roadNameStr = QString::fromStdString(lmd.getRoadName()); + speedLimitAheadValid = lmd.getSpeedLimitAheadValid(); + speedLimitAhead = lmd.getSpeedLimitAhead() * speedConv; + speedLimitAheadDistance = lmd.getSpeedLimitAheadDistance(); + if (speedLimitAheadDistance < speedLimitAheadDistancePrev && speedLimitAheadValidFrame < 2) { + speedLimitAheadValidFrame++; + } else if (speedLimitAheadDistance > speedLimitAheadDistancePrev && speedLimitAheadValidFrame > 0) { + speedLimitAheadValidFrame--; + } + } + speedLimitAheadDistancePrev = speedLimitAheadDistance; static int reverse_delay = 0; bool reverse_allowed = false; @@ -41,13 +59,6 @@ void HudRendererSP::updateState(const UIState &s) { } reversing = reverse_allowed; - is_metric = s.scene.is_metric; - - // Handle older routes where vEgoCluster is not set - v_ego_cluster_seen = v_ego_cluster_seen || car_state.getVEgoCluster() != 0.0; - float v_ego = v_ego_cluster_seen ? car_state.getVEgoCluster() : car_state.getVEgo(); - speed = cs_alive ? std::max(0.0, v_ego) : 0.0; - speed *= is_metric ? MS_TO_KPH : MS_TO_MPH; latActive = car_control.getLatActive(); steerOverride = car_state.getSteeringPressed(); @@ -123,6 +134,15 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { if (standstillTimer) { drawStandstillTimer(p, surface_rect.right() / 12 * 10, surface_rect.bottom() / 12 * 1.53); } + + // Speed Limit + if (speedLimitMode != SpeedLimitMode::OFF) { + drawSpeedLimitSigns(p); + drawUpcomingSpeedLimit(p); + } + + // Road Name + drawRoadName(p, surface_rect); } } @@ -311,3 +331,223 @@ void HudRendererSP::drawStandstillTimer(QPainter &p, int x, int y) { standstillElapsedTime = 0.0; } } + +void HudRendererSP::drawSpeedLimitSigns(QPainter &p) { + int speedLimitRounded = std::nearbyint(speedLimit); + bool overspeed = speedLimitRounded < std::nearbyint(speed) && speedLimitRounded > 0; + bool speedLimitWarningEnabled = speedLimitMode == SpeedLimitMode::WARNING; + QString speedLimitStr = speedLimit > 0 ? QString::number(speedLimitRounded) : "---"; + + // Offset display text + QString speedLimitSubText = ""; + if (speedLimitOffset != 0) { + speedLimitSubText = (speedLimitOffset > 0 ? "+" : "") + QString::number(std::nearbyint(speedLimitOffset)); + } + + // Position next to MAX speed box + const int sign_width = is_metric ? 200 : 172; + const int sign_x = is_metric ? 280 : 272; + const int sign_y = 45; + const int sign_height = 204; + QRect sign_rect(sign_x, sign_y, sign_width, sign_height); + + int alpha = 255; + QColor red_color = QColor(255, 0, 0, alpha); + QColor speed_color = (speedLimitWarningEnabled && overspeed) ? red_color : QColor(0, 0, 0, alpha); + + if (is_metric) { + // EU Vienna Convention style circular sign + QRect vienna_rect = sign_rect; + int circle_size = std::min(vienna_rect.width(), vienna_rect.height()); + QRect circle_rect(vienna_rect.x(), vienna_rect.y(), circle_size, circle_size); + + if (vienna_rect.width() > vienna_rect.height()) { + circle_rect.moveLeft(vienna_rect.x() + (vienna_rect.width() - circle_size) / 2); + } else if (vienna_rect.height() > vienna_rect.width()) { + circle_rect.moveTop(vienna_rect.y() + (vienna_rect.height() - circle_size) / 2); + } + + // White background circle + p.setPen(Qt::NoPen); + p.setBrush(QColor(255, 255, 255, alpha)); + p.drawEllipse(circle_rect); + + // Red border ring with color coding + QRect red_ring = circle_rect; + + p.setBrush(red_color); + p.drawEllipse(red_ring); + + // Center white circle for text + int ring_size = circle_size * 0.12; + QRect center_circle = red_ring.adjusted(ring_size, ring_size, -ring_size, -ring_size); + p.setBrush(QColor(255, 255, 255, alpha)); + p.drawEllipse(center_circle); + + // Speed value, smaller font for 3+ digits + int font_size = (speedLimitStr.size() >= 3) ? 70 : 85; + p.setFont(InterFont(font_size, QFont::Bold)); + + p.setPen(speed_color); + p.drawText(center_circle, Qt::AlignCenter, speedLimitStr); + + // Offset value in small circular box + if (!speedLimitSubText.isEmpty()) { + int offset_circle_size = circle_size * 0.4; + int overlap = offset_circle_size * 0.25; + QRect offset_circle_rect( + circle_rect.right() - offset_circle_size/1.25 + overlap, + circle_rect.top() - offset_circle_size/1.75 + overlap, + offset_circle_size, + offset_circle_size + ); + + p.setPen(QPen(QColor(77, 77, 77, 255), 6)); + p.setBrush(QColor(0, 0, 0, alpha)); + p.drawEllipse(offset_circle_rect); + + p.setFont(InterFont(offset_circle_size * 0.45, QFont::Bold)); + p.setPen(QColor(255, 255, 255, alpha)); + p.drawText(offset_circle_rect, Qt::AlignCenter, speedLimitSubText); + } + } else { + // US/Canada MUTCD style sign + p.setPen(Qt::NoPen); + p.setBrush(QColor(255, 255, 255, alpha)); + p.drawRoundedRect(sign_rect, 32, 32); + + // Inner border with violation color coding + QRect inner_rect = sign_rect.adjusted(10, 10, -10, -10); + QColor border_color = QColor(0, 0, 0, alpha); + + p.setPen(QPen(border_color, 4)); + p.setBrush(QColor(255, 255, 255, alpha)); + p.drawRoundedRect(inner_rect, 22, 22); + + // "SPEED LIMIT" text + p.setFont(InterFont(40, QFont::DemiBold)); + p.setPen(QColor(0, 0, 0, alpha)); + p.drawText(inner_rect.adjusted(0, 10, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED")); + p.drawText(inner_rect.adjusted(0, 50, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT")); + + // Speed value with color coding + p.setFont(InterFont(90, QFont::Bold)); + + p.setPen(speed_color); + p.drawText(inner_rect.adjusted(0, 80, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr); + + // Offset value in small box + if (!speedLimitSubText.isEmpty()) { + int offset_box_size = sign_rect.width() * 0.4; + int overlap = offset_box_size * 0.25; + QRect offset_box_rect( + sign_rect.right() - offset_box_size/1.5 + overlap, + sign_rect.top() - offset_box_size/1.25 + overlap, + offset_box_size, + offset_box_size + ); + + int corner_radius = offset_box_size * 0.2; + p.setPen(QPen(QColor(77, 77, 77, 255), 6)); + p.setBrush(QColor(0, 0, 0, alpha)); + p.drawRoundedRect(offset_box_rect, corner_radius, corner_radius); + + p.setFont(InterFont(offset_box_size * 0.45, QFont::Bold)); + p.setPen(QColor(255, 255, 255, alpha)); + p.drawText(offset_box_rect, Qt::AlignCenter, speedLimitSubText); + } + } +} + +void HudRendererSP::drawUpcomingSpeedLimit(QPainter &p) { + bool speed_limit_ahead = speedLimitAheadValid && speedLimitAhead > 0 && speedLimitAhead != speedLimit && speedLimitAheadValidFrame > 0; + if (!speed_limit_ahead) { + return; + } + + auto roundToInterval = [&](float distance, int interval, int threshold) { + int base = static_cast(distance / interval) * interval; + return (distance - base >= threshold) ? base + interval : base; + }; + + auto outputDistance = [&] { + if (is_metric) { + if (speedLimitAheadDistance < 50) return tr("Near"); + if (speedLimitAheadDistance >= 1000) return QString::number(speedLimitAheadDistance * METER_TO_KM, 'f', 1) + tr("km"); + + int rounded = (speedLimitAheadDistance < 200) ? std::max(10, roundToInterval(speedLimitAheadDistance, 10, 5)) : roundToInterval(speedLimitAheadDistance, 100, 50); + return QString::number(rounded) + tr("m"); + } else { + float distance_ft = speedLimitAheadDistance * METER_TO_FOOT; + if (distance_ft < 100) return tr("Near"); + if (distance_ft >= 900) return QString::number(speedLimitAheadDistance * METER_TO_MILE, 'f', 1) + tr("mi"); + + int rounded = (distance_ft < 500) ? std::max(50, roundToInterval(distance_ft, 50, 25)) : roundToInterval(distance_ft, 100, 50); + return QString::number(rounded) + tr("ft"); + } + }; + + QString speedStr = QString::number(std::nearbyint(speedLimitAhead)); + QString distanceStr = outputDistance(); + + // Position below current speed limit sign + const int sign_width = is_metric ? 200 : 172; + const int sign_x = is_metric ? 280 : 272; + const int sign_y = 45; + const int sign_height = 204; + + const int ahead_width = 170; + const int ahead_height = 160; + const int ahead_x = sign_x + (sign_width - ahead_width) / 2; + const int ahead_y = sign_y + sign_height + 10; + + QRect ahead_rect(ahead_x, ahead_y, ahead_width, ahead_height); + p.setPen(QPen(QColor(255, 255, 255, 100), 3)); + p.setBrush(QColor(0, 0, 0, 180)); + p.drawRoundedRect(ahead_rect, 16, 16); + + // "AHEAD" label + p.setFont(InterFont(40, QFont::DemiBold)); + p.setPen(QColor(200, 200, 200, 255)); + p.drawText(ahead_rect.adjusted(0, 4, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("AHEAD")); + + // Speed value + p.setFont(InterFont(70, QFont::Bold)); + p.setPen(QColor(255, 255, 255, 255)); + p.drawText(ahead_rect.adjusted(0, 38, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedStr); + + // Distance + p.setFont(InterFont(40, QFont::Normal)); + p.setPen(QColor(180, 180, 180, 255)); + p.drawText(ahead_rect.adjusted(0, 110, 0, 0), Qt::AlignTop | Qt::AlignHCenter, distanceStr); +} + +void HudRendererSP::drawRoadName(QPainter &p, const QRect &surface_rect) { + if (!roadName || roadNameStr.isEmpty()) return; + + // Measure text to size container + p.setFont(InterFont(40, QFont::Normal)); + QFontMetrics fm(p.font()); + + int text_width = fm.horizontalAdvance(roadNameStr); + int padding = 40; + int rect_width = text_width + padding; + + // Constrain to reasonable bounds + int min_width = 200; + int max_width = surface_rect.width() - 40; + rect_width = std::max(min_width, std::min(rect_width, max_width)); + + // Center at top of screen + QRect road_rect(surface_rect.width() / 2 - rect_width / 2, -6, rect_width, 60); + + p.setPen(Qt::NoPen); + p.setBrush(QColor(0, 0, 0, 120)); + p.drawRoundedRect(road_rect, 12, 12); + + p.setPen(QColor(255, 255, 255, 200)); + + // Truncate if still too long + QString truncated = fm.elidedText(roadNameStr, Qt::ElideRight, road_rect.width() - 20); + p.drawText(road_rect, Qt::AlignCenter, truncated); +} diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h index 4c9283595..073780bc1 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.h @@ -8,6 +8,7 @@ #pragma once #include "selfdrive/ui/qt/onroad/hud.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h" #include "selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.h" class HudRendererSP : public HudRenderer { @@ -28,6 +29,9 @@ private: void drawStandstillTimer(QPainter &p, int x, int y); bool pulseElement(int frame); void drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &surface_rect, int x_offset, int y_offset, std::string name); + void drawSpeedLimitSigns(QPainter &p); + void drawUpcomingSpeedLimit(QPainter &p); + void drawRoadName(QPainter &p, const QRect &surface_rect); bool lead_status; float lead_d_rel; @@ -63,4 +67,14 @@ private: bool smartCruiseControlVisionEnabled; bool smartCruiseControlVisionActive; int smartCruiseControlVisionFrame; + float speedLimit; + float speedLimitOffset; + bool speedLimitAheadValid; + float speedLimitAhead; + float speedLimitAheadDistance; + float speedLimitAheadDistancePrev; + int speedLimitAheadValidFrame; + SpeedLimitMode speedLimitMode = SpeedLimitMode::OFF; + bool roadName; + QString roadNameStr; }; diff --git a/selfdrive/ui/sunnypilot/ui.cc b/selfdrive/ui/sunnypilot/ui.cc index 7b10929bc..9acf84408 100644 --- a/selfdrive/ui/sunnypilot/ui.cc +++ b/selfdrive/ui/sunnypilot/ui.cc @@ -20,7 +20,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) { "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan", "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP", "carControl", "gpsLocationExternal", "gpsLocation", "liveTorqueParameters", - "carStateSP", "liveParameters" + "carStateSP", "liveParameters", "liveMapDataSP" }); // update timer @@ -53,6 +53,8 @@ void ui_update_params_sp(UIStateSP *s) { auto params = Params(); s->scene.dev_ui_info = std::atoi(params.get("DevUIInfo").c_str()); s->scene.standstill_timer = params.getBool("StandstillTimer"); + s->scene.speed_limit_mode = std::atoi(params.get("SpeedLimitMode").c_str()); + s->scene.road_name = params.getBool("RoadName"); } DeviceSP::DeviceSP(QObject *parent) : Device(parent) { diff --git a/selfdrive/ui/sunnypilot/ui_scene.h b/selfdrive/ui/sunnypilot/ui_scene.h index c941be675..768cc5d7a 100644 --- a/selfdrive/ui/sunnypilot/ui_scene.h +++ b/selfdrive/ui/sunnypilot/ui_scene.h @@ -10,4 +10,6 @@ typedef struct UISceneSP : UIScene { int dev_ui_info = 0; bool standstill_timer = false; + int speed_limit_mode = 0; + bool road_name = false; } UISceneSP; 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/mapd/mapd_installer.py b/sunnypilot/mapd/mapd_installer.py index 9f7a8c2a5..e49ff8c64 100755 --- a/sunnypilot/mapd/mapd_installer.py +++ b/sunnypilot/mapd/mapd_installer.py @@ -22,7 +22,7 @@ from openpilot.system.version import is_prebuilt from openpilot.sunnypilot.mapd import MAPD_PATH, MAPD_BIN_DIR import openpilot.system.sentry as sentry -VERSION = 'v1.10.0' +VERSION = 'v1.11.0' URL = f"https://github.com/pfeiferj/openpilot-mapd/releases/download/{VERSION}/mapd" diff --git a/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py index 0b0957b82..7ee7f3ea6 100644 --- a/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py +++ b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py @@ -16,7 +16,7 @@ State = custom.IntelligentCruiseButtonManagement.IntelligentCruiseButtonManageme SendButtonState = custom.IntelligentCruiseButtonManagement.SendButtonState ALLOWED_SPEED_THRESHOLD = 1.8 # m/s, ~4 MPH -HYST_GAP = 0.75 +HYST_GAP = 0.0 # currently disabled; TODO-SP: might need to be brand-specific INACTIVE_TIMER = 0.4 diff --git a/sunnypilot/selfdrive/car/tests/test_custom_cruise.py b/sunnypilot/selfdrive/car/tests/test_custom_cruise.py index 3bed8534e..a1f7f8f02 100644 --- a/sunnypilot/selfdrive/car/tests/test_custom_cruise.py +++ b/sunnypilot/selfdrive/car/tests/test_custom_cruise.py @@ -11,7 +11,8 @@ ButtonEvent = car.CarState.ButtonEvent ButtonType = car.CarState.ButtonEvent.Type -@parameterized_class(('pcm_cruise',), [(False,)]) +# TODO: test pcmCruise and pcmCruiseSpeed +@parameterized_class(('pcm_cruise', 'pcm_cruise_speed'), [(False, True)]) class TestCustomAccIncrements(TestVCruiseHelper): def setup_method(self): TestVCruiseHelper.setup_method(self) 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. diff --git a/third_party/mapd_pfeiferj/mapd b/third_party/mapd_pfeiferj/mapd index e5e45e2b9..eb2372cee 100755 Binary files a/third_party/mapd_pfeiferj/mapd and b/third_party/mapd_pfeiferj/mapd differ