cabana: improve time control (#25985)

* group signals/slots together * slider:fix wrong minimum * add TODO * moveing to mouse click position * show tickmark * fix seek back to the old pos after sliderReleased * reduce data copied in queued connection * drop packets while seeking * install event filter in streaming * stop replay in dctor old-commit-hash: 750b96aaedb88defd522a60a4bb5fbfeb46332db
2022-10-07 06:02:22 +08:00
parent c18d23c8be
commit 2d9dacacfe
8 changed files with 100 additions and 55 deletions
--- a/tools/cabana/detailwidget.cc
+++ b/tools/cabana/detailwidget.cc
@@ -206,7 +206,7 @@ void HistoryLog::updateState() {
    const auto &c = parser->history_log[i];
    auto label = labels[i];
    label->setVisible(true);
-    label->setText(QString("%1         %2").arg(c.ts, 0, 'f', 3).arg(c.hex_dat));
+    label->setText(QString("%1         %2").arg(c.ts, 0, 'f', 3).arg(toHex(c.dat)));
  }

  for (; i < std::size(labels); ++i) {
--- a/tools/cabana/messageswidget.cc
+++ b/tools/cabana/messageswidget.cc
@@ -83,7 +83,7 @@ void MessagesWidget::updateState() {
    getTableItem(i, 0)->setText(name);
    getTableItem(i, 1)->setText(c.id);
    getTableItem(i, 2)->setText(QString::number(parser->counters[c.id]));
-    getTableItem(i, 3)->setText(c.hex_dat);
+    getTableItem(i, 3)->setText(toHex(c.dat));
    table_widget->showRow(i);
    i++;
  }
--- a/tools/cabana/parser.cc
+++ b/tools/cabana/parser.cc
@@ -6,27 +6,25 @@

 Parser *parser = nullptr;

+static bool event_filter(const Event *e, void *opaque) {
+  Parser *p = (Parser*)opaque;
+  return p->eventFilter(e);
+}
+
 Parser::Parser(QObject *parent) : QObject(parent) {
  parser = this;

  qRegisterMetaType<std::vector<CanData>>();
  QObject::connect(this, &Parser::received, this, &Parser::process, Qt::QueuedConnection);
-
-  thread = new QThread();
-  connect(thread, &QThread::started, [=]() { recvThread(); });
-  QObject::connect(thread, &QThread::finished, thread, &QThread::deleteLater);
-  thread->start();
 }

 Parser::~Parser() {
  replay->stop();
-  exit = true;
-  thread->quit();
-  thread->wait();
 }

 bool Parser::loadRoute(const QString &route, const QString &data_dir, bool use_qcam) {
  replay = new Replay(route, {"can", "roadEncodeIdx"}, {}, nullptr, use_qcam ? REPLAY_FLAG_QCAMERA : 0, data_dir, this);
+  replay->installEventFilter(event_filter, this);
  QObject::connect(replay, &Replay::segmentsMerged, this, &Parser::segmentsMerged);
  if (replay->load()) {
    replay->start();
@@ -47,9 +45,9 @@ void Parser::openDBC(const QString &name) {

 void Parser::process(std::vector<CanData> msgs) {
  static double prev_update_ts = 0;
+
  for (const auto &can_data : msgs) {
    can_msgs[can_data.id] = can_data;
-    current_sec = can_data.ts;
    ++counters[can_data.id];

    if (can_data.id == current_msg_id) {
@@ -59,46 +57,45 @@ void Parser::process(std::vector<CanData> msgs) {
      history_log.push_front(can_data);
    }
  }
-  double current_ts = millis_since_boot();
-  if ((current_ts - prev_update_ts) > 1000.0 / FPS) {
-    prev_update_ts = current_ts;
+  double now = millis_since_boot();
+  if ((now - prev_update_ts) > 1000.0 / FPS) {
+    prev_update_ts = now;
    emit updated();
  }

  if (current_sec < begin_sec || current_sec > end_sec) {
    // loop replay in selected range.
-    replay->seekTo(begin_sec, false);
+    seekTo(begin_sec);
  }
 }

-void Parser::recvThread() {
-  AlignedBuffer aligned_buf;
-  std::unique_ptr<Context> context(Context::create());
-  std::unique_ptr<SubSocket> subscriber(SubSocket::create(context.get(), "can"));
-  subscriber->setTimeout(100);
+bool Parser::eventFilter(const Event *event) {
+  // drop packets when the GUI thread is calling seekTo. to make sure the current_sec is accurate.
+  if (!seeking && event->which == cereal::Event::Which::CAN) {
+    current_sec = (event->mono_time - replay->routeStartTime()) / (double)1e9;

-  std::vector<CanData> can;
-  while (!exit) {
-    std::unique_ptr<Message> msg(subscriber->receive());
-    if (!msg) continue;
+    auto can = event->event.getCan();
+    msgs_buf.clear();
+    msgs_buf.reserve(can.size());

-    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
-    cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
-
-    can.clear();
-    can.reserve(event.getCan().size());
-    for (const auto &c : event.getCan()) {
-      CanData &data = can.emplace_back();
+    for (const auto &c : can) {
+      CanData &data = msgs_buf.emplace_back();
      data.address = c.getAddress();
      data.bus_time = c.getBusTime();
      data.source = c.getSrc();
      data.dat.append((char *)c.getDat().begin(), c.getDat().size());
-      data.hex_dat = data.dat.toHex(' ').toUpper();
      data.id = QString("%1:%2").arg(data.source).arg(data.address, 1, 16);
-      data.ts = (event.getLogMonoTime() - replay->routeStartTime()) / (double)1e9;  // seconds
+      data.ts = current_sec;
    }
-    emit received(can);
+    emit received(msgs_buf);
  }
+  return true;
+}
+
+void Parser::seekTo(double ts) {
+  seeking = true;
+  replay->seekTo(ts, false);
+  seeking = false;
 }

 void Parser::addNewMsg(const Msg &msg) {
--- a/tools/cabana/parser.h
+++ b/tools/cabana/parser.h
@@ -6,7 +6,6 @@
 #include <QApplication>
 #include <QHash>
 #include <QObject>
-#include <QThread>

 #include "opendbc/can/common.h"
 #include "opendbc/can/common_dbc.h"
@@ -22,7 +21,6 @@ struct CanData {
  uint16_t bus_time;
  uint8_t source;
  QByteArray dat;
-  QString hex_dat;
 };

 class Parser : public QObject {
@@ -32,11 +30,13 @@ public:
  Parser(QObject *parent);
  ~Parser();
  static uint32_t addressFromId(const QString &id);
+  bool eventFilter(const Event *event);
  bool loadRoute(const QString &route, const QString &data_dir, bool use_qcam);
  void openDBC(const QString &name);
  void saveDBC(const QString &name) {}
  void addNewMsg(const Msg &msg);
  void removeSignal(const QString &id, const QString &sig_name);
+  void seekTo(double ts);
  const Signal *getSig(const QString &id, const QString &sig_name);
  void setRange(double min, double max);
  void resetRange();
@@ -66,13 +66,11 @@ public:
  QList<CanData> history_log;

 protected:
-  void recvThread();
  void process(std::vector<CanData> can);
  void segmentsMerged();

-  double current_sec = 0.;
-  std::atomic<bool> exit = false;
-  QThread *thread;
+  std::atomic<double> current_sec = 0.;
+  std::atomic<bool> seeking = false;
  QString dbc_name;
  double begin_sec = 0;
  double end_sec = 0;
@@ -82,6 +80,7 @@ protected:
  DBC *dbc = nullptr;
  std::map<uint32_t, const Msg *> msg_map;
  QString current_msg_id;
+  std::vector<CanData> msgs_buf;
 };

 Q_DECLARE_METATYPE(std::vector<CanData>);
@@ -89,5 +88,8 @@ Q_DECLARE_METATYPE(std::vector<CanData>);
 // TODO: Add helper function in dbc.h
 int bigEndianStartBitsIndex(int start_bit);
 int bigEndianBitIndex(int index);
+inline QString toHex(const QByteArray &dat) {
+  return dat.toHex(' ').toUpper();
+}

 extern Parser *parser;
--- a/tools/cabana/videowidget.cc
+++ b/tools/cabana/videowidget.cc
@@ -3,6 +3,7 @@
 #include <QButtonGroup>
 #include <QDateTime>
 #include <QHBoxLayout>
+#include <QMouseEvent>
 #include <QPushButton>
 #include <QVBoxLayout>

@@ -15,6 +16,7 @@ inline QString formatTime(int seconds) {
 VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) {
  QVBoxLayout *main_layout = new QVBoxLayout(this);

+  // TODO: figure out why the CameraViewWidget crashed occasionally.
  cam_widget = new CameraViewWidget("camerad", VISION_STREAM_ROAD, false, this);
  cam_widget->setFixedSize(parent->width(), parent->width() / 1.596);
  main_layout->addWidget(cam_widget);
@@ -24,16 +26,12 @@ VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) {
  time_label = new QLabel("00:00");
  slider_layout->addWidget(time_label);

-  slider = new QSlider(Qt::Horizontal, this);
-  QObject::connect(slider, &QSlider::sliderMoved, [=]() {
-    time_label->setText(formatTime(slider->value()));
-  });
-  slider->setSingleStep(1);
+  slider = new Slider(this);
+  slider->setSingleStep(0);
+  slider->setMinimum(0);
+  slider->setTickInterval(60);
+  slider->setTickPosition(QSlider::TicksBelow);
  slider->setMaximum(parser->replay->totalSeconds());
-  QObject::connect(slider, &QSlider::sliderReleased, [=]() {
-    time_label->setText(formatTime(slider->value()));
-    parser->replay->seekTo(slider->value(), false);
-  });
  slider_layout->addWidget(slider);

  total_time_label = new QLabel(formatTime(parser->replay->totalSeconds()));
@@ -45,11 +43,6 @@ VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) {
  QHBoxLayout *control_layout = new QHBoxLayout();
  QPushButton *play = new QPushButton("⏸");
  play->setStyleSheet("font-weight:bold");
-  QObject::connect(play, &QPushButton::clicked, [=]() {
-    bool is_paused = parser->replay->isPaused();
-    play->setText(is_paused ? "⏸" : "▶");
-    parser->replay->pause(!is_paused);
-  });
  control_layout->addWidget(play);

  QButtonGroup *group = new QButtonGroup(this);
@@ -68,6 +61,20 @@ VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) {

  QObject::connect(parser, &Parser::rangeChanged, this, &VideoWidget::rangeChanged);
  QObject::connect(parser, &Parser::updated, this, &VideoWidget::updateState);
+  QObject::connect(slider, &QSlider::sliderMoved, [=]() { time_label->setText(formatTime(slider->value())); });
+  QObject::connect(slider, &QSlider::sliderReleased, [this]() { setPosition(slider->value()); });
+  QObject::connect(slider, &Slider::setPosition, this, &VideoWidget::setPosition);
+
+  QObject::connect(play, &QPushButton::clicked, [=]() {
+    bool is_paused = parser->replay->isPaused();
+    play->setText(is_paused ? "⏸" : "▶");
+    parser->replay->pause(!is_paused);
+  });
+}
+
+void VideoWidget::setPosition(int value) {
+  time_label->setText(formatTime(value));
+  parser->seekTo(value);
 }

 void VideoWidget::rangeChanged(double min, double max) {
@@ -77,6 +84,7 @@ void VideoWidget::rangeChanged(double min, double max) {
  }
  time_label->setText(formatTime(min));
  total_time_label->setText(formatTime(max));
+  slider->setMinimum(min);
  slider->setMaximum(max);
  slider->setValue(parser->currentSec());
 }
@@ -88,3 +96,17 @@ void VideoWidget::updateState() {
    slider->setValue(current_sec);
  }
 }
+
+// Slider
+// TODO: show timeline bar like what replay did.
+Slider::Slider(QWidget *parent) : QSlider(Qt::Horizontal, parent) {
+}
+
+void Slider::mousePressEvent(QMouseEvent *e) {
+  QSlider::mousePressEvent(e);
+  if (e->button() == Qt::LeftButton && !isSliderDown()) {
+    int value = minimum() + ((maximum() - minimum()) * e->x()) / width();
+    setValue(value);
+    emit setPosition(value);
+  }
+}
--- a/tools/cabana/videowidget.h
+++ b/tools/cabana/videowidget.h
@@ -6,6 +6,17 @@

 #include "selfdrive/ui/qt/widgets/cameraview.h"

+class Slider : public QSlider {
+  Q_OBJECT
+
+public:
+  Slider(QWidget *parent);
+  void mousePressEvent(QMouseEvent* e) override;
+
+signals:
+  void setPosition(int value);
+};
+
 class VideoWidget : public QWidget {
  Q_OBJECT

@@ -15,8 +26,9 @@ public:
 protected:
  void rangeChanged(double min, double max);
  void updateState();
+  void setPosition(int value);

  CameraViewWidget *cam_widget;
  QLabel *time_label, *total_time_label;
-  QSlider *slider;
+  Slider *slider;
 };
--- a/tools/replay/replay.cc
+++ b/tools/replay/replay.cc
@@ -325,6 +325,8 @@ void Replay::startStream(const Segment *cur_segment) {
 }

 void Replay::publishMessage(const Event *e) {
+  if (event_filter && event_filter(e, filter_opaque)) return;
+
  if (sm == nullptr) {
    auto bytes = e->bytes();
    int ret = pm->send(sockets_[e->which], (capnp::byte *)bytes.begin(), bytes.size());
--- a/tools/replay/replay.h
+++ b/tools/replay/replay.h
@@ -32,6 +32,7 @@ enum class FindFlag {
 };

 enum class TimelineType { None, Engaged, AlertInfo, AlertWarning, AlertCritical, UserFlag };
+typedef bool (*replayEventFilter)(const Event *, void *);

 class Replay : public QObject {
  Q_OBJECT
@@ -47,6 +48,13 @@ public:
  void seekToFlag(FindFlag flag);
  void seekTo(double seconds, bool relative);
  inline bool isPaused() const { return paused_; }
+  // the filter is called in streaming thread.try to return quickly from it to avoid blocking streaming.
+  // the filter function must return true if the event should be filtered.
+  // otherwise it must return false.
+  inline void installEventFilter(replayEventFilter filter, void *opaque) {
+    filter_opaque = opaque;
+    event_filter = filter;
+  }
  inline bool hasFlag(REPLAY_FLAGS flag) const { return flags_ & flag; }
  inline void addFlag(REPLAY_FLAGS flag) { flags_ |= flag; }
  inline void removeFlag(REPLAY_FLAGS flag) { flags_ &= ~flag; }
@@ -119,4 +127,6 @@ protected:
  std::set<cereal::Event::Which> allow_list;
  std::string car_fingerprint_;
  float speed_ = 1.0;
+  replayEventFilter event_filter = nullptr;
+  void *filter_opaque = nullptr;
 };