diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index f9059c6f08..80cd82277e 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -59,6 +59,7 @@ MapWindow::~MapWindow() { } void MapWindow::initLayers() { + RETURN_IF_SUNNYPILOT // This doesn't work from initializeGL if (!m_map->layerExists("modelPathLayer")) { qDebug() << "Initializing modelPathLayer"; @@ -118,6 +119,7 @@ void MapWindow::initLayers() { } void MapWindow::updateState(const UIState &s) { + RETURN_IF_SUNNYPILOT if (!uiState()->scene.started) { return; } diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h index fd4218bfc1..00e8b6f78f 100644 --- a/selfdrive/ui/qt/maps/map.h +++ b/selfdrive/ui/qt/maps/map.h @@ -46,7 +46,7 @@ protected: void initLayers(); -private: +protected: void mousePressEvent(QMouseEvent *ev) final; void mouseDoubleClickEvent(QMouseEvent *ev) final; void mouseMoveEvent(QMouseEvent *ev) final; @@ -60,7 +60,7 @@ protected: bool loaded_once = false; bool prev_time_valid = true; -private: +protected: // Panning QPointF m_lastPos; int interaction_counter = 0; diff --git a/selfdrive/ui/qt/maps/map_panel.cc b/selfdrive/ui/qt/maps/map_panel.cc index 4f92c98482..cd448483ff 100644 --- a/selfdrive/ui/qt/maps/map_panel.cc +++ b/selfdrive/ui/qt/maps/map_panel.cc @@ -8,6 +8,7 @@ #ifdef SUNNYPILOT #include "selfdrive/ui/sunnypilot/ui.h" #include "selfdrive/ui/sunnypilot/qt/maps/map.h" +#define MapWindow MapWindowSP #else #include "selfdrive/ui/ui.h" #include "selfdrive/ui/qt/maps/map.h" diff --git a/selfdrive/ui/sunnypilot/qt/maps/map.cc b/selfdrive/ui/sunnypilot/qt/maps/map.cc index 7cb4a77539..d38f3f27b9 100644 --- a/selfdrive/ui/sunnypilot/qt/maps/map.cc +++ b/selfdrive/ui/sunnypilot/qt/maps/map.cc @@ -3,14 +3,75 @@ #include "common/swaglog.h" #include "selfdrive/ui/sunnypilot/qt/maps/map_helpers.h" +const float MAX_ZOOM = 17; +const float MIN_ZOOM = 14; +const float MAX_PITCH = 50; + MapWindowSP::MapWindowSP(const QMapLibre::Settings &settings) : MapWindow(settings) { QObject::disconnect(uiState(), &UIState::uiUpdate, this, &MapWindow::updateState); QObject::connect(uiStateSP(), &UIStateSP::uiUpdate, this, &MapWindowSP::updateState); } +MapWindowSP::~MapWindowSP() { + makeCurrent(); +} + void MapWindowSP::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")) { + qDebug() << "Initializing navLayer"; + QVariantMap nav; + nav["type"] = "line"; + nav["source"] = "navSource"; + m_map->addLayer("navLayer", nav, "road-intersection"); + + QVariantMap transition; + transition["duration"] = 400; // ms m_map->setPaintProperty("navLayer", "line-color", getNavPathColor(uiStateSP()->scene.navigate_on_openpilot_deprecated)); + m_map->setPaintProperty("navLayer", "line-color-transition", transition); + m_map->setPaintProperty("navLayer", "line-width", 7.5); + m_map->setLayoutProperty("navLayer", "line-cap", "round"); + } + if (!m_map->layerExists("pinLayer")) { + qDebug() << "Initializing pinLayer"; + m_map->addImage("default_marker", QImage("../assets/navigation/default_marker.svg")); + QVariantMap pin; + pin["type"] = "symbol"; + pin["source"] = "pinSource"; + m_map->addLayer("pinLayer", pin); + m_map->setLayoutProperty("pinLayer", "icon-pitch-alignment", "viewport"); + m_map->setLayoutProperty("pinLayer", "icon-image", "default_marker"); + m_map->setLayoutProperty("pinLayer", "icon-ignore-placement", true); + m_map->setLayoutProperty("pinLayer", "icon-allow-overlap", true); + m_map->setLayoutProperty("pinLayer", "symbol-sort-key", 0); + m_map->setLayoutProperty("pinLayer", "icon-anchor", "bottom"); + } + if (!m_map->layerExists("carPosLayer")) { + qDebug() << "Initializing carPosLayer"; + m_map->addImage("label-arrow", QImage("../assets/images/triangle.svg")); + + QVariantMap carPos; + carPos["type"] = "symbol"; + carPos["source"] = "carPosSource"; + m_map->addLayer("carPosLayer", carPos); + m_map->setLayoutProperty("carPosLayer", "icon-pitch-alignment", "map"); + m_map->setLayoutProperty("carPosLayer", "icon-image", "label-arrow"); + m_map->setLayoutProperty("carPosLayer", "icon-size", 0.5); + m_map->setLayoutProperty("carPosLayer", "icon-ignore-placement", true); + m_map->setLayoutProperty("carPosLayer", "icon-allow-overlap", true); + // TODO: remove, symbol-sort-key does not seem to matter outside of each layer + m_map->setLayoutProperty("carPosLayer", "symbol-sort-key", 0); } if ((!m_map->layerExists("buildingsLayer")) && uiStateSP()->scene.map_3d_buildings) { // Could put this behind the cellular metered toggle in case it increases data usage qDebug() << "Initializing buildingsLayer"; @@ -58,7 +119,7 @@ void MapWindowSP::initLayers() { } void MapWindowSP::updateState(const UIStateSP &s) { - if (!uiStateSP()->scene.started) { + if (!uiState()->scene.started) { return; } const SubMaster &sm = *(s.sm); @@ -77,7 +138,7 @@ void MapWindowSP::updateState(const UIStateSP &s) { auto car_control = sm["carControl"].getCarControl(); bool nav_enabled = sm["modelV2"].getModelV2().getNavEnabledDEPRECATED() && (sm["controlsState"].getControlsState().getEnabled() || car_control.getLatActive() || car_control.getLongActive()); - if (nav_enabled != uiStateSP()->scene.navigate_on_openpilot_deprecated) { + if (nav_enabled != uiState()->scene.navigate_on_openpilot_deprecated) { if (loaded_once) { m_map->setPaintProperty("navLayer", "line-color", getNavPathColor(nav_enabled)); } @@ -85,10 +146,116 @@ void MapWindowSP::updateState(const UIStateSP &s) { emit requestVisible(true); } } - uiStateSP()->scene.navigate_on_openpilot_deprecated = nav_enabled; + uiState()->scene.navigate_on_openpilot_deprecated = nav_enabled; } - MapWindow::updateState(s); + if (sm.updated("liveLocationKalman")) { + auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman(); + auto locationd_pos = locationd_location.getPositionGeodetic(); + auto locationd_orientation = locationd_location.getCalibratedOrientationNED(); + auto locationd_velocity = locationd_location.getVelocityCalibrated(); + auto locationd_ecef = locationd_location.getPositionECEF(); + + locationd_valid = (locationd_pos.getValid() && locationd_orientation.getValid() && locationd_velocity.getValid() && locationd_ecef.getValid()); + if (locationd_valid) { + // Check std norm + auto pos_ecef_std = locationd_ecef.getStd(); + bool pos_accurate_enough = sqrt(pow(pos_ecef_std[0], 2) + pow(pos_ecef_std[1], 2) + pow(pos_ecef_std[2], 2)) < 100; + locationd_valid = pos_accurate_enough; + } + + if (locationd_valid) { + last_position = QMapLibre::Coordinate(locationd_pos.getValue()[0], locationd_pos.getValue()[1]); + last_bearing = RAD2DEG(locationd_orientation.getValue()[2]); + velocity_filter.update(std::max(10.0, locationd_velocity.getValue()[0])); + } + } + + if (sm.updated("navRoute") && sm["navRoute"].getNavRoute().getCoordinates().size()) { + auto nav_dest = coordinate_from_param("NavDestination"); + bool allow_open = std::exchange(last_valid_nav_dest, nav_dest) != nav_dest && + nav_dest && !isVisible(); + qWarning() << "Got new navRoute from navd. Opening map:" << allow_open; + + // Show map on destination set/change + if (allow_open) { + emit requestSettings(false); + emit requestVisible(true); + } + } + + loaded_once = loaded_once || (m_map && m_map->isFullyLoaded()); + if (!loaded_once) { + setError(tr("Map Loading")); + return; + } + initLayers(); + + if (!locationd_valid) { + setError(tr("Waiting for GPS")); + } else if (routing_problem) { + setError(tr("Waiting for route")); + } else { + setError(""); + } + + if (locationd_valid) { + // Update current location marker + auto point = coordinate_to_collection(*last_position); + QMapLibre::Feature feature1(QMapLibre::Feature::PointType, point, {}, {}); + QVariantMap carPosSource; + carPosSource["type"] = "geojson"; + carPosSource["data"] = QVariant::fromValue(feature1); + m_map->updateSource("carPosSource", carPosSource); + + // Map bearing isn't updated when interacting, keep location marker up to date + if (last_bearing) { + m_map->setLayoutProperty("carPosLayer", "icon-rotate", *last_bearing - m_map->bearing()); + } + } + + if (interaction_counter == 0) { + if (last_position) m_map->setCoordinate(*last_position); + if (last_bearing) m_map->setBearing(*last_bearing); + m_map->setZoom(util::map_val(velocity_filter.x(), 0, 30, MAX_ZOOM, MIN_ZOOM)); + } else { + interaction_counter--; + } + + if (sm.updated("navInstruction")) { + // an invalid navInstruction packet with a nav destination is only possible if: + // - API exception/no internet + // - route response is empty + // - any time navd is waiting for recompute_countdown + routing_problem = !sm.valid("navInstruction") && coordinate_from_param("NavDestination").has_value(); + + if (sm.valid("navInstruction")) { + auto i = sm["navInstruction"].getNavInstruction(); + map_eta->updateETA(i.getTimeRemaining(), i.getTimeRemainingTypical(), i.getDistanceRemaining()); + + if (locationd_valid) { + m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance + map_instructions->updateInstructions(i); + } + } else { + clearRoute(); + } + } + + if (sm.rcv_frame("navRoute") != route_rcv_frame) { + qWarning() << "Updating navLayer with new route"; + auto route = sm["navRoute"].getNavRoute(); + auto route_points = capnp_coordinate_list_to_collection(route.getCoordinates()); + QMapLibre::Feature feature(QMapLibre::Feature::LineStringType, route_points, {}, {}); + QVariantMap navSource; + navSource["type"] = "geojson"; + navSource["data"] = QVariant::fromValue(feature); + m_map->updateSource("navSource", navSource); + m_map->setLayoutProperty("navLayer", "visibility", "visible"); + + route_rcv_frame = sm.rcv_frame("navRoute"); + updateDestinationMarker(); + } } void MapWindowSP::offroadTransition(bool offroad) {