StarPilot/selfdrive/ui/qt/onroad/annotated_camera.cc

#include "selfdrive/ui/qt/onroad/annotated_camera.h"

#include <QPainter>
#include <algorithm>
#include <cmath>
#include <exception>
#include <string>
#include <vector>

#include "common/params.h"
#include "common/swaglog.h"
#include "selfdrive/ui/qt/util.h"

constexpr int CAMERA_VIEW_NONE = 4;

// Window that shows camera view and variety of info drawn on top
AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget *parent)
    : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, parent) {
  pm = std::make_unique<PubMaster>(std::vector<const char*>{"uiDebug"});

  main_layout = new QVBoxLayout(this);
  main_layout->setMargin(UI_BORDER_SIZE);
  main_layout->setSpacing(0);

  experimental_btn = new ExperimentalButton(this);
  main_layout->addWidget(experimental_btn, 0, Qt::AlignTop | Qt::AlignRight);

  personality_btn = new DrivingPersonalityButton(this);
  personality_btn->setVisible(false);

  for (int i = 0; i < static_cast<int>(favorite_btns.size()); ++i) {
    favorite_btns[i] = new FavoriteButton(i, this);
    favorite_btns[i]->setVisible(false);
  }

  screen_recorder = new ScreenRecorder(this);
  screen_recorder->setVisible(false);
}

bool AnnotatedCameraWidget::handleHudPress(const QPoint &pos) {
  return hud.handleNavigationPress(pos);
}

bool AnnotatedCameraWidget::handleHudRelease(const QPoint &pos) {
  return hud.handleNavigationRelease(pos);
}

void AnnotatedCameraWidget::updateState(const UIState &s, const StarPilotUIState &fs) {
  // update engageability/experimental mode button
  experimental_btn->updateState(s, fs);
  dmon.updateState(s);

  const SubMaster &sm = *(s.sm);

  const cereal::CarState::Reader &carState = sm["carState"].getCarState();

  static Params params;
  const bool hide_steering_wheel = starpilot_toggles.value("hide_steering_wheel").toBool() || params.getBool("HideSteeringWheel");
  experimental_btn->setVisible(!hide_steering_wheel);

  const QPoint experimental_button_position = hide_steering_wheel
    ? QPoint(width() - UI_BORDER_SIZE - btn_size, UI_BORDER_SIZE)
    : QPoint(experimental_btn->x(), experimental_btn->y());
  starpilot_nvg->experimentalButtonPosition = experimental_button_position;

  std::vector<FavoriteButton*> visible_favorite_btns;
  const bool favorites_anchor_ready = starpilot_nvg->dmIconPosition != QPoint(0, 0) && !starpilot_nvg->hideBottomIcons;
  for (FavoriteButton *favorite_btn : favorite_btns) {
    favorite_btn->updateState();
    if (favorites_anchor_ready && favorite_btn->shouldShow()) {
      visible_favorite_btns.push_back(favorite_btn);
    } else {
      favorite_btn->setVisible(false);
    }
  }

  const bool onroad_distance_btn_enabled = favorites_anchor_ready && starpilot_toggles.value("onroad_distance_button").toBool();
  const int gap = UI_BORDER_SIZE / 2;
  int controls_width = 0;
  if (!visible_favorite_btns.empty()) {
    controls_width = visible_favorite_btns.size() * btn_size + (visible_favorite_btns.size() - 1) * gap;
  }
  if (onroad_distance_btn_enabled) {
    controls_width += (controls_width > 0 ? gap : 0) + personality_btn->width();
  }
  dmon.onroad_controls_width = controls_width;

  const int controls_y = favorites_anchor_ready
    ? std::clamp(starpilot_nvg->dmIconPosition.y() - (btn_size / 2), UI_BORDER_SIZE, height() - UI_BORDER_SIZE - btn_size)
    : 0;
  int cursor_x = starpilot_nvg->rightHandDM ? width() - UI_BORDER_SIZE : UI_BORDER_SIZE;

  personality_btn->setVisible(onroad_distance_btn_enabled);
  if (onroad_distance_btn_enabled) {
    const int personality_x = starpilot_nvg->rightHandDM ? cursor_x - personality_btn->width() : cursor_x;
    personality_btn->move(personality_x, controls_y);
    personality_btn->updateState(s, fs);
    cursor_x += starpilot_nvg->rightHandDM ? -(personality_btn->width() + gap) : personality_btn->width() + gap;
  }

  for (FavoriteButton *favorite_btn : visible_favorite_btns) {
    const int favorite_x = starpilot_nvg->rightHandDM ? cursor_x - favorite_btn->width() : cursor_x;
    favorite_btn->move(favorite_x, controls_y);
    favorite_btn->setVisible(true);
    cursor_x += starpilot_nvg->rightHandDM ? -(favorite_btn->width() + gap) : favorite_btn->width() + gap;
  }

  const QPoint screen_recorder_position = hide_steering_wheel
    ? experimental_button_position
    : QPoint(experimental_button_position.x() - UI_BORDER_SIZE - btn_size, experimental_button_position.y());
  screen_recorder->move(screen_recorder_position);
  screen_recorder->setVisible(starpilot_nvg->standstillDuration == 0 && !(starpilot_nvg->signalStyle == "static" && carState.getRightBlinker()) && starpilot_toggles.value("screen_recorder").toBool());
}

