From d1caa4891122a7ae311e6de008f59b80b842b8de Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Tue, 4 Jun 2024 11:57:28 -0700
Subject: [PATCH] FrogPilot community - Display model path in map

Co-Authored-By: pencilpusher <83676301+jakethesnake420@users.noreply.github.com>
---
 selfdrive/ui/qt/maps/map.cc | 12 ++++++++++++
 selfdrive/ui/qt/maps/map.h  |  2 ++
 2 files changed, 14 insertions(+)

diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc
index 5a47fa00b..b0efab38d 100644
--- a/selfdrive/ui/qt/maps/map.cc
+++ b/selfdrive/ui/qt/maps/map.cc
@@ -293,6 +293,18 @@ void MapWindow::updateState(const UIState &s) {
     route_rcv_frame = sm.rcv_frame("navRoute");
     updateDestinationMarker();
   }
+
+  // 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");
+  }
 }
 
 void MapWindow::setError(const QString &err_str) {
diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h
index fdef25682..65d412746 100644
--- a/selfdrive/ui/qt/maps/map.h
+++ b/selfdrive/ui/qt/maps/map.h
@@ -82,6 +82,8 @@ private:
   // FrogPilot variables
   Params params;
 
+  uint64_t model_rcv_frame = 0;
+
 private slots:
   void updateState(const UIState &s);