mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-06-12 04:05:04 +08:00
Compare commits
12 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
672053db65 | ||
|
|
774710e95e | ||
|
|
8cdc236585 | ||
|
|
c1d3ae427b | ||
|
|
2ab45b552d | ||
|
|
8c1d59fecd | ||
|
|
cde88fd8ed | ||
|
|
4b5de0eddb | ||
|
|
071147baaf | ||
|
|
18af4d6ad6 | ||
|
|
b81d5bca3c | ||
|
|
682d738ffa |
@@ -74,7 +74,7 @@ jobs:
|
||||
env:
|
||||
GIT_SSH_COMMAND: 'ssh -o UserKnownHostsFile=~/.ssh/known_hosts'
|
||||
run: |
|
||||
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/${{ vars.MODELS_GITLAB }} gitlab_docs
|
||||
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/docs.sunnypilot.ai2.git gitlab_docs
|
||||
cd gitlab_docs
|
||||
git checkout main
|
||||
git sparse-checkout set --no-cone models/
|
||||
@@ -191,7 +191,7 @@ jobs:
|
||||
GIT_SSH_COMMAND: 'ssh -o UserKnownHostsFile=~/.ssh/known_hosts'
|
||||
run: |
|
||||
echo "Cloning GitLab"
|
||||
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/${{ vars.MODELS_GITLAB }} gitlab_docs
|
||||
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/docs.sunnypilot.ai2.git gitlab_docs
|
||||
cd gitlab_docs
|
||||
echo "checkout models/${RECOMPILED_DIR}"
|
||||
git sparse-checkout set --no-cone models/${RECOMPILED_DIR}
|
||||
|
||||
@@ -109,7 +109,7 @@ jobs:
|
||||
GIT_SSH_COMMAND: 'ssh -o UserKnownHostsFile=~/.ssh/known_hosts'
|
||||
run: |
|
||||
echo "Cloning GitLab"
|
||||
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/${{ vars.MODELS_GITLAB }} gitlab_docs
|
||||
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/docs.sunnypilot.ai2.git gitlab_docs
|
||||
cd gitlab_docs
|
||||
echo "checkout models/${RECOMPILED_DIR}"
|
||||
git sparse-checkout set --no-cone models/${RECOMPILED_DIR}
|
||||
|
||||
@@ -156,8 +156,6 @@ jobs:
|
||||
with:
|
||||
name: models-${{ env.REF }}${{ inputs.artifact_suffix }}
|
||||
path: ${{ github.workspace }}/selfdrive/modeld/models
|
||||
- run: |
|
||||
rm -f ${{ github.workspace }}/selfdrive/modeld/models/{dmonitoring_model,big_driving_policy,big_driving_vision}.onnx
|
||||
|
||||
- name: Build Model
|
||||
run: |
|
||||
|
||||
@@ -369,7 +369,6 @@ struct CarControlSP @0xa5cd762cd951a455 {
|
||||
leadOne @2 :LeadData;
|
||||
leadTwo @3 :LeadData;
|
||||
intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
|
||||
speed @5 :Float32;
|
||||
|
||||
struct Param {
|
||||
key @0 :Text;
|
||||
@@ -457,14 +456,14 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
|
||||
|
||||
struct Navigationd @0xcb9fd56c7057593a {
|
||||
upcomingTurn @0 :Text;
|
||||
currentSpeedLimit @1 :UInt16;
|
||||
currentSpeedLimit @1 :UInt64;
|
||||
bannerInstructions @2 :Text;
|
||||
distanceFromRoute @3 :Float32;
|
||||
distanceFromRoute @3 :Float64;
|
||||
allManeuvers @4 :List(Maneuver);
|
||||
valid @5 :Bool;
|
||||
|
||||
struct Maneuver {
|
||||
distance @0 :Float32;
|
||||
distance @0 :Float64;
|
||||
type @1 :Text;
|
||||
modifier @2 :Text;
|
||||
instruction @3 :Text;
|
||||
|
||||
@@ -172,6 +172,14 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"VisualRadarTracks", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"VisualRadarTracksDelay", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
|
||||
{"VisualWideCam", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"VisualStyle", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
{"VisualStyleZoom", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"VisualStyleOverhead", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"VisualStyleOverheadZoom", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"VisualStyleOverheadThreshold", {PERSISTENT | BACKUP, INT, "20"}},
|
||||
|
||||
// MADS params
|
||||
{"Mads", {PERSISTENT | BACKUP, BOOL, "1"}},
|
||||
|
||||
Submodule opendbc_repo updated: a26f7827c5...c32e79f3c6
@@ -88,7 +88,7 @@ class SelfdriveD(CruiseHelper):
|
||||
# TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches
|
||||
self.car_state_sock = messaging.sub_sock('carState', timeout=20)
|
||||
|
||||
ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ['modelDataV2SP']
|
||||
ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ['modelDataV2SP'] + ['navigationd']
|
||||
if SIMULATION:
|
||||
ignore += ['driverCameraState', 'managerState']
|
||||
if REPLAY:
|
||||
@@ -98,7 +98,7 @@ class SelfdriveD(CruiseHelper):
|
||||
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
|
||||
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
|
||||
'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback',
|
||||
'modelDataV2SP', 'longitudinalPlanSP'] + \
|
||||
'modelDataV2SP', 'longitudinalPlanSP', 'navigationd'] + \
|
||||
self.camera_packets + self.sensor_packets + self.gps_packets,
|
||||
ignore_alive=ignore, ignore_avg_freq=ignore,
|
||||
ignore_valid=ignore, frequency=int(1/DT_CTRL))
|
||||
|
||||
@@ -25,6 +25,11 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
|
||||
// update engageability/experimental mode button
|
||||
experimental_btn->updateState(s);
|
||||
dmon.updateState(s);
|
||||
if (s.scene.visual_style == 0) {
|
||||
setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
|
||||
} else {
|
||||
setBackgroundColor(QColor(0, 0, 0));
|
||||
}
|
||||
}
|
||||
|
||||
void AnnotatedCameraWidget::initializeGL() {
|
||||
@@ -35,7 +40,12 @@ void AnnotatedCameraWidget::initializeGL() {
|
||||
qInfo() << "OpenGL language version:" << QString((const char*)glGetString(GL_SHADING_LANGUAGE_VERSION));
|
||||
|
||||
prev_draw_t = millis_since_boot();
|
||||
setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
|
||||
auto *s = uiState();
|
||||
if (s->scene.visual_style == 0) {
|
||||
setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
|
||||
} else {
|
||||
setBackgroundColor(QColor(0, 0, 0));
|
||||
}
|
||||
}
|
||||
|
||||
mat4 AnnotatedCameraWidget::calcFrameMatrix() {
|
||||
@@ -118,7 +128,13 @@ void AnnotatedCameraWidget::paintGL() {
|
||||
} else if (v_ego > 15) {
|
||||
wide_cam_requested = false;
|
||||
}
|
||||
wide_cam_requested = wide_cam_requested && sm["selfdriveState"].getSelfdriveState().getExperimentalMode();
|
||||
if (s->scene.visual_wide_cam == 1) {
|
||||
wide_cam_requested = true;
|
||||
} else if (s->scene.visual_wide_cam == 2) {
|
||||
wide_cam_requested = false;
|
||||
} else {
|
||||
wide_cam_requested = wide_cam_requested && sm["selfdriveState"].getSelfdriveState().getExperimentalMode();
|
||||
}
|
||||
}
|
||||
CameraWidget::setStreamType(wide_cam_requested ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD);
|
||||
CameraWidget::setFrameId(sm["modelV2"].getModelV2().getFrameId());
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
#include "selfdrive/ui/qt/onroad/model.h"
|
||||
#include <algorithm>
|
||||
|
||||
void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) {
|
||||
auto *s = uiState();
|
||||
@@ -49,8 +50,14 @@ void ModelRenderer::update_leads(const cereal::RadarState::Reader &radar_state,
|
||||
}
|
||||
|
||||
void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead) {
|
||||
auto *s = uiState();
|
||||
const auto &model_position = model.getPosition();
|
||||
float max_distance = std::clamp(*(model_position.getX().end() - 1), MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE);
|
||||
float max_distance;
|
||||
if (s->scene.visual_style == 0) {
|
||||
max_distance = std::clamp(*(model_position.getX().end() - 1), MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE);
|
||||
} else {
|
||||
max_distance = std::clamp(*(model_position.getX().end() - 1), MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE);
|
||||
}
|
||||
|
||||
// update lane lines
|
||||
const auto &lane_lines = model.getLaneLines();
|
||||
@@ -58,7 +65,11 @@ void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const
|
||||
int max_idx = get_path_length_idx(lane_lines[0], max_distance);
|
||||
for (int i = 0; i < std::size(lane_line_vertices); i++) {
|
||||
lane_line_probs[i] = line_probs[i];
|
||||
mapLineToPolygon(lane_lines[i], 0.025 * lane_line_probs[i], 0, &lane_line_vertices[i], max_idx);
|
||||
if (s->scene.visual_style == 2) {
|
||||
mapLineToPolygon(lane_lines[i], 0.075 * lane_line_probs[i], 0, &lane_line_vertices[i], max_idx);
|
||||
} else {
|
||||
mapLineToPolygon(lane_lines[i], 0.025 * lane_line_probs[i], 0, &lane_line_vertices[i], max_idx);
|
||||
}
|
||||
}
|
||||
|
||||
// update road edges
|
||||
@@ -66,7 +77,11 @@ void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const
|
||||
const auto &edge_stds = model.getRoadEdgeStds();
|
||||
for (int i = 0; i < std::size(road_edge_vertices); i++) {
|
||||
road_edge_stds[i] = edge_stds[i];
|
||||
mapLineToPolygon(road_edges[i], 0.025, 0, &road_edge_vertices[i], max_idx);
|
||||
if (s->scene.visual_style == 2) {
|
||||
mapLineToPolygon(road_edges[i], 0.1, 0, &road_edge_vertices[i], max_idx);
|
||||
} else {
|
||||
mapLineToPolygon(road_edges[i], 0.025, 0, &road_edge_vertices[i], max_idx);
|
||||
}
|
||||
}
|
||||
|
||||
// update path
|
||||
@@ -79,16 +94,112 @@ void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const
|
||||
}
|
||||
|
||||
void ModelRenderer::drawLaneLines(QPainter &painter) {
|
||||
// lanelines
|
||||
for (int i = 0; i < std::size(lane_line_vertices); ++i) {
|
||||
painter.setBrush(QColor::fromRgbF(1.0, 1.0, 1.0, std::clamp<float>(lane_line_probs[i], 0.0, 0.7)));
|
||||
painter.drawPolygon(lane_line_vertices[i]);
|
||||
}
|
||||
auto *s = uiState();
|
||||
if (s->scene.visual_style == 2) {
|
||||
QRectF r = clip_region;
|
||||
|
||||
// road edges
|
||||
for (int i = 0; i < std::size(road_edge_vertices); ++i) {
|
||||
painter.setBrush(QColor::fromRgbF(1.0, 0, 0, std::clamp<float>(1.0 - road_edge_stds[i], 0.0, 1.0)));
|
||||
painter.drawPolygon(road_edge_vertices[i]);
|
||||
qreal horizonY = r.bottom();
|
||||
if (!road_edge_vertices[0].isEmpty() || !road_edge_vertices[1].isEmpty()) {
|
||||
qreal leftH = r.top();
|
||||
qreal rightH = r.top();
|
||||
|
||||
if (!road_edge_vertices[0].isEmpty()) {
|
||||
leftH = std::numeric_limits<qreal>::max();
|
||||
for (const QPointF &pt : road_edge_vertices[0]) {
|
||||
if (pt.y() < leftH) leftH = pt.y();
|
||||
}
|
||||
}
|
||||
|
||||
if (!road_edge_vertices[1].isEmpty()) {
|
||||
rightH = std::numeric_limits<qreal>::max();
|
||||
for (const QPointF &pt : road_edge_vertices[1]) {
|
||||
if (pt.y() < rightH) rightH = pt.y();
|
||||
}
|
||||
}
|
||||
|
||||
horizonY = std::max(leftH, rightH);
|
||||
}
|
||||
|
||||
painter.fillRect(QRectF(r.left(), horizonY + 0, r.width(), r.bottom() - (horizonY + 0)), QColor("#111111"));
|
||||
|
||||
auto buildFill = [&](const QPolygonF &edgeRibbon, bool isLeftSide) -> QPolygonF {
|
||||
if (edgeRibbon.isEmpty()) return {};
|
||||
|
||||
QMap<int, QPointF> byY;
|
||||
for (const QPointF &pt : edgeRibbon) {
|
||||
int yi = int(std::round(pt.y()));
|
||||
if (!byY.contains(yi)) {
|
||||
byY[yi] = pt;
|
||||
} else {
|
||||
if (isLeftSide) {
|
||||
if (pt.x() > byY[yi].x()) byY[yi] = pt;
|
||||
} else {
|
||||
if (pt.x() < byY[yi].x()) byY[yi] = pt;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (byY.isEmpty()) return {};
|
||||
|
||||
QPolygonF curve;
|
||||
for (auto it = byY.cbegin(); it != byY.cend(); ++it) {
|
||||
curve << it.value();
|
||||
}
|
||||
if (curve.size() < 2) return {};
|
||||
|
||||
const qreal topY = curve.first().y();
|
||||
QPolygonF fill;
|
||||
if (isLeftSide) {
|
||||
fill << QPointF(r.left(), topY);
|
||||
for (const QPointF &pt : curve) fill << pt;
|
||||
fill << QPointF(r.left(), r.bottom());
|
||||
} else {
|
||||
fill << QPointF(r.right(), topY);
|
||||
for (const QPointF &pt : curve) fill << pt;
|
||||
fill << QPointF(r.right(), r.bottom());
|
||||
}
|
||||
return fill;
|
||||
};
|
||||
|
||||
QPolygonF leftFill = buildFill(road_edge_vertices[0], true);
|
||||
QPolygonF rightFill = buildFill(road_edge_vertices[1], false);
|
||||
|
||||
if (!leftFill.isEmpty()) {
|
||||
painter.setBrush(QColor("#222222"));
|
||||
painter.drawPolygon(leftFill);
|
||||
}
|
||||
if (!rightFill.isEmpty()) {
|
||||
painter.setBrush(QColor("#222222"));
|
||||
painter.drawPolygon(rightFill);
|
||||
}
|
||||
|
||||
for (int i = 0; i < std::size(lane_line_vertices); ++i) {
|
||||
painter.setBrush(QColor::fromRgbF(0.902, 0.902, 0.902, std::clamp<float>(lane_line_probs[i], 0.0, 0.7)));
|
||||
painter.drawPolygon(lane_line_vertices[i]);
|
||||
}
|
||||
|
||||
for (int i = 0; i < std::size(road_edge_vertices); ++i) {
|
||||
painter.setBrush(QColor(0x55, 0x55, 0x55, 255));
|
||||
painter.drawPolygon(road_edge_vertices[i]);
|
||||
}
|
||||
|
||||
QLinearGradient bgGrad(r.left(), horizonY - 100, r.left(), horizonY + 100);
|
||||
bgGrad.setColorAt(0.0, QColor("#000000"));
|
||||
bgGrad.setColorAt(0.5, QColor("#111111"));
|
||||
bgGrad.setColorAt(1.0, QColor("#111111"));
|
||||
painter.fillRect(QRectF(r.left(), horizonY - 200, r.width(), 200), bgGrad);
|
||||
|
||||
} else {
|
||||
// lanelines
|
||||
for (int i = 0; i < std::size(lane_line_vertices); ++i) {
|
||||
painter.setBrush(QColor::fromRgbF(1.0, 1.0, 1.0, std::clamp<float>(lane_line_probs[i], 0.0, 0.7)));
|
||||
painter.drawPolygon(lane_line_vertices[i]);
|
||||
}
|
||||
|
||||
// road edges
|
||||
for (int i = 0; i < std::size(road_edge_vertices); ++i) {
|
||||
painter.setBrush(QColor::fromRgbF(1.0, 0, 0, std::clamp<float>(1.0 - road_edge_stds[i], 0.0, 1.0)));
|
||||
painter.drawPolygon(road_edge_vertices[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -175,6 +286,7 @@ QColor ModelRenderer::blendColors(const QColor &start, const QColor &end, float
|
||||
|
||||
void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data,
|
||||
const QPointF &vd, const QRect &surface_rect) {
|
||||
auto *s = uiState();
|
||||
const float speedBuff = 10.;
|
||||
const float leadBuff = 40.;
|
||||
const float d_rel = lead_data.getDRel();
|
||||
@@ -197,20 +309,133 @@ void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadDa
|
||||
float g_yo = sz / 10;
|
||||
|
||||
QPointF glow[] = {{x + (sz * 1.35) + g_xo, y + sz + g_yo}, {x, y - g_yo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo}};
|
||||
painter.setBrush(QColor(218, 202, 37, 255));
|
||||
if (s->scene.visual_style == 2) {
|
||||
painter.setBrush(QColor(0xE6, 0xE6, 0xE6, 255));
|
||||
} else {
|
||||
painter.setBrush(QColor(218, 202, 37, 255));
|
||||
}
|
||||
painter.drawPolygon(glow, std::size(glow));
|
||||
|
||||
// chevron
|
||||
QPointF chevron[] = {{x + (sz * 1.25), y + sz}, {x, y}, {x - (sz * 1.25), y + sz}};
|
||||
painter.setBrush(QColor(201, 34, 49, fillAlpha));
|
||||
if (s->scene.visual_style == 2) {
|
||||
painter.setBrush(QColor(0, 0, 0, fillAlpha));
|
||||
} else {
|
||||
painter.setBrush(QColor(201, 34, 49, fillAlpha));
|
||||
}
|
||||
painter.drawPolygon(chevron, std::size(chevron));
|
||||
}
|
||||
|
||||
// Projects a point in car to space to the corresponding point in full frame image space.
|
||||
float mapRange(float x, float in_min, float in_max, float out_min, float out_max) {
|
||||
if (in_min < in_max) {
|
||||
x = std::clamp(x, in_min, in_max);
|
||||
} else {
|
||||
x = std::clamp(x, in_max, in_min);
|
||||
}
|
||||
return out_min + (x - in_min) * (out_max - out_min) / (in_max - in_min);
|
||||
}
|
||||
|
||||
// Projects a point in car space to the corresponding point in full frame image space.
|
||||
bool ModelRenderer::mapToScreen(float in_x, float in_y, float in_z, QPointF *out) {
|
||||
auto *s = uiState();
|
||||
auto &sm = *(s->sm);
|
||||
float blend_speed_mph = fabsf(sm["carState"].getCarState().getVEgo() * 2.23694f);
|
||||
|
||||
Eigen::Vector3f input(in_x, in_y, in_z);
|
||||
|
||||
if ((s->scene.visual_style_zoom == 1 || s->scene.visual_style_zoom == 2) && s->scene.visual_style != 0) {
|
||||
float zoom_start = 20.0f;
|
||||
float zoom_end = 50.0f;
|
||||
|
||||
if (s->scene.visual_style_zoom == 2) {
|
||||
std::swap(zoom_start, zoom_end);
|
||||
}
|
||||
|
||||
float IN_X_OFFSET = mapRange(blend_speed_mph, zoom_start, zoom_end, 0.0f, 24.0f);
|
||||
float IN_Y_OFFSET = mapRange(blend_speed_mph, zoom_start, zoom_end, 1.0f, 2.0f);
|
||||
float IN_Z_OFFSET = mapRange(blend_speed_mph, zoom_start, zoom_end, 0.0f, 5.0f);
|
||||
float PITCH_DEG = mapRange(blend_speed_mph, zoom_start, zoom_end, 0.0f, 5.0f);
|
||||
|
||||
input = Eigen::Vector3f(in_x + IN_X_OFFSET, in_y / IN_Y_OFFSET, in_z + IN_Z_OFFSET);
|
||||
Eigen::AngleAxisf pitch_rot(PITCH_DEG * M_PI / 180.0f, Eigen::Vector3f::UnitY());
|
||||
input = pitch_rot * input;
|
||||
}
|
||||
|
||||
auto pt = car_space_transform * input;
|
||||
*out = QPointF(pt.x() / pt.z(), pt.y() / pt.z());
|
||||
bool normal_valid = (pt.z() > 1e-3f &&
|
||||
std::isfinite(pt.x()) && std::isfinite(pt.y()));
|
||||
QPointF normal_view;
|
||||
if (normal_valid) {
|
||||
normal_view = QPointF(pt.x() / pt.z(), pt.y() / pt.z());
|
||||
}
|
||||
|
||||
const float base_scale_x = 20.0f;
|
||||
const float base_scale_y = 15.0f;
|
||||
const float y_offset = 450.0f;
|
||||
|
||||
float factor_scale_x = 0.0f;
|
||||
if (blend_speed_mph > 0.0f) {
|
||||
if (s->scene.visual_style_overhead_zoom == 1) {
|
||||
factor_scale_x = mapRange(blend_speed_mph, 0.0f, 50.0f, 30.0f, 0.0f);
|
||||
} else if (s->scene.visual_style_overhead_zoom == 2) {
|
||||
factor_scale_x = mapRange(blend_speed_mph, 50.0f, 0.0f, 30.0f, 0.0f);
|
||||
}
|
||||
}
|
||||
|
||||
float scale_x = base_scale_x + factor_scale_x;
|
||||
float scale_y = base_scale_y;
|
||||
|
||||
QPointF topdown_view(
|
||||
clip_region.center().x() + in_y * scale_x,
|
||||
(clip_region.bottom() - y_offset) - in_x * scale_y
|
||||
);
|
||||
|
||||
if ((s->scene.visual_style_overhead == 1 || s->scene.visual_style_overhead == 2) && s->scene.visual_style != 0) {
|
||||
static float blend = 0.0f;
|
||||
static float target_blend = 0.0f;
|
||||
static double last_t = millis_since_boot();
|
||||
|
||||
const bool inverted = (s->scene.visual_style_overhead == 2);
|
||||
const float threshold = s->scene.visual_style_overhead_threshold;
|
||||
const float hysteresis = 5.0f;
|
||||
|
||||
if (!inverted) {
|
||||
if (target_blend < 0.5f && blend_speed_mph > threshold) {
|
||||
target_blend = 1.0f;
|
||||
} else if (target_blend > 0.5f && blend_speed_mph < threshold - hysteresis) {
|
||||
target_blend = 0.0f;
|
||||
}
|
||||
} else {
|
||||
if (target_blend < 0.5f && blend_speed_mph < threshold) {
|
||||
target_blend = 1.0f;
|
||||
} else if (target_blend > 0.5f && blend_speed_mph > threshold + hysteresis) {
|
||||
target_blend = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
double now = millis_since_boot();
|
||||
double dt = (now - last_t) / 1000.0;
|
||||
last_t = now;
|
||||
|
||||
const float transition_time = 1.50f;
|
||||
float step = dt / transition_time;
|
||||
|
||||
if (blend < target_blend) {
|
||||
blend = std::min(blend + step, target_blend);
|
||||
} else if (blend > target_blend) {
|
||||
blend = std::max(blend - step, target_blend);
|
||||
}
|
||||
|
||||
if (!normal_valid) return false;
|
||||
*out = QPointF(
|
||||
(1 - blend) * normal_view.x() + blend * topdown_view.x(),
|
||||
(1 - blend) * normal_view.y() + blend * topdown_view.y()
|
||||
);
|
||||
} else {
|
||||
if (!normal_valid) return false;
|
||||
*out = normal_view;
|
||||
}
|
||||
|
||||
return clip_region.contains(*out);
|
||||
}
|
||||
|
||||
|
||||
@@ -196,6 +196,7 @@ mat4 CameraWidget::calcFrameMatrix() {
|
||||
}
|
||||
|
||||
void CameraWidget::paintGL() {
|
||||
auto *s = uiState();
|
||||
glClearColor(bg.redF(), bg.greenF(), bg.blueF(), bg.alphaF());
|
||||
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
|
||||
|
||||
@@ -248,7 +249,9 @@ void CameraWidget::paintGL() {
|
||||
|
||||
glUniformMatrix4fv(program->uniformLocation("uTransform"), 1, GL_TRUE, frame_mat.v);
|
||||
glEnableVertexAttribArray(0);
|
||||
glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void *)0);
|
||||
if (s->scene.visual_style == 0) {
|
||||
glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void *)0);
|
||||
}
|
||||
glDisableVertexAttribArray(0);
|
||||
glBindVertexArray(0);
|
||||
glBindTexture(GL_TEXTURE_2D, 0);
|
||||
|
||||
@@ -36,7 +36,7 @@ DisplayPanel::DisplayPanel(QWidget *parent) : QWidget(parent) {
|
||||
interactivityTimeout = new OptionControlSP("InteractivityTimeout", tr("Interactivity Timeout"),
|
||||
tr("Apply a custom timeout for settings UI."
|
||||
"\nThis is the time after which settings UI closes automatically if user is not interacting with the screen."),
|
||||
"", {0, 120}, 10, true, nullptr, false);
|
||||
"", {999999, 1000000}, 1000000, true, nullptr, false);
|
||||
|
||||
connect(interactivityTimeout, &OptionControlSP::updateLabels, [=]() {
|
||||
refresh();
|
||||
|
||||
@@ -11,6 +11,18 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
|
||||
param_watcher = new ParamWatcher(this);
|
||||
connect(param_watcher, &ParamWatcher::paramChanged, [=](const QString ¶m_name, const QString ¶m_value) {
|
||||
paramsRefresh();
|
||||
if (param_name == "VisualStyle") {
|
||||
visual_style_value = param_value.toInt();
|
||||
} else if (param_name == "VisualStyleOverhead") {
|
||||
visual_style_overhead_value = param_value.toInt();
|
||||
} else if (param_name == "VisualRadarTracks") {
|
||||
bool radar_tracks_enabled = param_value.toInt() != 0;
|
||||
visual_radar_tracks_delay_settings->setVisible(radar_tracks_enabled);
|
||||
}
|
||||
visual_style_zoom_settings->setVisible(visual_style_value != 0);
|
||||
visual_style_overhead_settings->setVisible(visual_style_value != 0);
|
||||
visual_style_overhead_zoom_settings->setVisible(visual_style_value != 0 && visual_style_overhead_value != 0);
|
||||
visual_style_overhead_threshold_settings->setVisible(visual_style_value != 0 && visual_style_overhead_value != 0);
|
||||
});
|
||||
|
||||
main_layout = new QStackedLayout(this);
|
||||
@@ -90,6 +102,13 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
|
||||
"",
|
||||
false,
|
||||
},
|
||||
{
|
||||
"VisualRadarTracks",
|
||||
tr("Show Radar Tracks"),
|
||||
tr("Shows what the cars radar sees."),
|
||||
"",
|
||||
false,
|
||||
},
|
||||
};
|
||||
|
||||
// Add regular toggles first
|
||||
@@ -116,6 +135,111 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
|
||||
param_watcher->addParam(param);
|
||||
}
|
||||
|
||||
// Visuals: Radar Tracks Delay
|
||||
visual_radar_tracks_delay_settings = new OptionControlSP("VisualRadarTracksDelay", tr("Adjust Visual Radar Tracks Delay"),
|
||||
tr("Delays radar tracks to better match what you see through the camera."),
|
||||
"", {0, 100}, 10, false, nullptr, true);
|
||||
|
||||
connect(visual_radar_tracks_delay_settings, &OptionControlSP::updateLabels, [=]() {
|
||||
float radar_tracks_delay_value = QString::fromStdString(params.get("VisualRadarTracksDelay")).toFloat();
|
||||
visual_radar_tracks_delay_settings->setLabel(QString::number(radar_tracks_delay_value, 'f', 1) + " s");
|
||||
});
|
||||
|
||||
float radar_tracks_delay_value = QString::fromStdString(params.get("VisualRadarTracksDelay")).toFloat();
|
||||
visual_radar_tracks_delay_settings->setLabel(QString::number(radar_tracks_delay_value, 'f', 1) + " s");
|
||||
|
||||
list->addItem(visual_radar_tracks_delay_settings);
|
||||
|
||||
// Wide Cam
|
||||
std::vector<QString> visual_wide_cam_settings_texts{tr("Auto"), tr("On"), tr("Off")};
|
||||
visual_wide_cam_settings = new ButtonParamControlSP(
|
||||
"VisualWideCam", tr("Wide Cam"), tr("Override the wide cam view regardless of experimental mode status."),
|
||||
"",
|
||||
visual_wide_cam_settings_texts,
|
||||
250);
|
||||
list->addItem(visual_wide_cam_settings);
|
||||
|
||||
// Visual Style
|
||||
std::vector<QString> visual_style_settings_texts{tr("Default"), tr("Minimal"), tr("Vision")};
|
||||
visual_style_settings = new ButtonParamControlSP(
|
||||
"VisualStyle", tr("Visual Style"),
|
||||
tr(
|
||||
"Switch between different on-road visualization layouts."
|
||||
"<ul style='margin-left: 10px; margin-top: 4px;'>"
|
||||
"<li><b>Default:</b> Standard OpenPilot layout with camera and path view.</li>"
|
||||
"<li><b>Minimal:</b> Clean interface without camera feed or extra elements.</li>"
|
||||
"<li><b>Vision:</b> Experimental layout that focuses on model perception and environment.</li>"
|
||||
"</ul>"
|
||||
),
|
||||
"",
|
||||
visual_style_settings_texts,
|
||||
380);
|
||||
list->addItem(visual_style_settings);
|
||||
|
||||
// Visual Style Zoom
|
||||
std::vector<QString> visual_style_zoom_settings_texts{tr("Disabled"), tr("Enabled"), tr("Inverted")};
|
||||
visual_style_zoom_settings = new ButtonParamControlSP(
|
||||
"VisualStyleZoom", tr("Visual Style Zoom"),
|
||||
tr(
|
||||
"Enables dynamic zooming based on driving speed in the selected visual style."
|
||||
"<ul style='margin-left: 10px; margin-top: 4px;'>"
|
||||
"<li><b>Disabled:</b> Keeps the zoom fixed.</li>"
|
||||
"<li><b>Enabled:</b> Zooms in at low speed and out at high speed.</li>"
|
||||
"<li><b>Inverted:</b> Reverses the zoom behavior.</li>"
|
||||
"</ul>"
|
||||
),
|
||||
"",
|
||||
visual_style_zoom_settings_texts,
|
||||
380);
|
||||
list->addItem(visual_style_zoom_settings);
|
||||
|
||||
// Visual Style Overhead
|
||||
std::vector<QString> visual_style_overhead_settings_texts{tr("Disabled"), tr("Enabled"), tr("Inverted")};
|
||||
visual_style_overhead_settings = new ButtonParamControlSP(
|
||||
"VisualStyleOverhead", tr("Visual Style Overhead"),
|
||||
tr(
|
||||
"Toggles an overhead (top-down) camera view for a 2D-style perspective."
|
||||
"<ul style='margin-left: 10px; margin-top: 4px;'>"
|
||||
"<li><b>Disabled:</b> Keeps the standard forward 3D view.</li>"
|
||||
"<li><b>Enabled:</b> Switches to overhead view when active.</li>"
|
||||
"<li><b>Inverted:</b> Reverses when the transition happens.</li>"
|
||||
"</ul>"
|
||||
),
|
||||
"",
|
||||
visual_style_overhead_settings_texts,
|
||||
380);
|
||||
list->addItem(visual_style_overhead_settings);
|
||||
|
||||
// Visual Style Overhead Zoom
|
||||
std::vector<QString> visual_style_overhead_zoom_settings_texts{tr("Disabled"), tr("Enabled"), tr("Inverted")};
|
||||
visual_style_overhead_zoom_settings = new ButtonParamControlSP(
|
||||
"VisualStyleOverheadZoom", tr("Visual Style Overhead Zoom"),
|
||||
tr(
|
||||
"Controls zooming behavior while in overhead mode."
|
||||
"<ul style='margin-left: 10px; margin-top: 4px;'>"
|
||||
"<li><b>Disabled:</b> Keeps a fixed zoom level in overhead mode.</li>"
|
||||
"<li><b>Enabled:</b> Zooms dynamically based on speed while overhead.</li>"
|
||||
"<li><b>Inverted:</b> Opposite zoom direction.</li>"
|
||||
"</ul>"
|
||||
),
|
||||
"",
|
||||
visual_style_overhead_zoom_settings_texts,
|
||||
380);
|
||||
list->addItem(visual_style_overhead_zoom_settings);
|
||||
|
||||
// Visual Style Overhead Threshold
|
||||
visual_style_overhead_threshold_settings = new OptionControlSP(
|
||||
"VisualStyleOverheadThreshold", tr("Visual Style Overhead Threshold"),
|
||||
tr("Sets the speed (in mph) where the display transitions between normal and overhead view."),
|
||||
"", {10, 80}, 5, false, nullptr, false);
|
||||
auto updateThresholdLabel = [=]() {
|
||||
int mph = QString::fromStdString(params.get("VisualStyleOverheadThreshold")).toInt();
|
||||
visual_style_overhead_threshold_settings->setLabel(QString("%1 mph").arg(mph));
|
||||
};
|
||||
connect(visual_style_overhead_threshold_settings, &OptionControlSP::updateLabels, updateThresholdLabel);
|
||||
updateThresholdLabel();
|
||||
list->addItem(visual_style_overhead_threshold_settings);
|
||||
|
||||
// Visuals: Display Metrics below Chevron
|
||||
std::vector<QString> chevron_info_settings_texts{tr("Off"), tr("Distance"), tr("Speed"), tr("Time"), tr("All")};
|
||||
chevron_info_settings = new ButtonParamControlSP(
|
||||
@@ -136,6 +260,19 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
|
||||
380);
|
||||
list->addItem(dev_ui_settings);
|
||||
|
||||
bool radar_tracks_enabled = QString::fromStdString(params.get("VisualRadarTracks")).toInt() != 0;
|
||||
visual_radar_tracks_delay_settings->setVisible(radar_tracks_enabled);
|
||||
param_watcher->addParam("VisualRadarTracks");
|
||||
|
||||
visual_style_value = QString::fromStdString(params.get("VisualStyle")).toInt();
|
||||
visual_style_overhead_value = QString::fromStdString(params.get("VisualStyleOverhead")).toInt();
|
||||
visual_style_zoom_settings->setVisible(visual_style_value != 0);
|
||||
visual_style_overhead_settings->setVisible(visual_style_value != 0);
|
||||
visual_style_overhead_zoom_settings->setVisible(visual_style_value != 0 && visual_style_overhead_value != 0);
|
||||
visual_style_overhead_threshold_settings->setVisible(visual_style_value != 0 && visual_style_overhead_value != 0);
|
||||
param_watcher->addParam("VisualStyle");
|
||||
param_watcher->addParam("VisualStyleOverhead");
|
||||
|
||||
sunnypilotScroller = new ScrollViewSP(list, this);
|
||||
vlayout->addWidget(sunnypilotScroller);
|
||||
|
||||
@@ -191,4 +328,19 @@ void VisualsPanel::paramsRefresh() {
|
||||
if (dev_ui_settings) {
|
||||
dev_ui_settings->refresh();
|
||||
}
|
||||
if (visual_wide_cam_settings) {
|
||||
visual_wide_cam_settings->refresh();
|
||||
}
|
||||
if (visual_style_settings) {
|
||||
visual_style_settings->refresh();
|
||||
}
|
||||
if (visual_style_zoom_settings) {
|
||||
visual_style_zoom_settings->refresh();
|
||||
}
|
||||
if (visual_style_overhead_settings) {
|
||||
visual_style_overhead_settings->refresh();
|
||||
}
|
||||
if (visual_style_overhead_zoom_settings) {
|
||||
visual_style_overhead_zoom_settings->refresh();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -32,4 +32,14 @@ protected:
|
||||
ButtonParamControlSP *dev_ui_settings;
|
||||
|
||||
bool has_longitudinal_control = false;
|
||||
|
||||
OptionControlSP *visual_radar_tracks_delay_settings;
|
||||
ButtonParamControlSP *visual_wide_cam_settings;
|
||||
int visual_style_value = 0;
|
||||
int visual_style_overhead_value = 0;
|
||||
ButtonParamControlSP *visual_style_settings;
|
||||
ButtonParamControlSP *visual_style_zoom_settings;
|
||||
ButtonParamControlSP *visual_style_overhead_settings;
|
||||
ButtonParamControlSP *visual_style_overhead_zoom_settings;
|
||||
OptionControlSP *visual_style_overhead_threshold_settings;
|
||||
};
|
||||
|
||||
@@ -176,12 +176,7 @@ void HudRendererSP::updateState(const UIState &s) {
|
||||
navigationDistance = QString::number(std::round(dist / 50.0) * 50) + " m";
|
||||
}
|
||||
} else {
|
||||
float dist_km = dist / 1000;
|
||||
if (dist_km >= 10.0) {
|
||||
navigationDistance = QString::number(std::floor(dist_km)) + " km";
|
||||
} else {
|
||||
navigationDistance = QString::number(dist_km, 'f', 1) + " km";
|
||||
}
|
||||
navigationDistance = QString::number(dist / 1000, 'f', 1) + " km";
|
||||
}
|
||||
} else {
|
||||
float dist_ft = dist * 3.28084f;
|
||||
@@ -193,12 +188,7 @@ void HudRendererSP::updateState(const UIState &s) {
|
||||
navigationDistance = QString::number((std::round(dist_ft / 50.0) * 50)) + " ft";
|
||||
}
|
||||
} else {
|
||||
float dist_mi = dist_ft / 5280;
|
||||
if (dist_mi >= 10.0) {
|
||||
navigationDistance = QString::number(std::floor(dist_mi)) + " mi";
|
||||
} else {
|
||||
navigationDistance = QString::number(dist_mi, 'f', 1) + " mi";
|
||||
}
|
||||
navigationDistance = QString::number(dist_ft / 5280, 'f', 1) + " mi";
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1096,15 +1086,10 @@ void HudRendererSP::drawNavigationHUD(QPainter &p, const QRect &surface_rect) {
|
||||
|
||||
const int container_width = 1080;
|
||||
const int container_height = 225;
|
||||
int container_x = (surface_rect.width() - container_width) / 2;
|
||||
const int container_x = (surface_rect.width() - container_width) / 2;
|
||||
const int container_y = 62;
|
||||
const int border_radius = 42;
|
||||
|
||||
|
||||
if (speedLimitAssistState == cereal::LongitudinalPlanSP::SpeedLimit::AssistState::PRE_ACTIVE) {
|
||||
container_x += 190;
|
||||
}
|
||||
|
||||
QRect container_rect(container_x, container_y, container_width, container_height);
|
||||
|
||||
p.setPen(Qt::NoPen);
|
||||
@@ -1127,7 +1112,7 @@ void HudRendererSP::drawNavigationHUD(QPainter &p, const QRect &surface_rect) {
|
||||
// Distance
|
||||
p.setFont(InterFont(48, QFont::Bold));
|
||||
p.setPen(Qt::white);
|
||||
QRect distance_rect(icon_x - 25, icon_y + icon_size, icon_size + 50, 38);
|
||||
QRect distance_rect(icon_x, icon_y + icon_size, icon_size, 38);
|
||||
p.drawText(distance_rect, Qt::AlignCenter, navigationDistance);
|
||||
|
||||
const int then_section_width = 180;
|
||||
|
||||
@@ -8,6 +8,12 @@
|
||||
#include "selfdrive/ui/sunnypilot/qt/onroad/model.h"
|
||||
|
||||
|
||||
void ModelRendererSP::drawRadarPoint(QPainter &painter, const QPointF &pos, float v_rel, float radius) {
|
||||
painter.setBrush(QColor(255, 255, 255, 200));
|
||||
painter.setPen(Qt::NoPen);
|
||||
painter.drawEllipse(pos, radius, radius);
|
||||
}
|
||||
|
||||
void ModelRendererSP::update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead) {
|
||||
ModelRenderer::update_model(model, lead);
|
||||
const auto &model_position = model.getPosition();
|
||||
@@ -67,6 +73,26 @@ void ModelRendererSP::draw(QPainter &painter, const QRect &surface_rect) {
|
||||
const bool right_blindspot = car_state.getRightBlindspot();
|
||||
drawBlindspot(painter, surface_rect, left_blindspot, right_blindspot);
|
||||
}
|
||||
|
||||
if (s->scene.visual_radar_tracks) {
|
||||
if (sm.alive("liveTracks") && sm.rcv_frame("liveTracks") >= s->scene.started_frame) {
|
||||
const auto &tracks = sm["liveTracks"].getLiveTracks().getPoints();
|
||||
for (const auto &track : tracks) {
|
||||
if (!std::isfinite(track.getDRel()) || !std::isfinite(track.getYRel())) continue;
|
||||
float t_lag = s->scene.visual_radar_tracks_delay;
|
||||
float d_pred = track.getDRel();
|
||||
float y_pred = track.getYRel();
|
||||
if (t_lag > 0.0f) {
|
||||
d_pred += track.getVRel() * t_lag + 0.5f * track.getARel() * t_lag * t_lag;
|
||||
}
|
||||
QPointF screen_pt;
|
||||
if (mapToScreen(d_pred, -y_pred, path_offset_z, &screen_pt)) {
|
||||
drawRadarPoint(painter, screen_pt, track.getVRel(), 10.0f);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
drawLeadStatus(painter, surface_rect.height(), surface_rect.width());
|
||||
|
||||
painter.restore();
|
||||
|
||||
@@ -28,4 +28,6 @@ private:
|
||||
|
||||
// Lead status animation
|
||||
float lead_status_alpha = 0.0f;
|
||||
|
||||
void drawRadarPoint(QPainter &painter, const QPointF &pos, float v_rel, float radius = 10.0f);
|
||||
};
|
||||
|
||||
@@ -29,7 +29,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) {
|
||||
"wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
|
||||
"modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
|
||||
"carControl", "gpsLocationExternal", "gpsLocation", "liveTorqueParameters",
|
||||
"carStateSP", "liveParameters", "liveMapDataSP", "carParamsSP", "navigationd"
|
||||
"carStateSP", "liveParameters", "liveMapDataSP", "carParamsSP", "navigationd", "liveTracks"
|
||||
});
|
||||
|
||||
// update timer
|
||||
@@ -44,6 +44,14 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) {
|
||||
});
|
||||
param_watcher->addParam("DevUIInfo");
|
||||
param_watcher->addParam("StandstillTimer");
|
||||
param_watcher->addParam("VisualRadarTracks");
|
||||
param_watcher->addParam("VisualRadarTracksDelay");
|
||||
param_watcher->addParam("VisualWideCam");
|
||||
param_watcher->addParam("VisualStyle");
|
||||
param_watcher->addParam("VisualStyleZoom");
|
||||
param_watcher->addParam("VisualStyleOverhead");
|
||||
param_watcher->addParam("VisualStyleOverheadZoom");
|
||||
param_watcher->addParam("VisualStyleOverheadThreshold");
|
||||
}
|
||||
|
||||
// This method overrides completely the update method from the parent class intentionally.
|
||||
@@ -76,6 +84,17 @@ void ui_update_params_sp(UIStateSP *s) {
|
||||
s->scene.chevron_info = std::atoi(params.get("ChevronInfo").c_str());
|
||||
s->scene.blindspot_ui = params.getBool("BlindSpot");
|
||||
s->scene.rainbow_mode = params.getBool("RainbowMode");
|
||||
|
||||
s->scene.visual_radar_tracks = QString::fromStdString(params.get("VisualRadarTracks")).toInt();
|
||||
s->scene.visual_radar_tracks_delay = QString::fromStdString(params.get("VisualRadarTracksDelay")).toFloat();
|
||||
|
||||
s->scene.visual_wide_cam = QString::fromStdString(params.get("VisualWideCam")).toInt();
|
||||
|
||||
s->scene.visual_style = QString::fromStdString(params.get("VisualStyle")).toInt();
|
||||
s->scene.visual_style_zoom = QString::fromStdString(params.get("VisualStyleZoom")).toInt();
|
||||
s->scene.visual_style_overhead = QString::fromStdString(params.get("VisualStyleOverhead")).toInt();
|
||||
s->scene.visual_style_overhead_zoom = QString::fromStdString(params.get("VisualStyleOverheadZoom")).toInt();
|
||||
s->scene.visual_style_overhead_threshold = QString::fromStdString(params.get("VisualStyleOverheadThreshold")).toInt();
|
||||
}
|
||||
|
||||
void UIStateSP::reset_onroad_sleep_timer(OnroadTimerStatusToggle toggleTimerStatus) {
|
||||
|
||||
@@ -21,4 +21,12 @@ typedef struct UISceneSP : UIScene {
|
||||
int chevron_info;
|
||||
bool blindspot_ui;
|
||||
bool rainbow_mode;
|
||||
int visual_radar_tracks = 0;
|
||||
float visual_radar_tracks_delay = 0;
|
||||
int visual_wide_cam = 0;
|
||||
int visual_style = 0;
|
||||
int visual_style_zoom = 0;
|
||||
int visual_style_overhead = 0;
|
||||
int visual_style_overhead_zoom = 0;
|
||||
int visual_style_overhead_threshold = 20.0;
|
||||
} UISceneSP;
|
||||
|
||||
@@ -72,15 +72,6 @@ class Coordinate:
|
||||
return x * EARTH_MEAN_RADIUS
|
||||
|
||||
|
||||
def bearing_between_two_points(point_one: Coordinate, point_two: Coordinate) -> float:
|
||||
dlon = math.radians(point_two.longitude - point_one.longitude)
|
||||
bearing_radians = math.atan2(math.sin(dlon)* math.cos(point_two.latitude), math.cos(point_one.latitude) * math.sin(point_two.latitude) -
|
||||
math.sin(point_one.latitude) * math.cos(point_two.latitude) * math.cos(dlon))
|
||||
bearing_degrees = math.degrees(bearing_radians)
|
||||
bearing_normalized = (bearing_degrees + 360) % 360
|
||||
return bearing_normalized
|
||||
|
||||
|
||||
def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate):
|
||||
if a.distance_to(b) < 0.01:
|
||||
return a.distance_to(p)
|
||||
|
||||
@@ -31,12 +31,14 @@ class NavigationDesires:
|
||||
self.desire = log.Desire.none
|
||||
if self.nav_allowed and nav_msg.valid and lateral_active:
|
||||
upcoming = nav_msg.upcomingTurn
|
||||
if upcoming == 'slightLeft' and not CS.rightBlinker and not CS.leftBlindspot and CS.steeringPressed and CS.steeringTorque > 0:
|
||||
if upcoming == 'slightLeft' and (not CS.leftBlindspot or CS.vEgo < self._turn_speed_limit):
|
||||
self.desire = log.Desire.keepLeft
|
||||
elif upcoming == 'slightRight' and not CS.leftBlinker and not CS.rightBlindspot and CS.steeringPressed and CS.steeringTorque < 0:
|
||||
elif upcoming == 'slightRight' and (not CS.rightBlindspot or CS.vEgo < self._turn_speed_limit):
|
||||
self.desire = log.Desire.keepRight
|
||||
elif upcoming == 'left' and not CS.rightBlinker and not CS.leftBlindspot and CS.vEgo < self._turn_speed_limit:
|
||||
elif (upcoming == 'left' and CS.steeringPressed and CS.steeringTorque > 0 and not CS.rightBlinker
|
||||
and not CS.leftBlindspot and CS.vEgo < self._turn_speed_limit):
|
||||
self.desire = log.Desire.turnLeft
|
||||
elif upcoming == 'right' and not CS.leftBlinker and not CS.rightBlindspot and CS.vEgo < self._turn_speed_limit:
|
||||
elif (upcoming == 'right' and CS.steeringPressed and CS.steeringTorque < 0 and not CS.leftBlinker
|
||||
and not CS.rightBlindspot and CS.vEgo < self._turn_speed_limit):
|
||||
self.desire = log.Desire.turnRight
|
||||
return self.desire
|
||||
|
||||
@@ -22,20 +22,20 @@ def make_car(vEgo=0, leftBlinker=False, rightBlinker=False, leftBlindspot=False,
|
||||
)
|
||||
|
||||
NAVIGATION_PARAMS: list[tuple] = [
|
||||
('slightLeft', make_car(steeringPressed=True, steeringTorque=1), log.Desire.keepLeft),
|
||||
('slightRight', make_car(steeringPressed=True, steeringTorque=-1), log.Desire.keepRight),
|
||||
('slightLeft', make_car(), log.Desire.keepLeft),
|
||||
('slightRight', make_car(), log.Desire.keepRight),
|
||||
('slightLeft', make_car(vEgo=9, leftBlindspot=True), log.Desire.none),
|
||||
('slightRight', make_car(vEgo=9, rightBlindspot=True), log.Desire.none),
|
||||
('left', make_car(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False), log.Desire.turnLeft),
|
||||
('left', make_car(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False, steeringPressed=True, steeringTorque=1), log.Desire.turnLeft),
|
||||
('left', make_car(vEgo=5, leftBlinker=False, rightBlinker=True), log.Desire.none),
|
||||
('right', make_car(vEgo=6, rightBlinker=True, leftBlindspot=False), log.Desire.turnRight),
|
||||
('right', make_car(vEgo=6, rightBlinker=True, leftBlindspot=False, steeringPressed=True, steeringTorque=-1), log.Desire.turnRight),
|
||||
('right', make_car(vEgo=6, rightBlinker=True, rightBlindspot=True), log.Desire.none),
|
||||
('left', make_car(vEgo=9, leftBlinker=True), log.Desire.none),
|
||||
]
|
||||
|
||||
INTEGRATION_PARAMS: list[tuple] = [(carstate, upcoming, log.Desire.none, expected) for upcoming, carstate, expected in NAVIGATION_PARAMS] + [
|
||||
(make_car(vEgo=6, leftBlinker=True, steeringPressed=True, steeringTorque=1), 'slightLeft', log.Desire.turnLeft, log.Desire.keepLeft),
|
||||
(make_car(vEgo=5, rightBlinker=True, steeringPressed=True, steeringTorque=-1), 'slightRight', log.Desire.turnRight, log.Desire.keepRight),
|
||||
(make_car(vEgo=9, leftBlinker=True, steeringPressed=True, steeringTorque=1), 'slightLeft', log.Desire.turnLeft, log.Desire.keepLeft),
|
||||
(make_car(vEgo=9, rightBlinker=True, steeringPressed=True, steeringTorque=-1), 'slightRight', log.Desire.turnRight, log.Desire.keepRight),
|
||||
(make_car(vEgo=9, leftBlinker=True), 'slightLeft', log.Desire.laneChangeLeft, log.Desire.laneChangeLeft),
|
||||
(make_car(vEgo=9, rightBlinker=True), 'slightRight', log.Desire.laneChangeRight, log.Desire.laneChangeRight),
|
||||
(make_car(vEgo=9), 'none', log.Desire.none, log.Desire.none),
|
||||
|
||||
@@ -4,25 +4,20 @@ Copyright (c) 2021-, James Vecellio, Haibin Wen, sunnypilot, and a number of oth
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from numpy import interp
|
||||
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.params import Params
|
||||
|
||||
from openpilot.sunnypilot.navd.helpers import Coordinate, bearing_between_two_points, string_to_direction
|
||||
from openpilot.sunnypilot.navd.helpers import Coordinate, string_to_direction
|
||||
|
||||
|
||||
class NavigationInstructions:
|
||||
def __init__(self):
|
||||
self.coord = Coordinate(0, 0)
|
||||
self.params = Params()
|
||||
|
||||
self._cached_route = None
|
||||
self._route_loaded = False
|
||||
self._no_route = False
|
||||
|
||||
self.closest_idx: float = 0
|
||||
self.min_distance: float = 0
|
||||
|
||||
def get_route_progress(self, current_lat, current_lon) -> dict | None:
|
||||
route = self.get_current_route()
|
||||
if not route or not route['geometry'] or not route['steps']:
|
||||
@@ -32,8 +27,8 @@ class NavigationInstructions:
|
||||
self.coord.longitude = current_lon
|
||||
|
||||
# Find the closest point on the route relative to self
|
||||
self.closest_idx, self.min_distance = min(((idx, self.coord.distance_to(coord)) for idx, coord in enumerate(route['geometry'])), key=lambda x: x[1])
|
||||
closest_cumulative = route['cumulative_distances'][self.closest_idx]
|
||||
closest_idx, min_distance = min(((idx, self.coord.distance_to(coord)) for idx, coord in enumerate(route['geometry'])), key=lambda x: x[1])
|
||||
closest_cumulative = route['cumulative_distances'][closest_idx]
|
||||
|
||||
# Find the current step index, which is the HIGHEST idx where the step location cumulative less/equal closest cumulative
|
||||
current_step_idx = max((idx for idx, step in enumerate(route['steps']) if step['cumulative_distance'] <= closest_cumulative), default=-1)
|
||||
@@ -58,7 +53,7 @@ class NavigationInstructions:
|
||||
all_maneuvers.append({'distance': distance, 'type': step['maneuver'], 'modifier': step['modifier'], 'instruction': step['instruction']})
|
||||
|
||||
return {
|
||||
'distance_from_route': self.min_distance,
|
||||
'distance_from_route': min_distance,
|
||||
'current_step': current_step,
|
||||
'next_turn': next_turn,
|
||||
'current_maxspeed': current_maxspeed,
|
||||
@@ -114,41 +109,30 @@ class NavigationInstructions:
|
||||
self._route_loaded = False
|
||||
self._no_route = False
|
||||
|
||||
def route_bearing_misalign(self, route, bearing, v_ego) -> bool:
|
||||
route_bearing_misalign:bool = False
|
||||
|
||||
if v_ego < 5.0:
|
||||
route_bearing_misalign = False
|
||||
elif self.closest_idx > 0 and self.closest_idx < len(route['geometry']) -1:
|
||||
current_coord = route['geometry'][self.closest_idx - 1]
|
||||
future_coord = route['geometry'][self.closest_idx + 1]
|
||||
route_bearing = bearing_between_two_points(current_coord, future_coord)
|
||||
current_bearing_normalized = (bearing + 360) % 360
|
||||
bearing_difference = abs(current_bearing_normalized - route_bearing)
|
||||
|
||||
if min(bearing_difference, 360 - bearing_difference) > 91:
|
||||
route_bearing_misalign = True # flag for recompute/cancellation
|
||||
return route_bearing_misalign
|
||||
|
||||
def get_upcoming_turn_from_progress(self, progress, current_lat, current_lon, v_ego: float) -> str:
|
||||
def get_upcoming_turn_from_progress(self, progress, current_lat, current_lon) -> str:
|
||||
if progress and progress['next_turn']:
|
||||
speed_breakpoints: list = [0, 5, 10, 15, 20, 25, 30, 35, 40]
|
||||
distance_breakpoints: list = [20, 25, 30, 45, 60, 75, 90, 105, 120]
|
||||
distance_interp = interp(v_ego, speed_breakpoints, distance_breakpoints)
|
||||
|
||||
self.coord.latitude = current_lat
|
||||
self.coord.longitude = current_lon
|
||||
distance = self.coord.distance_to(progress['next_turn']['location'])
|
||||
|
||||
if distance <= distance_interp:
|
||||
if distance <= 30.0:
|
||||
modifier = progress['next_turn']['modifier']
|
||||
return str(modifier)
|
||||
return 'none'
|
||||
|
||||
@staticmethod
|
||||
def arrived_at_destination(progress, v_ego) -> bool:
|
||||
if v_ego < 1.0:
|
||||
maneuvers = progress['all_maneuvers'][0]
|
||||
if maneuvers['type'] == 'arrive' or maneuvers['instruction'].startswith('Your destination'):
|
||||
return True
|
||||
def get_current_speed_limit_from_progress(progress, is_metric: bool) -> int:
|
||||
if progress and progress['current_maxspeed']:
|
||||
speed, _ = progress['current_maxspeed']
|
||||
if is_metric:
|
||||
return int(speed)
|
||||
else:
|
||||
return int(round(speed * CV.KPH_TO_MPH))
|
||||
return 0
|
||||
|
||||
@staticmethod
|
||||
def arrived_at_destination(progress) -> bool:
|
||||
if progress['all_maneuvers'][0]['type'] == 'arrive':
|
||||
return True
|
||||
elif progress['all_maneuvers'][0]['instruction'].startswith('Your destination'):
|
||||
return True
|
||||
return False
|
||||
|
||||
@@ -54,17 +54,17 @@ class TestMapbox:
|
||||
assert 'modifier' in step
|
||||
|
||||
def test_upcoming_turn_detection(self):
|
||||
upcoming = self.nav.get_upcoming_turn_from_progress(self.progress, self.current_lat, self.current_lon, v_ego=40.0)
|
||||
upcoming = self.nav.get_upcoming_turn_from_progress(self.progress, self.current_lat, self.current_lon)
|
||||
assert isinstance(upcoming, str)
|
||||
assert upcoming == 'none'
|
||||
|
||||
if self.route['steps']:
|
||||
turn_lat = self.route['steps'][1]['location'].latitude
|
||||
turn_lon = self.route['steps'][1]['location'].longitude
|
||||
close_lat = turn_lat - 0.000175 # slightly before the turn
|
||||
close_lat = turn_lat - 0.00025 # slightly before the turn
|
||||
if self.progress and self.progress.get('next_turn'):
|
||||
expected_turn = self.progress['next_turn']['modifier']
|
||||
upcoming_close = self.nav.get_upcoming_turn_from_progress(self.progress, close_lat, turn_lon, v_ego=0.0)
|
||||
upcoming_close = self.nav.get_upcoming_turn_from_progress(self.progress, close_lat, turn_lon)
|
||||
if expected_turn:
|
||||
assert upcoming_close == expected_turn == 'right', "Should be a right turn upcoming"
|
||||
|
||||
@@ -79,20 +79,16 @@ class TestMapbox:
|
||||
assert isinstance(self.progress['all_maneuvers'], list)
|
||||
|
||||
def test_speed_limit_handling(self):
|
||||
speed_limit_metric = self.progress['current_maxspeed'][0]
|
||||
speed_limit_imperial = (round(speed_limit_metric * CV.KPH_TO_MPH))
|
||||
speed_limit_metric = self.nav.get_current_speed_limit_from_progress(self.progress, True)
|
||||
speed_limit_imperial = self.nav.get_current_speed_limit_from_progress(self.progress, False)
|
||||
assert isinstance(speed_limit_metric, int)
|
||||
assert isinstance(speed_limit_imperial, int)
|
||||
expected_metric = int(self.progress['current_maxspeed'][0])
|
||||
expected_imperial = int(round(self.progress['current_maxspeed'][0] * CV.KPH_TO_MPH))
|
||||
assert speed_limit_metric == expected_metric
|
||||
assert speed_limit_imperial == expected_imperial
|
||||
|
||||
def test_arrival_detection(self):
|
||||
is_arrived = self.nav.arrived_at_destination(self.progress, 2.0)
|
||||
is_arrived = self.nav.arrived_at_destination(self.progress)
|
||||
assert isinstance(is_arrived, bool)
|
||||
assert not is_arrived
|
||||
|
||||
def test_bearing_misalign(self):
|
||||
lat = self.route['steps'][1]['location'].latitude
|
||||
lon = self.route['steps'][1]['location'].longitude
|
||||
self.nav.get_route_progress(lat, lon)
|
||||
route_bearing_misaligned = self.nav.route_bearing_misalign(self.route, 45, 5.0)
|
||||
# based on math: closest index: 7, normalized bearing: 45 route bearing: 180.5486953778888, expected differential: 135.54869538
|
||||
assert route_bearing_misaligned
|
||||
|
||||
@@ -4,8 +4,7 @@ Copyright (c) 2021-, James Vecellio, Haibin Wen, sunnypilot, and a number of oth
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from math import degrees
|
||||
from numpy import interp
|
||||
import math
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import custom
|
||||
@@ -25,7 +24,7 @@ class Navigationd:
|
||||
self.mapbox = MapboxIntegration()
|
||||
self.nav_instructions = NavigationInstructions()
|
||||
|
||||
self.sm = messaging.SubMaster(['carControlSP', 'liveLocationKalman'])
|
||||
self.sm = messaging.SubMaster(['liveLocationKalman'])
|
||||
self.pm = messaging.PubMaster(['navigationd'])
|
||||
self.rk = Ratekeeper(3) # 3 Hz
|
||||
|
||||
@@ -42,23 +41,26 @@ class Navigationd:
|
||||
self.frame: int = -1
|
||||
self.last_position: Coordinate | None = None
|
||||
self.last_bearing: float | None = None
|
||||
self.is_metric: bool = False
|
||||
self.valid: bool = False
|
||||
|
||||
def _update_params(self):
|
||||
if self.last_position is not None:
|
||||
self.frame += 1
|
||||
if self.frame % 15 == 0:
|
||||
if self.frame % 9 == 0:
|
||||
self.allow_navigation = self.params.get('AllowNavigation', return_default=True)
|
||||
self.is_metric = self.params.get('IsMetric', return_default=True)
|
||||
self.new_destination = self.params.get('MapboxRoute')
|
||||
self.recompute_allowed = self.params.get('MapboxRecompute', return_default=True)
|
||||
|
||||
self.allow_recompute: bool = (self.new_destination != self.destination and self.new_destination != '') or (
|
||||
self.recompute_allowed and self.reroute_counter > 9 and self.route)
|
||||
self.recompute_allowed and self.reroute_counter > 9 and self.route
|
||||
)
|
||||
|
||||
if self.allow_recompute:
|
||||
postvars = {'place_name': self.new_destination}
|
||||
postvars, valid_addr = self.mapbox.set_destination(postvars, self.last_position.longitude, self.last_position.latitude, self.last_bearing)
|
||||
|
||||
cloudlog.debug(f'Set new destination to: {self.new_destination}, valid: {valid_addr}')
|
||||
if valid_addr:
|
||||
self.destination = self.new_destination
|
||||
self.nav_instructions.clear_route_cache()
|
||||
@@ -77,14 +79,11 @@ class Navigationd:
|
||||
def _update_navigation(self) -> tuple[str, dict | None, dict]:
|
||||
banner_instructions: str = ''
|
||||
nav_data: dict = {}
|
||||
if self.allow_navigation and self.route and self.last_position is not None:
|
||||
if self.allow_navigation and self.last_position is not None:
|
||||
if progress := self.nav_instructions.get_route_progress(self.last_position.latitude, self.last_position.longitude):
|
||||
v_ego = float(max(self.sm['carControlSP'].speed, 0.0))
|
||||
nav_data['upcoming_turn'] = self.nav_instructions.get_upcoming_turn_from_progress(progress, self.last_position.latitude,
|
||||
self.last_position.longitude, v_ego)
|
||||
speed_limit, _ = progress['current_maxspeed']
|
||||
nav_data['current_speed_limit'] = speed_limit
|
||||
arrived = self.nav_instructions.arrived_at_destination(progress, v_ego)
|
||||
nav_data['upcoming_turn'] = self.nav_instructions.get_upcoming_turn_from_progress(progress, self.last_position.latitude, self.last_position.longitude)
|
||||
nav_data['current_speed_limit'] = self.nav_instructions.get_current_speed_limit_from_progress(progress, self.is_metric)
|
||||
arrived = self.nav_instructions.arrived_at_destination(progress)
|
||||
|
||||
if progress['current_step']:
|
||||
parsed = parse_banner_instructions(progress['current_step']['bannerInstructions'], progress['distance_to_end_of_step'])
|
||||
@@ -92,35 +91,27 @@ class Navigationd:
|
||||
banner_instructions = parsed['maneuverPrimaryText']
|
||||
|
||||
nav_data['distance_from_route'] = progress['distance_from_route']
|
||||
speed_breakpoints: list = [0.0, 5.0, 10.0, 20.0, 40.0]
|
||||
distance_list: list = [100.0, 125.0, 150.0, 200.0, 250.0]
|
||||
large_distance: bool = progress['distance_from_route'] > float(interp(v_ego, speed_breakpoints, distance_list))
|
||||
large_distance = progress['distance_from_route'] > 100
|
||||
|
||||
route_bearing_misalign: bool = self.nav_instructions.route_bearing_misalign(self.route, self.last_bearing, v_ego)
|
||||
|
||||
if large_distance and not arrived:
|
||||
if large_distance:
|
||||
self.cancel_route_counter = self.cancel_route_counter + 1 if progress['distance_from_route'] > NAV_CV.QUARTER_MILE else 0
|
||||
if self.recompute_allowed:
|
||||
self.reroute_counter += 1
|
||||
elif arrived:
|
||||
self.cancel_route_counter += 1
|
||||
self.recompute_allowed = False
|
||||
elif route_bearing_misalign:
|
||||
self.cancel_route_counter += 1
|
||||
if self.recompute_allowed:
|
||||
self.reroute_counter += 1
|
||||
else:
|
||||
self.cancel_route_counter = 0
|
||||
self.reroute_counter = 0
|
||||
|
||||
# Don't recompute in last segment to prevent reroute loops
|
||||
if progress['current_step_idx'] == len(self.route['steps']) - 1:
|
||||
self.recompute_allowed = False
|
||||
self.allow_navigation = False
|
||||
if self.route:
|
||||
if progress['current_step_idx'] == len(self.route['steps']) - 1:
|
||||
self.allow_recompute = False
|
||||
else:
|
||||
banner_instructions = ''
|
||||
progress = None
|
||||
nav_data = {}
|
||||
self.valid = False
|
||||
|
||||
return banner_instructions, progress, nav_data
|
||||
|
||||
@@ -140,18 +131,19 @@ class Navigationd:
|
||||
else []
|
||||
)
|
||||
msg.navigationd.allManeuvers = all_maneuvers
|
||||
|
||||
return msg
|
||||
|
||||
def run(self):
|
||||
cloudlog.warning('navigationd init')
|
||||
|
||||
while True:
|
||||
self.sm.update(0)
|
||||
self.sm.update()
|
||||
location = self.sm['liveLocationKalman']
|
||||
localizer_valid = location.positionGeodetic.valid if location else False
|
||||
|
||||
if localizer_valid:
|
||||
self.last_bearing = degrees(location.calibratedOrientationNED.value[2])
|
||||
self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
|
||||
self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
|
||||
|
||||
self._update_params()
|
||||
|
||||
@@ -65,3 +65,12 @@ class TestNavigationd:
|
||||
|
||||
assert received_msg.bannerInstructions == msg.navigationd.bannerInstructions
|
||||
assert received_msg.valid == msg.navigationd.valid
|
||||
|
||||
def test_cancel_route(self):
|
||||
nav = Navigationd()
|
||||
nav.last_position = Coordinate(latitude=37.0, longitude=128.0)
|
||||
nav.route = {'580 Winchester dr, oxnard, CA': True}
|
||||
nav.cancel_route_counter = 30
|
||||
nav._update_params()
|
||||
assert nav.route is None
|
||||
assert nav.destination is None
|
||||
|
||||
@@ -72,8 +72,6 @@ class ControlsExt:
|
||||
|
||||
CC_SP.intelligentCruiseButtonManagement = sm['selfdriveStateSP'].intelligentCruiseButtonManagement
|
||||
|
||||
CC_SP.speed = sm['carState'].vEgo
|
||||
|
||||
return CC_SP
|
||||
|
||||
@staticmethod
|
||||
|
||||
Reference in New Issue
Block a user