nav: various cleanup (#21521)

* nav: various cleanup

* eta resize instant

* show
old-commit-hash: 9ea913bcc8

selfdrive/common/util.h

@@ -127,6 +127,7 @@ public:
     return x_;
   }
   inline void reset(float x) { x_ = x; }
+  inline float x(){ return x_; }
 
 private:
   float x_, k_;

selfdrive/ui/qt/maps/map.cc

@@ -4,7 +4,6 @@
 
 #include <QDebug>
 
-#include "selfdrive/common/util.h"
 #include "selfdrive/common/swaglog.h"
 #include "selfdrive/ui/ui.h"
 #include "selfdrive/ui/qt/util.h"
@@ -13,7 +12,6 @@
 
 
 const int PAN_TIMEOUT = 100;
-const bool DRAW_MODEL_PATH = false;
 const qreal REROUTE_DISTANCE = 25;
 const float MANEUVER_TRANSITION_THRESHOLD = 10;
 
@@ -24,19 +22,17 @@ const float MIN_PITCH = 0;
 const float MAP_SCALE = 2;
 
 
-MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings) {
+MapWindow::MapWindow(const QMapboxGLSettings &settings) :
-  if (DRAW_MODEL_PATH) {
+  m_settings(settings), velocity_filter(0, 10, 0.1) {
-    sm = new SubMaster({"liveLocationKalman", "modelV2"});
+  sm = new SubMaster({"liveLocationKalman"});
-  } else {
-    sm = new SubMaster({"liveLocationKalman"});
-  }
 
   timer = new QTimer(this);
   QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timerUpdate()));
+  timer->start(100);
 
   recompute_timer = new QTimer(this);
-  recompute_timer->start(1000);
   QObject::connect(recompute_timer, SIGNAL(timeout()), this, SLOT(recomputeRoute()));
+  recompute_timer->start(1000);
 
   // Instructions
   map_instructions = new MapInstructions(this);
@@ -124,20 +120,12 @@ void MapWindow::timerUpdate() {
     update();
   }
 
