mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-06-25 18:32:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

nav: various cleanup (#21521)

Browse Source 
* nav: various cleanup

* eta resize instant

* show
old-commit-hash: 9ea913bcc8
This commit is contained in:
Willem Melching
2021-07-08 14:33:58 +02:00
committed by GitHub
parent cc75e9ba9f
commit c042bbf09c
3 changed files with 87 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/common/util.h
+1
View File
@@ -127,6 +127,7 @@ public:
return x_;
}
inline void reset(float x) { x_ = x; }
inline float x(){ return x_; }
private:
float x_, k_;
selfdrive/ui/qt/maps/map.cc
+83 -94
View File
@@ -4,7 +4,6 @@
#include <QDebug>
#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;
// Update map once it's loaded
loaded_once = loaded_once || m_map->isFullyLoaded();
if (!loaded_once) {
map_instructions->showError("Map loading");
return;
}
initLayers();
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<float>(velocity_filter.x(), 0, 30, MAX_ZOOM, MIN_ZOOM));
} else {
zoom_counter--;
}
// 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<QMapbox::Feature>(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);
}
if (zoom_counter == 0) {
static FirstOrderFilter velocity_filter(velocity, 10, 0.1);
m_map->setZoom(util::map_val<float>(velocity_filter.update(velocity), 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;
// 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<QMapbox::Feature>(feature1);
m_map->updateSource("carPosSource", carPosSource);
recompute_backoff = std::max(0, recompute_backoff - 1);
recompute_countdown = 0;
} else {
qWarning() << "Destination reached";
Params().remove("NavDestination");
// 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<QMapbox::Feature>(feature2);
m_map->updateSource("modelPathSource", modelPathSource);
}
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);
}
// 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();
}
}
// 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
selfdrive/ui/qt/maps/map.h
+3
View File
@@ -24,6 +24,7 @@
#include <QPixmap>
#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<QMapbox::Coordinate> last_position;
std::optional<float> last_bearing;
FirstOrderFilter velocity_filter;
bool localizer_valid = false;
// Route
bool gps_ok = false;