Navigation: add destination marker (#26873)

* add navigation destination marker

* fix removal

* update default marker icon

* update default marker

Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>

selfdrive/assets/navigation/default_marker.svg

@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<svg fill="red" width="38" height="42" viewBox="0 0 15 15" xmlns="http://www.w3.org/2000/svg"  id="marker">
+  <path stroke="darkred" stroke-width="0.5" d="M7.5 1C5.42312 1 3 2.2883 3 5.56759C3 7.79276 6.46156 12.7117 7.5 14C8.42309 12.7117 12 7.90993 12 5.56759C12 2.2883 9.57688 1 7.5 1Z"/>
+  <circle cx="7.5" cy="5.5" r="2" fill="darkred"/>
+</svg>

selfdrive/ui/qt/maps/map.cc

@@ -47,7 +47,7 @@ MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings),
   map_eta->setVisible(false);
 
   auto last_gps_position = coordinate_from_param("LastGPSPosition");
-  if (last_gps_position) {
+  if (last_gps_position.has_value()) {
     last_position = *last_gps_position;
   }
 
@@ -82,6 +82,7 @@ void MapWindow::initLayers() {
     m_map->setPaintProperty("navLayer", "line-color", QColor("#31a1ee"));
     m_map->setPaintProperty("navLayer", "line-width", 7.5);
     m_map->setLayoutProperty("navLayer", "line-cap", "round");
+    m_map->addAnnotationIcon("default_marker", QImage("../assets/navigation/default_marker.svg"));
   }
   if (!m_map->layerExists("carPosLayer")) {
     qDebug() << "Initializing carPosLayer";
@@ -220,7 +221,6 @@ void MapWindow::updateState(const UIState &s) {
         emit instructionsChanged(i);
       }
     } else {
-      m_map->setPitch(MIN_PITCH);
       clearRoute();
     }
   }
@@ -237,6 +237,7 @@ void MapWindow::updateState(const UIState &s) {
     m_map->setLayoutProperty("navLayer", "visibility", "visible");
 
     route_rcv_frame = sm.rcv_frame("navRoute");
+    update_destination_marker();
   }
 }
 
@@ -274,6 +275,7 @@ void MapWindow::clearRoute() {
   if (!m_map.isNull()) {
     m_map->setLayoutProperty("navLayer", "visibility", "none");
     m_map->setPitch(MIN_PITCH);
+    update_destination_marker();
   }
 
   map_instructions->hideIfNoError();
@@ -361,6 +363,19 @@ void MapWindow::offroadTransition(bool offroad) {
   last_bearing = {};
 }
 
+void MapWindow::update_destination_marker() {
+  if (marker_id != -1) {
+    m_map->removeAnnotation(marker_id);
+    marker_id = -1;
+  }
+
+  auto nav_dest = coordinate_from_param("NavDestination");
+  if (nav_dest.has_value()) {
+    auto ano = QMapbox::SymbolAnnotation {*nav_dest, "default_marker"};
+    marker_id = m_map->addAnnotation(QVariant::fromValue<QMapbox::SymbolAnnotation>(ano));
+  }
+}
+
 MapInstructions::MapInstructions(QWidget * parent) : QWidget(parent) {
   is_rhd = Params().getBool("IsRhdDetected");
   QHBoxLayout *main_layout = new QHBoxLayout(this);

selfdrive/ui/qt/maps/map.h

@@ -79,6 +79,7 @@ private:
 
   QMapboxGLSettings m_settings;
   QScopedPointer<QMapboxGL> m_map;
+  QMapbox::AnnotationID marker_id = -1;
 
   void initLayers();
 
@@ -111,6 +112,7 @@ private:
   MapETA* map_eta;
 
   void clearRoute();
+  void update_destination_marker();
   uint64_t route_rcv_frame = 0;
 
 private slots: