diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index 0f6d4a0be91417..365852419c9aed 100644 --- 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(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 index 5849b64f55b582..d9f93a0e5b2663 100644 --- 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;