sadmen
/
dragonpilot
mirror of https://github.com/dragonpilot-community/dragonpilot.git
1
0
Fork 0
Code Issues 64 Packages Projects Releases Wiki Activity

Various nav improvements (#21133)

Browse Source 
* various nav improvements

* use traffic aware routing

* read last position from param

* allow compute without gps when no route

* cleanup

* properly hide route

* set pitch
pull/214/head
Willem Melching 4 years ago committed by GitHub
parent 164e018d38
commit 14f09e4ee1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 145 additions and 73 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      selfdrive/common/params.cc
  2. 3
      selfdrive/ui/qt/home.cc
  3. 151
      selfdrive/ui/qt/maps/map.cc
  4. 6
      selfdrive/ui/qt/maps/map.h
  5. 20
      selfdrive/ui/qt/maps/map_helpers.cc
  6. 2
      selfdrive/ui/qt/maps/map_helpers.h

2
selfdrive/common/params.cc
Unescape View File

@ -184,7 +184,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"LastUpdateTime", PERSISTENT},
{"LiveParameters", PERSISTENT},
{"MapboxToken", PERSISTENT},
{"NavDestination", PERSISTENT}, // TODO: CLEAR_ON_MANAGER_START
{"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"OpenpilotEnabledToggle", PERSISTENT},
{"PandaFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
{"PandaFirmwareHex", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},

3
selfdrive/ui/qt/home.cc
Unescape View File

@ -50,6 +50,9 @@ void HomeWindow::offroadTransition(bool offroad) {
if (offroad) {
slayout->setCurrentWidget(home);
} else {
if (onroad->map != nullptr){
onroad->map->setVisible(!Params().get("NavDestination").empty());
}
slayout->setCurrentWidget(onroad);
}
sidebar->setVisible(offroad);

151
selfdrive/ui/qt/maps/map.cc
Unescape View File

@ -1,12 +1,11 @@
#include <cmath>
#include <QDebug>
#include <QJsonDocument>
#include <QJsonObject>
#include "selfdrive/common/util.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/params.h"
#include "selfdrive/ui/ui.h"
#include "selfdrive/ui/qt/util.h"
#include "selfdrive/ui/qt/maps/map_helpers.h"
#include "selfdrive/ui/qt/maps/map.h"
@ -52,6 +51,12 @@ MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings) {
}
connect(routing_manager, SIGNAL(finished(QGeoRouteReply*)), this, SLOT(routeCalculated(QGeoRouteReply*)));
auto last_gps_position = coordinate_from_param("LastGPSPosition");
if (last_gps_position) {
last_position = *last_gps_position;
}
grabGesture(Qt::GestureType::PinchGesture);
}
@ -99,29 +104,68 @@ void MapWindow::initLayers() {
}
void MapWindow::timerUpdate() {
if (!isVisible()) return;
initLayers();
sm->update(0);
if (sm->updated("liveLocationKalman")) {
auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman();
gps_ok = location.getGpsOK();
// Update map location, orientation and zoom on valid localizer output
if (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) {
auto pos = location.getPositionGeodetic();
float velocity = location.getVelocityCalibrated().getValue()[0];
auto orientation = location.getOrientationNED();
auto coordinate = QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]);
last_position = coordinate;
float velocity = location.getVelocityCalibrated().getValue()[0];
if (pan_counter == 0){
m_map->setCoordinate(coordinate);
m_map->setBearing(RAD2DEG(orientation.getValue()[2]));
} else {
pan_counter--;
}
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--;
}
auto coordinate = QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]);
// 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);
if (location.getStatus() == cereal::LiveLocationKalman::Status::VALID){
last_position = coordinate;
gps_ok = location.getGpsOK();
// 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 (sm->frame % 10 == 0 && shouldRecompute()){
// Recompute route if needed
if (sm->frame % 10 == 0) {
if (recompute_countdown == 0 && shouldRecompute()) {
recompute_countdown = std::pow(2, recompute_backoff);
recompute_backoff = std::min(7, recompute_backoff + 1);
calculateRoute(nav_destination);
} else {
recompute_countdown = std::max(0, recompute_countdown - 1);
}
}
// Show route instructions
if (segment.isValid()) {
auto cur_maneuver = segment.maneuver();
auto attrs = cur_maneuver.extendedAttributes();
@ -144,48 +188,30 @@ void MapWindow::timerUpdate() {
emit distanceChanged(next_maneuver_distance);
m_map->setPitch(MAX_PITCH); // TODO: smooth pitching based on maneuver distance
// Switch to next route segment
if (next_maneuver_distance < REROUTE_DISTANCE && next_maneuver_distance > last_maneuver_distance){
segment = next_segment;
recompute_backoff = std::max(0, recompute_backoff - 1);
recompute_countdown = 0;
}
last_maneuver_distance = next_maneuver_distance;
}
} else {
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();
}
}
if (pan_counter == 0){
m_map->setCoordinate(coordinate);
m_map->setBearing(RAD2DEG(orientation.getValue()[2]));
} else {
pan_counter--;
}
if (zoom_counter == 0){
m_map->setZoom(util::map_val<float>(velocity_filter.update(velocity), 0, 30, MAX_ZOOM, MIN_ZOOM));
} else {
zoom_counter--;
}
// 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);
// 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);
}
}
update();
}
if (!segment.isValid()){
map_instructions->setVisible(false);
}
@ -210,6 +236,8 @@ void MapWindow::initializeGL() {
}
void MapWindow::paintGL() {
if (!isVisible()) return;
m_map->resize(size() / MAP_SCALE);
m_map->setFramebufferObject(defaultFramebufferObject(), size());
m_map->render();
@ -218,6 +246,7 @@ void MapWindow::paintGL() {
void MapWindow::calculateRoute(QMapbox::Coordinate destination) {
QGeoRouteRequest request(to_QGeoCoordinate(last_position), to_QGeoCoordinate(destination));
request.setFeatureWeight(QGeoRouteRequest::TrafficFeature, QGeoRouteRequest::AvoidFeatureWeight);
routing_manager->calculateRoute(request);
}
@ -233,30 +262,33 @@ void MapWindow::routeCalculated(QGeoRouteReply *reply) {
navSource["type"] = "geojson";
navSource["data"] = QVariant::fromValue<QMapbox::Feature>(feature);
m_map->updateSource("navSource", navSource);
has_route = true;
m_map->setLayoutProperty("navLayer", "visibility", "visible");
}
reply->deleteLater();
}
void MapWindow::clearRoute() {
segment = QGeoRouteSegment(); // Clear route
m_map->setLayoutProperty("navLayer", "visibility", "none");
m_map->setPitch(MIN_PITCH);
}
bool MapWindow::shouldRecompute(){
if (!gps_ok) return false; // Don't recompute when gps drifts in tunnels
QString nav_destination_json = QString::fromStdString(Params().get("NavDestination"));
if (nav_destination_json.isEmpty()) return false;
bool MapWindow::shouldRecompute(){
auto new_destination = coordinate_from_param("NavDestination");
if (!new_destination) {
clearRoute();
return false;
}
QJsonDocument doc = QJsonDocument::fromJson(nav_destination_json.toUtf8());
if (doc.isNull()) return false;
if (!gps_ok && segment.isValid()) return false; // Don't recompute when gps drifts in tunnels
QJsonObject json = doc.object();
if (json["latitude"].isDouble() && json["longitude"].isDouble()){
QMapbox::Coordinate new_destination(json["latitude"].toDouble(), json["longitude"].toDouble());
if (new_destination != nav_destination){
nav_destination = new_destination;
if (*new_destination != nav_destination){
nav_destination = *new_destination;
setVisible(true); // Show map on destination set/change
return true;
}
}
if (!segment.isValid()){
return true;
@ -385,16 +417,27 @@ MapInstructions::MapInstructions(QWidget * parent) : QWidget(parent){
void MapInstructions::updateDistance(float d){
QString distance_str;
if (QUIState::ui_state.scene.is_metric) {
if (d > 500) {
distance_str.setNum(d / 1000, 'f', 1);
distance_str += " km";
} else {
distance_str.setNum(50 * int(d / 50));
distance_str += " m";
}
} else {
float miles = d * METER_2_MILE;
float feet = d * METER_2_FOOT;
if (feet > 500){
if (feet > 500) {
distance_str.setNum(miles, 'f', 1);
distance_str += " miles";
} else {
distance_str.setNum(50 * int(feet / 50));
distance_str += " feet";
}
}
distance->setText(distance_str);
}

6
selfdrive/ui/qt/maps/map.h
Unescape View File

@ -58,7 +58,6 @@ private:
// Route
bool gps_ok = false;
bool has_route = false;
QGeoServiceProvider *geoservice_provider;
QGeoRoutingManager *routing_manager;
QGeoRoute route;
@ -67,7 +66,12 @@ private:
QMapbox::Coordinate last_position = QMapbox::Coordinate(37.7393118509158, -122.46471285025565);
QMapbox::Coordinate nav_destination;
double last_maneuver_distance = 1000;
// Route recompute
int recompute_backoff = 0;
int recompute_countdown = 0;
void calculateRoute(QMapbox::Coordinate destination);
void clearRoute();
bool shouldRecompute();
private slots:

20
selfdrive/ui/qt/maps/map_helpers.cc
Unescape View File

@ -1,4 +1,8 @@
#include <QJsonDocument>
#include <QJsonObject>
#include "selfdrive/ui/qt/maps/map_helpers.h"
#include "selfdrive/common/params.h"
QGeoCoordinate to_QGeoCoordinate(const QMapbox::Coordinate &in) {
@ -84,3 +88,19 @@ float minimum_distance(QGeoCoordinate a, QGeoCoordinate b, QGeoCoordinate p) {
const QGeoCoordinate projection = add(a, mul(ab, t));
return projection.distanceTo(p);
}
std::optional<QMapbox::Coordinate> coordinate_from_param(std::string param) {
QString json_str = QString::fromStdString(Params().get(param));
if (json_str.isEmpty()) return {};
QJsonDocument doc = QJsonDocument::fromJson(json_str.toUtf8());
if (doc.isNull()) return {};
QJsonObject json = doc.object();
if (json["latitude"].isDouble() && json["longitude"].isDouble()){
QMapbox::Coordinate coord(json["latitude"].toDouble(), json["longitude"].toDouble());
return coord;
} else {
return {};
}
}

2
selfdrive/ui/qt/maps/map_helpers.h
Unescape View File

@ -1,5 +1,6 @@
#pragma once
#include <optional>
#include <eigen3/Eigen/Dense>
#include <QMapboxGL>
#include <QGeoCoordinate>
@ -21,3 +22,4 @@ QMapbox::CoordinatesCollections coordinate_to_collection(QMapbox::Coordinate c);
QMapbox::CoordinatesCollections coordinate_list_to_collection(QList<QGeoCoordinate> coordinate_list);
float minimum_distance(QGeoCoordinate a, QGeoCoordinate b, QGeoCoordinate p);
std::optional<QMapbox::Coordinate> coordinate_from_param(std::string param);

Write Preview
Loading…
Cancel
Save