diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 98c636b0a4..79bf5fa88a 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -127,6 +127,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { if (alert.size == cereal::ControlsState::AlertSize::NONE) { return; } + return; static std::map alert_sizes = { {cereal::ControlsState::AlertSize::SMALL, 271}, {cereal::ControlsState::AlertSize::MID, 420}, @@ -492,6 +493,23 @@ void AnnotatedCameraWidget::updateFrameMat() { .translate(-intrinsic_matrix.v[2], -intrinsic_matrix.v[5]); } +static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, QPointF *out) { + const float margin = 500.0f; + const QRectF clip_region{-margin, -margin, s->fb_w + 2 * margin, s->fb_h + 2 * margin}; + + const vec3 pt = (vec3){{in_x, in_y, in_z}}; + const vec3 Ep = matvecmul3(s->scene.wide_cam ? s->scene.view_from_wide_calib : s->scene.view_from_calib, pt); + const vec3 KEp = matvecmul3(s->scene.wide_cam ? ecam_intrinsic_matrix : fcam_intrinsic_matrix, Ep); + + // Project. + QPointF point = s->car_space_transform.map(QPointF{KEp.v[0] / KEp.v[2], KEp.v[1] / KEp.v[2]}); + if (clip_region.contains(point)) { + *out = point; + return true; + } + return false; +} + void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { painter.save(); @@ -511,24 +529,65 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { } // paint path - QLinearGradient bg(0, height(), 0, height() / 4); +// QLinearGradient bg(0, height(), 0, height() / 4); + QLinearGradient bg(0, height(), 0, 0); float start_hue, end_hue; if (sm["controlsState"].getControlsState().getExperimentalMode()) { - const auto &acceleration = sm["modelV2"].getModelV2().getAcceleration(); - float acceleration_future = 0; - if (acceleration.getZ().size() > 16) { - acceleration_future = acceleration.getX()[16]; // 2.5 seconds - } - start_hue = 60; - // speed up: 120, slow down: 0 - end_hue = fmax(fmin(start_hue + acceleration_future * 45, 148), 0); + QPointF out; + + calib_frame_to_full_frame(s, 5, 0, 1.22, &out); + qDebug() << out; + + int track_vertices_len = scene.track_vertices.length(); + assert(track_vertices_len % 2 == 0); + QVector right_points = scene.track_vertices.mid(0, track_vertices_len / 2); + qDebug() << right_points.length(); + for (int i = 0; i < right_points.length(); i++) { + const auto &acceleration = sm["modelV2"].getModelV2().getAcceleration(); + float acceleration_future = 0; + if (i > acceleration.getZ().size()) { + break; + } +// if (acceleration.getZ().size() > 16) { + acceleration_future = acceleration.getX()[i]; // 2.5 seconds +// } + qDebug() << "Using acceleration:" << acceleration_future; + + // need to flip around so 0 is bottom of frame (not really, can also flip linear gradient above) + float lin_grad_point = (height() - right_points[i].y()) / height(); + qDebug() << right_points[i] << right_points[i].y() << lin_grad_point; + // Some points are out of frame + // TODO: tho maybe it makes sense to clip instead, so gradient is correct. or no clip/skip at all + if (lin_grad_point < 0) { + continue; + } - // FIXME: painter.drawPolygon can be slow if hue is not rounded - end_hue = int(end_hue * 100 + 0.5) / 100; + start_hue = 60; + // speed up: 120, slow down: 0 + end_hue = fmax(fmin(start_hue + acceleration_future * 45, 148), 0); - bg.setColorAt(0.0, QColor::fromHslF(start_hue / 360., 0.97, 0.56, 0.4)); - bg.setColorAt(0.5, QColor::fromHslF(end_hue / 360., 1.0, 0.68, 0.35)); - bg.setColorAt(1.0, QColor::fromHslF(end_hue / 360., 1.0, 0.68, 0.0)); + // FIXME: painter.drawPolygon can be slow if hue is not rounded + end_hue = int(end_hue * 100 + 0.5) / 100; + bg.setColorAt(lin_grad_point, QColor::fromHslF(end_hue / 360., 0.97, 0.56, 0.4)); + + } +// qDebug() << right_points; + +// const auto &acceleration = sm["modelV2"].getModelV2().getAcceleration(); +// float acceleration_future = 0; +// if (acceleration.getZ().size() > 16) { +// acceleration_future = acceleration.getX()[16]; // 2.5 seconds +// } +// start_hue = 60; +// // speed up: 120, slow down: 0 +// end_hue = fmax(fmin(start_hue + acceleration_future * 45, 148), 0); +// +// // FIXME: painter.drawPolygon can be slow if hue is not rounded +// end_hue = int(end_hue * 100 + 0.5) / 100; +// +// bg.setColorAt(0.0, QColor::fromHslF(start_hue / 360., 0.97, 0.56, 0.4)); +// bg.setColorAt(0.5, QColor::fromHslF(end_hue / 360., 1.0, 0.68, 0.35)); +// bg.setColorAt(1.0, QColor::fromHslF(end_hue / 360., 1.0, 0.68, 0.0)); } else { bg.setColorAt(0.0, QColor::fromHslF(148 / 360., 0.94, 0.51, 0.4)); bg.setColorAt(0.5, QColor::fromHslF(112 / 360., 1.0, 0.68, 0.35)); @@ -651,7 +710,7 @@ void AnnotatedCameraWidget::paintGL() { } else if (v_ego > 15) { wide_cam_requested = false; } - wide_cam_requested = wide_cam_requested && sm["controlsState"].getControlsState().getExperimentalMode(); + wide_cam_requested = wide_cam_requested && sm["controlsState"].getControlsState().getExperimentalMode() && false; // TODO: also detect when ecam vision stream isn't available // for replay of old routes, never go to widecam wide_cam_requested = wide_cam_requested && s->scene.calibration_wide_valid;