Merge branch 'feature/slc' into ui/slc-ui

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,11 +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',
-                            "liveMapDataSP", "carStateSP"],
+                            'liveMapDataSP', 'carStateSP', gps_location_service],
                            poll='modelV2')
 
   while True:

selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc

@@ -24,17 +24,7 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) {
   currentModelLblBtn = new ButtonControlSP(tr("Current Model"), tr("SELECT"), current_model);
   currentModelLblBtn->setValue(current_model);
 
-  connect(currentModelLblBtn, &ButtonControlSP::clicked, [=]() {
-
-      InputDialog d(tr("Search Model"), this, tr("Enter search keywords, or leave blank to list all models."), false);
-      d.setMinLength(0);
-      const int ret = d.exec();
-      if (ret) {
-        handleCurrentModelLblBtnClicked(d.text());
-      }
-
-    });
-
+  connect(currentModelLblBtn, &ButtonControlSP::clicked, this, &ModelsPanel::handleCurrentModelLblBtnClicked);
   connect(uiState(), &UIState::offroadTransition, [=](bool offroad) {
       is_onroad = !offroad;
       updateLabels();
@@ -144,7 +134,7 @@ void ModelsPanel::updateModelManagerState() {
  * @brief Handles the model bundle selection button click
  * Displays available bundles, allows selection, and initiates download
  */
-void ModelsPanel::handleCurrentModelLblBtnClicked(const QString &query) {
+void ModelsPanel::handleCurrentModelLblBtnClicked() {
   currentModelLblBtn->setEnabled(false);
   currentModelLblBtn->setValue(tr("Fetching models..."));
 
@@ -152,8 +142,7 @@ void ModelsPanel::handleCurrentModelLblBtnClicked(const QString &query) {
   QMap<uint32_t, QString> index_to_bundle;
   const auto bundles = model_manager.getAvailableBundles();
   for (const auto &bundle: bundles) {
-    std::string bundleStr = std::string(bundle.getDisplayName()) + " $SNAME$ " + std::string(bundle.getInternalName());
-    index_to_bundle.insert(bundle.getIndex(), QString::fromStdString(bundleStr));
+    index_to_bundle.insert(bundle.getIndex(), QString::fromStdString(bundle.getDisplayName()));
   }
 
   // Sort bundles by index in descending order
@@ -167,24 +156,10 @@ void ModelsPanel::handleCurrentModelLblBtnClicked(const QString &query) {
     bundleNames.append(index_to_bundle[index]);
   }
 
-  QStringList filteredBundleNames = searchFromList(query, bundleNames);
-
   currentModelLblBtn->setValue(GetActiveModelName());
 
-  if (filteredBundleNames.isEmpty()) {
-    ConfirmationDialog::alert(tr("No model found for keywords: %1").arg(query), this);
-    return;
-  }
-
-  for (const QString &bundleName: filteredBundleNames) {
-    int index = bundleName.indexOf("$SNAME$");
-    if (index != -1) {
-      filteredBundleNames.replace(filteredBundleNames.indexOf(bundleName), bundleName.left(index).trimmed());
-    }
-  }
-
   const QString selectedBundleName = MultiOptionDialog::getSelection(
-    tr("Select a Model"), filteredBundleNames, GetActiveModelName(), this);
+    tr("Select a Model"), bundleNames, GetActiveModelName(), this);
 
   if (selectedBundleName.isEmpty() || !canContinueOnMeteredDialog()) {
     return;

selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h

@@ -7,7 +7,6 @@
 
 #pragma once
 
-#include "selfdrive/ui/sunnypilot/qt/util.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 
 class ModelsPanel : public QWidget {
@@ -31,7 +30,7 @@ private:
 
   // UI update related methods
   void updateLabels();
-  void handleCurrentModelLblBtnClicked(const QString &query);
+  void handleCurrentModelLblBtnClicked();
   void handleBundleDownloadProgress();
   void showResetParamsDialog();
   cereal::ModelManagerSP::Reader model_manager;

sunnypilot/mapd/live_map_data/base_map_data.py

@@ -16,11 +16,6 @@ from openpilot.sunnypilot.navd.helpers import Coordinate, coordinate_from_param
 class BaseMapData(ABC):
   def __init__(self):
     self.params = Params()
-    self._last_gps: Coordinate | None = None
-    self._gps_location_service = get_gps_location_service(self.params)
-    self._gps_packets = [self._gps_location_service]
-    self._sm = messaging.SubMaster(['livePose', 'carControl'] + self._gps_packets)
-    self._pm = messaging.PubMaster(['liveMapDataSP'])
 
     self.gps_location_service = get_gps_location_service(self.params)
     self.sm = messaging.SubMaster(['livePose', 'carControl'] + [self.gps_location_service])
@@ -65,15 +60,6 @@ class BaseMapData(ABC):
     mapd_sp_send.valid = self.sm.all_checks(service_list=[self.gps_location_service, 'livePose'])
     live_map_data = mapd_sp_send.liveMapDataSP
 
-    if self._last_gps:
-      live_map_data.lastGpsTimestamp = self._last_gps.annotations.get('unixTimestampMillis', 0)
-      live_map_data.lastGpsLatitude = self._last_gps.latitude
-      live_map_data.lastGpsLongitude = self._last_gps.longitude
-      live_map_data.lastGpsSpeed = self._last_gps.annotations.get('speed', 0)
-      live_map_data.lastGpsBearingDeg = self._last_gps.annotations.get('bearingDeg', 0)
-      live_map_data.lastGpsAccuracy = self._last_gps.annotations.get('accuracy', 0)
-      live_map_data.lastGpsBearingAccuracyDeg = self._last_gps.annotations.get('bearingAccuracyDeg', 0)
-
     live_map_data.speedLimitValid = bool(speed_limit > 0)
     live_map_data.speedLimit = speed_limit
     live_map_data.speedLimitAheadValid = bool(next_speed_limit > 0)

sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py

@@ -2,7 +2,8 @@ import time
 import numpy as np
 
 from cereal import messaging, custom
-
+from openpilot.common.gps import get_gps_location_service
+from openpilot.common.params import Params
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.helpers import debug
@@ -15,6 +16,7 @@ class SpeedLimitResolver:
   _current_speed_limit: float
 
   def __init__(self, policy: Policy):
+    self._gps_location_service = get_gps_location_service(Params())
     self._limit_solutions = {}
     self._distance_solutions = {}
 
@@ -58,10 +60,13 @@ class SpeedLimitResolver:
     # Load limits from map_data
     if sm.updated['liveMapDataSP']:
       self._reset_limit_sources(Source.map_data)
-      self._process_map_data(sm['liveMapDataSP'])
+      self._process_map_data(sm)
 
-  def _process_map_data(self, map_data: custom.LiveMapDataSP) -> None:
-    gps_fix_age = time.time() - map_data.lastGpsTimestamp * 1e-3
+  def _process_map_data(self, sm: messaging.SubMaster) -> None:
+    gps_data = sm[self._gps_location_service]
+    map_data = sm['liveMapDataSP']
+
+    gps_fix_age = time.time() - gps_data.unixTimestampMillis * 1e-3
     if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
       debug(f'SL: Ignoring map data as is too old. Age: {gps_fix_age}')
       return
@@ -69,10 +74,13 @@ class SpeedLimitResolver:
     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(speed_limit, next_speed_limit, map_data)
+    self._calculate_map_data_limits(sm, speed_limit, next_speed_limit)
 
-  def _calculate_map_data_limits(self, speed_limit: float, next_speed_limit: float, map_data: custom.LiveMapDataSP) -> None:
-    distance_since_fix = self._v_ego * (time.time() - map_data.lastGpsTimestamp * 1e-3)
+  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.time() - gps_data.unixTimestampMillis * 1e-3)
     distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix)
 
     self._limit_solutions[Source.map_data] = speed_limit

sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py

@@ -36,13 +36,16 @@ def setup_sm_mock(mocker: MockerFixture):
     'speedLimitAhead': 0.,
     'speedLimitAheadValid': 0.,
     'speedLimitAheadDistance': 0.,
-    'lastGpsTimestamp': time.time() * 1e3,
+  }, mocker)
+  gps_data = create_mock({
+    'unixTimestampMillis': time.time() * 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
 
@@ -121,8 +124,7 @@ class TestSpeedLimitResolverValidation:
   def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture):
     resolver = resolver_class(policy)
     sm_mock = mocker.MagicMock()
-    sm_mock['liveMapDataSP'].lastGpsTimestamp = (time.time() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3
-    resolver._sm = sm_mock
-    resolver._get_from_map_data()
+    sm_mock['gpsLocation'].unixTimestampMillis = (time.time() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3
+    resolver._get_from_map_data(sm_mock)
     assert resolver._limit_solutions[Source.map_data] == 0.
     assert resolver._distance_solutions[Source.map_data] == 0.