diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h index 65fcd3f2b4..460fb8f5c0 100644 --- a/selfdrive/common/util.h +++ b/selfdrive/common/util.h @@ -127,6 +127,7 @@ public: return x_; } inline void reset(float x) { x_ = x; } + inline float x(){ return x_; } private: float x_, k_; diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index c3063cffb1..b822a10b8e 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -4,7 +4,6 @@ #include -#include "selfdrive/common/util.h" #include "selfdrive/common/swaglog.h" #include "selfdrive/ui/ui.h" #include "selfdrive/ui/qt/util.h" @@ -13,7 +12,6 @@ const int PAN_TIMEOUT = 100; -const bool DRAW_MODEL_PATH = false; const qreal REROUTE_DISTANCE = 25; const float MANEUVER_TRANSITION_THRESHOLD = 10; @@ -24,19 +22,17 @@ const float MIN_PITCH = 0; const float MAP_SCALE = 2; -MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings) { - if (DRAW_MODEL_PATH) { - sm = new SubMaster({"liveLocationKalman", "modelV2"}); - } else { - sm = new SubMaster({"liveLocationKalman"}); - } +MapWindow::MapWindow(const QMapboxGLSettings &settings) : + m_settings(settings), velocity_filter(0, 10, 0.1) { + sm = new SubMaster({"liveLocationKalman"}); timer = new QTimer(this); QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timerUpdate())); + timer->start(100); recompute_timer = new QTimer(this); - recompute_timer->start(1000); QObject::connect(recompute_timer, SIGNAL(timeout()), this, SLOT(recomputeRoute())); + recompute_timer->start(1000); // Instructions map_instructions = new MapInstructions(this); @@ -124,20 +120,12 @@ void MapWindow::timerUpdate() { update(); } - loaded_once = loaded_once || m_map->isFullyLoaded(); - if (!loaded_once) { - map_instructions->showError("Map loading"); - return; - } - - initLayers(); - sm->update(0); if (sm->updated("liveLocationKalman")) { auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); gps_ok = location.getGpsOK(); - bool localizer_valid = location.getStatus() == cereal::LiveLocationKalman::Status::VALID; + localizer_valid = location.getStatus() == cereal::LiveLocationKalman::Status::VALID; if (localizer_valid) { auto pos = location.getPositionGeodetic(); @@ -149,81 +137,83 @@ void MapWindow::timerUpdate() { last_position = coordinate; last_bearing = bearing; + velocity_filter.update(velocity); + } else { + map_instructions->showError("Waiting for GPS"); + } + } - if (pan_counter == 0) { - m_map->setCoordinate(coordinate); - m_map->setBearing(bearing); - } else { - pan_counter--; - } + if (m_map.isNull()) return; - if (zoom_counter == 0) { - static FirstOrderFilter velocity_filter(velocity, 10, 0.1); - m_map->setZoom(util::map_val(velocity_filter.update(velocity), 0, 30, MAX_ZOOM, MIN_ZOOM)); - } else { - zoom_counter--; - } + // Update map once it's loaded + loaded_once = loaded_once || m_map->isFullyLoaded(); + if (!loaded_once) { + map_instructions->showError("Map loading"); + return; + } - // Update current location marker - auto point = coordinate_to_collection(coordinate); - QMapbox::Feature feature1(QMapbox::Feature::PointType, point, {}, {}); - QVariantMap carPosSource; - carPosSource["type"] = "geojson"; - carPosSource["data"] = QVariant::fromValue(feature1); - m_map->updateSource("carPosSource", carPosSource); - - // Update model path - if (DRAW_MODEL_PATH) { - auto model = (*sm)["modelV2"].getModelV2(); - auto path_points = model_to_collection(location.getCalibratedOrientationECEF(), location.getPositionECEF(), model.getPosition()); - QMapbox::Feature feature2(QMapbox::Feature::LineStringType, path_points, {}, {}); - QVariantMap modelPathSource; - modelPathSource["type"] = "geojson"; - modelPathSource["data"] = QVariant::fromValue(feature2); - m_map->updateSource("modelPathSource", modelPathSource); - } + initLayers(); - if (segment.isValid()) { - // Show route instructions - auto cur_maneuver = segment.maneuver(); - auto attrs = cur_maneuver.extendedAttributes(); - if (cur_maneuver.isValid() && attrs.contains("mapbox.banner_instructions")) { - float along_geometry = distance_along_geometry(segment.path(), to_QGeoCoordinate(*last_position)); - float distance_to_maneuver = segment.distance() - along_geometry; - emit distanceChanged(std::max(0.0f, distance_to_maneuver)); - - m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance - - auto banner = attrs["mapbox.banner_instructions"].toList(); - if (banner.size()) { - auto banner_0 = banner[0].toMap(); - float show_at = banner_0["distance_along_geometry"].toDouble(); - emit instructionsChanged(banner_0, distance_to_maneuver < show_at); - } + if (pan_counter == 0) { + if (last_position) m_map->setCoordinate(*last_position); + if (last_bearing) m_map->setBearing(*last_bearing); + } else { + pan_counter--; + } + + if (zoom_counter == 0) { + m_map->setZoom(util::map_val(velocity_filter.x(), 0, 30, MAX_ZOOM, MIN_ZOOM)); + } else { + zoom_counter--; + } - // Transition to next route segment - if (distance_to_maneuver < -MANEUVER_TRANSITION_THRESHOLD) { - auto next_segment = segment.nextRouteSegment(); - if (next_segment.isValid()) { - segment = next_segment; - - recompute_backoff = std::max(0, recompute_backoff - 1); - recompute_countdown = 0; - } else { - qWarning() << "Destination reached"; - Params().remove("NavDestination"); - - // Clear route if driving away from destination - float d = segment.maneuver().position().distanceTo(to_QGeoCoordinate(*last_position)); - if (d > REROUTE_DISTANCE) { - clearRoute(); - } - } + // Update current location marker + if (localizer_valid) { + auto point = coordinate_to_collection(*last_position); + QMapbox::Feature feature1(QMapbox::Feature::PointType, point, {}, {}); + QVariantMap carPosSource; + carPosSource["type"] = "geojson"; + carPosSource["data"] = QVariant::fromValue(feature1); + m_map->updateSource("carPosSource", carPosSource); + } + + // Show route instructions + if (segment.isValid()) { + auto cur_maneuver = segment.maneuver(); + auto attrs = cur_maneuver.extendedAttributes(); + if (cur_maneuver.isValid() && attrs.contains("mapbox.banner_instructions")) { + float along_geometry = distance_along_geometry(segment.path(), to_QGeoCoordinate(*last_position)); + float distance_to_maneuver = segment.distance() - along_geometry; + emit distanceChanged(std::max(0.0f, distance_to_maneuver)); + + m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance + + auto banner = attrs["mapbox.banner_instructions"].toList(); + if (banner.size()) { + auto banner_0 = banner[0].toMap(); + float show_at = banner_0["distance_along_geometry"].toDouble(); + emit instructionsChanged(banner_0, distance_to_maneuver < show_at); + } + + // Transition to next route segment + if (distance_to_maneuver < -MANEUVER_TRANSITION_THRESHOLD) { + auto next_segment = segment.nextRouteSegment(); + if (next_segment.isValid()) { + segment = next_segment; + + recompute_backoff = std::max(0, recompute_backoff - 1); + recompute_countdown = 0; + } else { + qWarning() << "Destination reached"; + Params().remove("NavDestination"); + + // Clear route if driving away from destination + float d = segment.maneuver().position().distanceTo(to_QGeoCoordinate(*last_position)); + if (d > REROUTE_DISTANCE) { + clearRoute(); } } } - } else { - map_instructions->showError("Waiting for GPS"); } } } @@ -252,7 +242,6 @@ void MapWindow::initializeGL() { loaded_once = true; } }); - timer->start(100); } void MapWindow::paintGL() { @@ -268,23 +257,23 @@ static float get_time_typical(const QGeoRouteSegment &segment) { void MapWindow::recomputeRoute() { - // Last position is valid if read from param or from GPS if (!last_position) { return; } - bool should_recompute = shouldRecompute(); auto new_destination = coordinate_from_param("NavDestination"); - if (!new_destination) { clearRoute(); return; } + bool should_recompute = shouldRecompute(); if (*new_destination != nav_destination) { qWarning() << "Got new destination from NavDestination param" << *new_destination; + setVisible(true); // Show map on destination set/change // TODO: close sidebar + should_recompute = true; } @@ -292,8 +281,8 @@ void MapWindow::recomputeRoute() { if (!gps_ok && segment.isValid()) return; // Don't recompute when gps drifts in tunnels - // Only do API request when map is loaded - if (!m_map.isNull()) { + // Only do API request when map is fully loaded + if (loaded_once) { if (recompute_countdown == 0 && should_recompute) { recompute_countdown = std::pow(2, recompute_backoff); recompute_backoff = std::min(7, recompute_backoff + 1); @@ -363,7 +352,6 @@ void MapWindow::routeCalculated(QGeoRouteReply *reply) { } } else { qWarning() << "Got error in route reply" << reply->errorString(); - } reply->deleteLater(); @@ -726,8 +714,6 @@ MapETA::MapETA(QWidget * parent) : QWidget(parent) { void MapETA::updateETA(float s, float s_typical, float d) { - setVisible(true); - // ETA auto eta_time = QDateTime::currentDateTime().addSecs(s).time(); if (params.getBool("NavSettingTime24h")) { @@ -775,6 +761,9 @@ void MapETA::updateETA(float s, float s_typical, float d) { distance_str.setNum(num, 'f', num < 100 ? 1 : 0); distance->setText(distance_str); + show(); + adjustSize(); + repaint(); adjustSize(); // Rounded corners diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h index a16b56863e..68d6a6421a 100644 --- a/selfdrive/ui/qt/maps/map.h +++ b/selfdrive/ui/qt/maps/map.h @@ -24,6 +24,7 @@ #include #include "selfdrive/common/params.h" +#include "selfdrive/common/util.h" #include "cereal/messaging/messaging.h" class MapInstructions : public QWidget { @@ -105,6 +106,8 @@ private: // Position std::optional last_position; std::optional last_bearing; + FirstOrderFilter velocity_filter; + bool localizer_valid = false; // Route bool gps_ok = false;