void AnnotatedCameraWidget::initializeGL() {
  CameraWidget::initializeGL();
  qInfo() << "OpenGL version:" << QString((const char*)glGetString(GL_VERSION));
  qInfo() << "OpenGL vendor:" << QString((const char*)glGetString(GL_VENDOR));
  qInfo() << "OpenGL renderer:" << QString((const char*)glGetString(GL_RENDERER));
  qInfo() << "OpenGL language version:" << QString((const char*)glGetString(GL_SHADING_LANGUAGE_VERSION));

  prev_draw_t = millis_since_boot();
  setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
}

mat4 AnnotatedCameraWidget::calcFrameMatrix() {
  // Project point at "infinity" to compute x and y offsets
  // to ensure this ends up in the middle of the screen
  // for narrow come and a little lower for wide cam.
  // TODO: use proper perspective transform?

  // Select intrinsic matrix and calibration based on camera type
  auto *s = uiState();
  bool wide_cam = active_stream_type == VISION_STREAM_WIDE_ROAD;
  const auto &intrinsic_matrix = wide_cam ? ECAM_INTRINSIC_MATRIX : FCAM_INTRINSIC_MATRIX;
  const auto &calibration = wide_cam ? s->scene.view_from_wide_calib : s->scene.view_from_calib;

   // Compute the calibration transformation matrix
  const auto calib_transform = intrinsic_matrix * calibration;

  float zoom = wide_cam ? 2.0 : 1.1;
  Eigen::Vector3f inf(1000., 0., 0.);
  auto Kep = calib_transform * inf;

  int w = width(), h = height();
  float center_x = intrinsic_matrix(0, 2);
  float center_y = intrinsic_matrix(1, 2);

  float max_x_offset = center_x * zoom - w / 2 - 5;
  float max_y_offset = center_y * zoom - h / 2 - 5;
  float x_offset = std::clamp<float>((Kep.x() / Kep.z() - center_x) * zoom, -max_x_offset, max_x_offset);
  float y_offset = std::clamp<float>((Kep.y() / Kep.z() - center_y) * zoom, -max_y_offset, max_y_offset);

  // Apply transformation such that video pixel coordinates match video
  // 1) Put (0, 0) in the middle of the video
  // 2) Apply same scaling as video
  // 3) Put (0, 0) in top left corner of video
  Eigen::Matrix3f video_transform =(Eigen::Matrix3f() <<
    zoom, 0.0f, (w / 2 - x_offset) - (center_x * zoom),
    0.0f, zoom, (h / 2 - y_offset) - (center_y * zoom),
    0.0f, 0.0f, 1.0f).finished();

  model.setTransform(video_transform * calib_transform);

  float zx = zoom * 2 * center_x / w;
  float zy = zoom * 2 * center_y / h;
  return mat4{{
    zx, 0.0, 0.0, -x_offset / w * 2,
    0.0, zy, 0.0, y_offset / h * 2,
    0.0, 0.0, 1.0, 0.0,
    0.0, 0.0, 0.0, 1.0,
  }};
}

