synchronize model frame with camera frame

commaai · Sep 29, 2024 · 822febd · 822febd
1 parent ce0e3d5
commit 822febd
Show file tree

Hide file tree

Showing 5 changed files with 36 additions and 13 deletions.
diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc
@@ -19,12 +19,16 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget *par
 
  experimental_btn = new ExperimentalButton(this);
  main_layout->addWidget(experimental_btn, 0, Qt::AlignTop | Qt::AlignRight);
+ // Disable automatic UI updates on frame received
+ setUpdateOnFrame(false);
 }
 
 void AnnotatedCameraWidget::updateState(const UIState &s) {
  // update engageability/experimental mode button
  experimental_btn->updateState(s);
  dmon.updateState(s);
+
+ update();
 }
 
 void AnnotatedCameraWidget::initializeGL() {

diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc
@@ -201,11 +201,11 @@ void CameraWidget::paintGL() {
  if (frames.empty()) return;
 
  int frame_idx = frames.size() - 1;
-
- // Always draw latest frame until sync logic is more stable
- // for (frame_idx = 0; frame_idx < frames.size() - 1; frame_idx++) {
- // if (frames[frame_idx].first == draw_frame_id) break;
- // }
+ if (draw_frame_id >= 0) {
+  for (frame_idx = 0; frame_idx < frames.size() - 1; ++frame_idx) {
+  if (frames[frame_idx].first == draw_frame_id) break;
+  }
+ }
 
  // Log duplicate/dropped frames
  if (frames[frame_idx].first == prev_frame_id) {
@@ -306,7 +306,7 @@ void CameraWidget::vipcConnected(VisionIpcClient *vipc_client) {
 }
 
 void CameraWidget::vipcFrameReceived() {
- update();
+ if (update_on_frame) update();
 }
 
 void CameraWidget::vipcThread() {

diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h
@@ -35,6 +35,7 @@ class CameraWidget : public QOpenGLWidget, protected QOpenGLFunctions {
  explicit CameraWidget(std::string stream_name, VisionStreamType stream_type, QWidget* parent = nullptr);
  ~CameraWidget();
  void setBackgroundColor(const QColor &color) { bg = color; }
+ void setUpdateOnFrame(bool v) { update_on_frame = v; }
  void setFrameId(int frame_id) { draw_frame_id = frame_id; }
  void setStreamType(VisionStreamType type) { requested_stream_type = type; }
  VisionStreamType getStreamType() { return active_stream_type; }
@@ -77,7 +78,8 @@ class CameraWidget : public QOpenGLWidget, protected QOpenGLFunctions {
  QThread *vipc_thread = nullptr;
  std::recursive_mutex frame_lock;
  std::deque<std::pair<uint32_t, VisionBuf*>> frames;
- uint32_t draw_frame_id = 0;
+ bool update_on_frame = true;
+ uint32_t draw_frame_id = -1;
  uint32_t prev_frame_id = 0;
 
 protected slots:

diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
@@ -191,10 +191,24 @@ UIState::UIState(QObject *parent) : QObject(parent) {
  prime_state = new PrimeState(this);
  language = QString::fromStdString(Params().get("LanguageSetting"));
 
- // update timer
- timer = new QTimer(this);
- QObject::connect(timer, &QTimer::timeout, this, &UIState::update);
- timer->start(1000 / UI_FREQ);
+ thread = new QThread();
+ QObject::connect(thread, &QThread::started, [this]() { sheduleUpdate(); });
+ thread->start();
+}
+
+UIState::~UIState() {
+ thread->requestInterruption();
+ thread->quit();
+ thread->wait();
+ delete thread;
+}
+
+void UIState::sheduleUpdate() {
+ SubMaster sub_master({"modelV2"});
+ while (!QThread::currentThread()->isInterruptionRequested()) {
+ sub_master.update(100);
+ QMetaObject::invokeMethod(this, std::bind(&UIState::update, this), Qt::QueuedConnection);
+ }
 }
 
 void UIState::update() {

diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h
@@ -4,10 +4,10 @@
 #include <memory>
 #include <string>
 
-#include <QTimer>
 #include <QColor>
 #include <QFuture>
 #include <QPolygonF>
+#include <QThread>
 
 #include "cereal/messaging/messaging.h"
 #include "common/mat.h"
@@ -81,6 +81,7 @@ class UIState : public QObject {
 
 public:
  UIState(QObject* parent = 0);
+ ~UIState();
  void updateStatus();
  inline bool engaged() const {
  return scene.started && (*sm)["selfdriveState"].getSelfdriveState().getEnabled();
@@ -104,8 +105,10 @@ private slots:
  void update();
 
 private:
- QTimer *timer;
+ void sheduleUpdate();
+
  bool started_prev = false;
+ QThread *thread = nullptr;
 };
 
 UIState *uiState();