Merge pull request commaai#100 from mike86437/map-uipath

Add model uipath to map
MoreTore · Feb 27, 2024 · 5024666 · 5024666
2 parents f3cfad6 + b5f096e
commit 5024666
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 12 deletions.
diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc
@@ -54,18 +54,7 @@ MapWindow::~MapWindow() {
 
 void MapWindow::initLayers() {
   // This doesn't work from initializeGL
-  if (!m_map->layerExists("modelPathLayer")) {
-    qDebug() << "Initializing modelPathLayer";
-    QVariantMap modelPath;
-    //modelPath["id"] = "modelPathLayer";
-    modelPath["type"] = "line";
-    modelPath["source"] = "modelPathSource";
-    m_map->addLayer("modelPathLayer", modelPath);
-    m_map->setPaintProperty("modelPathLayer", "line-color", QColor("red"));
-    m_map->setPaintProperty("modelPathLayer", "line-width", 5.0);
-    m_map->setLayoutProperty("modelPathLayer", "line-cap", "round");
-  }
-  if (!m_map->layerExists("navLayer")) {
+    if (!m_map->layerExists("navLayer")) {
     qDebug() << "Initializing navLayer";
     QVariantMap nav;
     nav["type"] = "line";
@@ -79,6 +68,17 @@ void MapWindow::initLayers() {
     m_map->setPaintProperty("navLayer", "line-width", 7.5);
     m_map->setLayoutProperty("navLayer", "line-cap", "round");
   }
+if (!m_map->layerExists("modelPathLayer")) {
+    qDebug() << "Initializing modelPathLayer";
+    QVariantMap modelPath;
+    //modelPath["id"] = "modelPathLayer";
+    modelPath["type"] = "line";
+    modelPath["source"] = "modelPathSource";
+    m_map->addLayer("modelPathLayer", modelPath);
+    m_map->setPaintProperty("modelPathLayer", "line-color", QColor("red"));
+    m_map->setPaintProperty("modelPathLayer", "line-width", 5.0);
+    m_map->setLayoutProperty("modelPathLayer", "line-cap", "round");
+  }
   if (!m_map->layerExists("pinLayer")) {
     qDebug() << "Initializing pinLayer";
     m_map->addImage("default_marker", QImage("../assets/navigation/default_marker.svg"));
@@ -194,6 +194,17 @@ void MapWindow::updateState(const UIState &s) {
       last_bearing = RAD2DEG(locationd_orientation.getValue()[2]);
       velocity_filter.update(std::max(10.0, locationd_velocity.getValue()[0]));
     }
+}
+  // Credit to jakethesnake420
+  if (loaded_once && (sm.rcv_frame("uiPlan") != model_rcv_frame)) {
+    auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman();
+    auto model_path = model_to_collection(locationd_location.getCalibratedOrientationECEF(), locationd_location.getPositionECEF(), sm["uiPlan"].getUiPlan().getPosition());
+    QMapLibre::Feature model_path_feature(QMapLibre::Feature::LineStringType, model_path, {}, {});
+    QVariantMap modelV2Path;
+    modelV2Path["type"] =  "geojson";
+    modelV2Path["data"] = QVariant::fromValue<QMapLibre::Feature>(model_path_feature);
+    m_map->updateSource("modelPathSource", modelV2Path);
+    model_rcv_frame = sm.rcv_frame("uiPlan");
   }
 
   if (sm.updated("navRoute") && sm["navRoute"].getNavRoute().getCoordinates().size()) {

diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h
@@ -77,6 +77,7 @@ class MapWindow : public QOpenGLWidget {
   void clearRoute();
   void updateDestinationMarker();
   uint64_t route_rcv_frame = 0;
+  uint64_t model_rcv_frame = 0;
 
   // FrogPilot variables
   Params params;