void AnnotatedCameraWidget::paintGL() {
}

void AnnotatedCameraWidget::paintEvent(QPaintEvent *event) {
  UIState *s = uiState();
  SubMaster &sm = *(s->sm);
  const double start_draw_t = millis_since_boot();

  QPainter painter(this);
  static Params params;
  const std::string camera_view_param = params.get("CameraView");
  int camera_view = starpilot_toggles.value("camera_view").toInt();
  if (!camera_view_param.empty()) {
    try {
      camera_view = std::stoi(camera_view_param);
    } catch (const std::exception &) {
      LOGW("invalid CameraView param: %s", camera_view_param.c_str());
    }
  }
  const bool camera_view_none = camera_view == CAMERA_VIEW_NONE;

  // draw camera frame
  if (camera_view_none) {
    painter.fillRect(rect(), Qt::black);
  } else {
    std::lock_guard lk(frame_lock);

    if (frames.empty()) {
      if (skip_frame_count > 0) {
        skip_frame_count--;
        qDebug() << "skipping frame, not ready";
        return;
      }
    } else {
      // skip drawing up to this many frames if we're
      // missing camera frames. this smooths out the
      // transitions from the narrow and wide cameras
      skip_frame_count = 5;
    }

    // Wide or narrow cam dependent on speed
    bool has_wide_cam = available_streams.count(VISION_STREAM_WIDE_ROAD);
    if (has_wide_cam) {
      float v_ego = sm["carState"].getCarState().getVEgo();
      if ((v_ego < 10) || available_streams.size() == 1) {
        wide_cam_requested = true;
      } else if (v_ego > 15) {
        wide_cam_requested = false;
      }
      wide_cam_requested = wide_cam_requested && sm["selfdriveState"].getSelfdriveState().getExperimentalMode() && camera_view == 0;
    }
    CameraWidget::setStreamType(camera_view == 1 ? VISION_STREAM_DRIVER :
                                ((camera_view == 3 && has_wide_cam) || wide_cam_requested) ? VISION_STREAM_WIDE_ROAD :
                                VISION_STREAM_ROAD);
    CameraWidget::setFrameId(sm["modelV2"].getModelV2().getFrameId());

    painter.beginNativePainting();
    CameraWidget::paintGL();
    painter.endNativePainting();
  }

  painter.setRenderHint(QPainter::Antialiasing);
  painter.setPen(Qt::NoPen);

  dmon.starpilot_nvg = starpilot_nvg;
  hud.starpilot_nvg = starpilot_nvg;
  model.starpilot_nvg = starpilot_nvg;

  experimental_btn->starpilot_scene = starpilot_scene;
  model.starpilot_scene = starpilot_scene;

  dmon.starpilot_toggles = starpilot_toggles;
  experimental_btn->starpilot_toggles = starpilot_toggles;
  hud.starpilot_toggles = starpilot_toggles;
  model.starpilot_toggles = starpilot_toggles;

  if (!camera_view_none) {
    model.draw(painter, rect());
  }
  dmon.draw(painter, rect());
  hud.updateState(*s);
  hud.draw(painter, rect());

  starpilot_nvg->paintStarPilotWidgets(painter, *s, camera_view_none);

  double cur_draw_t = millis_since_boot();
  double dt = cur_draw_t - prev_draw_t;
  fps = fps_filter.update(1. / dt * 1000);
  if (fps < 15) {
    LOGW("slow frame rate: %.2f fps", fps);
  }
  prev_draw_t = cur_draw_t;

  // publish debug msg
  MessageBuilder msg;
  auto m = msg.initEvent().initUiDebug();
  m.setDrawTimeMillis(cur_draw_t - start_draw_t);
  pm->send("uiDebug", msg);
}

void AnnotatedCameraWidget::showEvent(QShowEvent *event) {
  CameraWidget::showEvent(event);

  ui_update_params(uiState());
  prev_draw_t = millis_since_boot();
}