-  loaded_once = loaded_once || m_map->isFullyLoaded();
-  if (!loaded_once) {
-    map_instructions->showError("Map loading");
-    return;
-  }
-
-  initLayers();
-
   sm->update(0);
   if (sm->updated("liveLocationKalman")) {
     auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman();
     gps_ok = location.getGpsOK();
 
-    bool localizer_valid = location.getStatus() == cereal::LiveLocationKalman::Status::VALID;
+    localizer_valid = location.getStatus() == cereal::LiveLocationKalman::Status::VALID;
 
     if (localizer_valid) {
       auto pos = location.getPositionGeodetic();
@@ -149,81 +137,83 @@ void MapWindow::timerUpdate() {
 
       last_position = coordinate;
       last_bearing = bearing;
+      velocity_filter.update(velocity);
+    } else {
+      map_instructions->showError("Waiting for GPS");
+    }
+  }
 
-      if (pan_counter == 0) {
+  if (m_map.isNull()) return;
-        m_map->setCoordinate(coordinate);
-        m_map->setBearing(bearing);
-      } else {
-        pan_counter--;
-      }
 
-      if (zoom_counter == 0) {
+  // Update map once it's loaded
-        static FirstOrderFilter velocity_filter(velocity, 10, 0.1);
+  loaded_once = loaded_once || m_map->isFullyLoaded();
-        m_map->setZoom(util::map_val<float>(velocity_filter.update(velocity), 0, 30, MAX_ZOOM, MIN_ZOOM));
+  if (!loaded_once) {
-      } else {
+    map_instructions->showError("Map loading");
-        zoom_counter--;
+    return;
-      }
+  }
 
-      // Update current location marker
+  initLayers();
-      auto point = coordinate_to_collection(coordinate);
-      QMapbox::Feature feature1(QMapbox::Feature::PointType, point, {}, {});
-      QVariantMap carPosSource;
-      carPosSource["type"] = "geojson";
-      carPosSource["data"] = QVariant::fromValue<QMapbox::Feature>(feature1);
-      m_map->updateSource("carPosSource", carPosSource);
-
-      // Update model path
-      if (DRAW_MODEL_PATH) {
-        auto model = (*sm)["modelV2"].getModelV2();
-        auto path_points = model_to_collection(location.getCalibratedOrientationECEF(), location.getPositionECEF(), model.getPosition());
-        QMapbox::Feature feature2(QMapbox::Feature::LineStringType, path_points, {}, {});
-        QVariantMap modelPathSource;
-        modelPathSource["type"] = "geojson";
-        modelPathSource["data"] = QVariant::fromValue<QMapbox::Feature>(feature2);
-        m_map->updateSource("modelPathSource", modelPathSource);
-      }
 
-      if (segment.isValid()) {
+  if (pan_counter == 0) {
-        // Show route instructions
+    if (last_position) m_map->setCoordinate(*last_position);
-        auto cur_maneuver = segment.maneuver();
+    if (last_bearing) m_map->setBearing(*last_bearing);
-        auto attrs = cur_maneuver.extendedAttributes();
+  } else {
-        if (cur_maneuver.isValid() && attrs.contains("mapbox.banner_instructions")) {
+    pan_counter--;
-          float along_geometry = distance_along_geometry(segment.path(), to_QGeoCoordinate(*last_position));
+  }
-          float distance_to_maneuver = segment.distance() - along_geometry;
+
-          emit distanceChanged(std::max(0.0f, distance_to_maneuver));
+  if (zoom_counter == 0) {
-
+    m_map->setZoom(util::map_val<float>(velocity_filter.x(), 0, 30, MAX_ZOOM, MIN_ZOOM));
-          m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance
+  } else {
-
+    zoom_counter--;
-          auto banner = attrs["mapbox.banner_instructions"].toList();
+  }
-          if (banner.size()) {
-            auto banner_0 = banner[0].toMap();
-            float show_at = banner_0["distance_along_geometry"].toDouble();
-            emit instructionsChanged(banner_0, distance_to_maneuver < show_at);
-          }
 
-          // Transition to next route segment
+  // Update current location marker
-          if (distance_to_maneuver < -MANEUVER_TRANSITION_THRESHOLD) {
+  if (localizer_valid) {
-            auto next_segment = segment.nextRouteSegment();
+    auto point = coordinate_to_collection(*last_position);
-            if (next_segment.isValid()) {
+    QMapbox::Feature feature1(QMapbox::Feature::PointType, point, {}, {});
-              segment = next_segment;
+    QVariantMap carPosSource;
-
+    carPosSource["type"] = "geojson";
-              recompute_backoff = std::max(0, recompute_backoff - 1);
+    carPosSource["data"] = QVariant::fromValue<QMapbox::Feature>(feature1);
-              recompute_countdown = 0;
+    m_map->updateSource("carPosSource", carPosSource);
-            } else {
+  }
-              qWarning() << "Destination reached";
+
-              Params().remove("NavDestination");
+  // Show route instructions
-
+  if (segment.isValid()) {
-              // Clear route if driving away from destination
+    auto cur_maneuver = segment.maneuver();
-              float d = segment.maneuver().position().distanceTo(to_QGeoCoordinate(*last_position));
+    auto attrs = cur_maneuver.extendedAttributes();
-              if (d > REROUTE_DISTANCE) {
+    if (cur_maneuver.isValid() && attrs.contains("mapbox.banner_instructions")) {
-                clearRoute();
+      float along_geometry = distance_along_geometry(segment.path(), to_QGeoCoordinate(*last_position));
-              }
+      float distance_to_maneuver = segment.distance() - along_geometry;
-            }
+      emit distanceChanged(std::max(0.0f, distance_to_maneuver));
+
+      m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance
+
+      auto banner = attrs["mapbox.banner_instructions"].toList();
+      if (banner.size()) {
+        auto banner_0 = banner[0].toMap();
+        float show_at = banner_0["distance_along_geometry"].toDouble();
+        emit instructionsChanged(banner_0, distance_to_maneuver < show_at);
+      }
+
+      // Transition to next route segment
+      if (distance_to_maneuver < -MANEUVER_TRANSITION_THRESHOLD) {
+        auto next_segment = segment.nextRouteSegment();
+        if (next_segment.isValid()) {
+          segment = next_segment;
+
+          recompute_backoff = std::max(0, recompute_backoff - 1);
+          recompute_countdown = 0;
+        } else {
+          qWarning() << "Destination reached";
+          Params().remove("NavDestination");
+
+          // Clear route if driving away from destination
+          float d = segment.maneuver().position().distanceTo(to_QGeoCoordinate(*last_position));
+          if (d > REROUTE_DISTANCE) {
+            clearRoute();
           }
         }
       }
-    } else {
-      map_instructions->showError("Waiting for GPS");
     }
   }
 }
@@ -252,7 +242,6 @@ void MapWindow::initializeGL() {
       loaded_once = true;
     }
   });
-  timer->start(100);
 }
 
 void MapWindow::paintGL() {
@@ -268,23 +257,23 @@ static float get_time_typical(const QGeoRouteSegment &segment) {
 
 
 void MapWindow::recomputeRoute() {
-  // Last position is valid if read from param or from GPS
   if (!last_position) {
     return;
   }
 
-  bool should_recompute = shouldRecompute();
   auto new_destination = coordinate_from_param("NavDestination");
-
   if (!new_destination) {
     clearRoute();
     return;
   }
 
+  bool should_recompute = shouldRecompute();
   if (*new_destination != nav_destination) {
     qWarning() << "Got new destination from NavDestination param" << *new_destination;
+
     setVisible(true); // Show map on destination set/change
     // TODO: close sidebar
+
     should_recompute = true;
   }
 
@@ -292,8 +281,8 @@ void MapWindow::recomputeRoute() {
 
   if (!gps_ok && segment.isValid()) return; // Don't recompute when gps drifts in tunnels
 
-  // Only do API request when map is loaded
+  // Only do API request when map is fully loaded
-  if (!m_map.isNull()) {
+  if (loaded_once) {
     if (recompute_countdown == 0 && should_recompute) {
       recompute_countdown = std::pow(2, recompute_backoff);
       recompute_backoff = std::min(7, recompute_backoff + 1);
@@ -363,7 +352,6 @@ void MapWindow::routeCalculated(QGeoRouteReply *reply) {
     }
   } else {
     qWarning() << "Got error in route reply" << reply->errorString();
-  
   }
 
   reply->deleteLater();
@@ -726,8 +714,6 @@ MapETA::MapETA(QWidget * parent) : QWidget(parent) {
 
 
 void MapETA::updateETA(float s, float s_typical, float d) {
-  setVisible(true);
-
   // ETA
   auto eta_time = QDateTime::currentDateTime().addSecs(s).time();
   if (params.getBool("NavSettingTime24h")) {
@@ -775,6 +761,9 @@ void MapETA::updateETA(float s, float s_typical, float d) {
   distance_str.setNum(num, 'f', num < 100 ? 1 : 0);
   distance->setText(distance_str);
 
+  show();
+  adjustSize();
+  repaint();
   adjustSize();
 
   // Rounded corners

selfdrive/ui/qt/maps/map.h

@@ -24,6 +24,7 @@
 #include <QPixmap>
 
 #include "selfdrive/common/params.h"
+#include "selfdrive/common/util.h"
 #include "cereal/messaging/messaging.h"
 
 class MapInstructions : public QWidget {
@@ -105,6 +106,8 @@ private:
   // Position
   std::optional<QMapbox::Coordinate> last_position;
   std::optional<float> last_bearing;
+  FirstOrderFilter velocity_filter;
+  bool localizer_valid = false;
 
   // Route
   bool gps_ok = false;