You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
118 lines
3.8 KiB
118 lines
3.8 KiB
#include "selfdrive/ui/qt/maps/map_helpers.h"
|
|
|
|
#include <QJsonDocument>
|
|
#include <QJsonObject>
|
|
|
|
#include "common/params.h"
|
|
#include "selfdrive/hardware/hw.h"
|
|
#include "selfdrive/ui/qt/api.h"
|
|
|
|
QString get_mapbox_token() {
|
|
// Valid for 4 weeks since we can't swap tokens on the fly
|
|
return MAPBOX_TOKEN.isEmpty() ? CommaApi::create_jwt({}, 4 * 7 * 24 * 3600) : MAPBOX_TOKEN;
|
|
}
|
|
|
|
QMapboxGLSettings get_mapbox_settings() {
|
|
QMapboxGLSettings settings;
|
|
|
|
if (!Hardware::PC()) {
|
|
settings.setCacheDatabasePath(MAPS_CACHE_PATH);
|
|
}
|
|
settings.setApiBaseUrl(MAPS_HOST);
|
|
settings.setAccessToken(get_mapbox_token());
|
|
|
|
return settings;
|
|
}
|
|
|
|
QGeoCoordinate to_QGeoCoordinate(const QMapbox::Coordinate &in) {
|
|
return QGeoCoordinate(in.first, in.second);
|
|
}
|
|
|
|
QMapbox::CoordinatesCollections model_to_collection(
|
|
const cereal::LiveLocationKalman::Measurement::Reader &calibratedOrientationECEF,
|
|
const cereal::LiveLocationKalman::Measurement::Reader &positionECEF,
|
|
const cereal::ModelDataV2::XYZTData::Reader &line){
|
|
|
|
Eigen::Vector3d ecef(positionECEF.getValue()[0], positionECEF.getValue()[1], positionECEF.getValue()[2]);
|
|
Eigen::Vector3d orient(calibratedOrientationECEF.getValue()[0], calibratedOrientationECEF.getValue()[1], calibratedOrientationECEF.getValue()[2]);
|
|
Eigen::Matrix3d ecef_from_local = euler2rot(orient);
|
|
|
|
QMapbox::Coordinates coordinates;
|
|
auto x = line.getX();
|
|
auto y = line.getY();
|
|
auto z = line.getZ();
|
|
for (int i = 0; i < x.size(); i++) {
|
|
Eigen::Vector3d point_ecef = ecef_from_local * Eigen::Vector3d(x[i], y[i], z[i]) + ecef;
|
|
Geodetic point_geodetic = ecef2geodetic((ECEF){.x = point_ecef[0], .y = point_ecef[1], .z = point_ecef[2]});
|
|
QMapbox::Coordinate coordinate(point_geodetic.lat, point_geodetic.lon);
|
|
coordinates.push_back(coordinate);
|
|
}
|
|
|
|
QMapbox::CoordinatesCollection collection;
|
|
collection.push_back(coordinates);
|
|
|
|
QMapbox::CoordinatesCollections collections;
|
|
collections.push_back(collection);
|
|
return collections;
|
|
}
|
|
|
|
QMapbox::CoordinatesCollections coordinate_to_collection(QMapbox::Coordinate c) {
|
|
QMapbox::Coordinates coordinates;
|
|
coordinates.push_back(c);
|
|
|
|
QMapbox::CoordinatesCollection collection;
|
|
collection.push_back(coordinates);
|
|
|
|
QMapbox::CoordinatesCollections collections;
|
|
collections.push_back(collection);
|
|
return collections;
|
|
}
|
|
|
|
QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List<cereal::NavRoute::Coordinate>::Reader& coordinate_list) {
|
|
QMapbox::Coordinates coordinates;
|
|
|
|
for (auto const &c: coordinate_list) {
|
|
QMapbox::Coordinate coordinate(c.getLatitude(), c.getLongitude());
|
|
coordinates.push_back(coordinate);
|
|
}
|
|
|
|
QMapbox::CoordinatesCollection collection;
|
|
collection.push_back(coordinates);
|
|
|
|
QMapbox::CoordinatesCollections collections;
|
|
collections.push_back(collection);
|
|
return collections;
|
|
|
|
}
|
|
|
|
QMapbox::CoordinatesCollections coordinate_list_to_collection(QList<QGeoCoordinate> coordinate_list) {
|
|
QMapbox::Coordinates coordinates;
|
|
|
|
for (auto &c : coordinate_list) {
|
|
QMapbox::Coordinate coordinate(c.latitude(), c.longitude());
|
|
coordinates.push_back(coordinate);
|
|
}
|
|
|
|
QMapbox::CoordinatesCollection collection;
|
|
collection.push_back(coordinates);
|
|
|
|
QMapbox::CoordinatesCollections collections;
|
|
collections.push_back(collection);
|
|
return collections;
|
|
}
|
|
|
|
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 {};
|
|
}
|
|
}
|
|
|