replay: Add next engagement / disengagement jump capabilities (commaai#23248)

* seek engament/disengament in qlogs

* cleanup

* cleanup

* little more

* add output

* remove qinfo

* fix typo

* do nothing if tm less than current mono time

* const

* short variable name

* lgtm

Author: deanlee

selfdrive/ui/replay/main.cc

@@ -55,6 +55,10 @@ void keyboardThread(Replay *replay_) {
         qDebug() << "invalid argument";
       }
       getch();  // remove \n from entering seek
+    } else if (c == 'e') {
+      replay_->seekToFlag(FindFlag::nextEngagement);
+    } else if (c == 'd') {
+      replay_->seekToFlag(FindFlag::nextDisEngagement);
     } else if (c == 'm') {
       replay_->seekTo(+60, true);
     } else if (c == 'M') {

selfdrive/ui/replay/replay.cc

@@ -31,7 +31,9 @@ Replay::Replay(QString route, QStringList allow, QStringList block, SubMaster *s
   events_ = std::make_unique<std::vector<Event *>>();
   new_events_ = std::make_unique<std::vector<Event *>>();
 
+  qRegisterMetaType<FindFlag>("FindFlag");
   connect(this, &Replay::seekTo, this, &Replay::doSeek);
+  connect(this, &Replay::seekToFlag, this, &Replay::doSeekToFlag);
   connect(this, &Replay::segmentChanged, this, &Replay::queueSegment);
 }
 
@@ -109,6 +111,55 @@ void Replay::doSeek(int seconds, bool relative) {
   queueSegment();
 }
 
+void Replay::doSeekToFlag(FindFlag flag) {
+  if (flag == FindFlag::nextEngagement) {
+    qInfo() << "seeking to the next engagement...";
+  } else if (flag == FindFlag::nextDisEngagement) {
+    qInfo() << "seeking to the disengagement...";
+  }
+
+  updateEvents([&]() {
+    if (auto next = find(flag)) {
+      uint64_t tm = *next - 2 * 1e9;  // seek to 2 seconds before next
+      if (tm <= cur_mono_time_) {
+        return true;
+      }
+
+      cur_mono_time_ = tm;
+      current_segment_ = currentSeconds() / 60;
+      return isSegmentMerged(current_segment_);
+    } else {
+      qWarning() << "seeking failed";
+      return true;
+    }
+  });
+
+  queueSegment();
+}
+
+std::optional<uint64_t> Replay::find(FindFlag flag) {
+  // Search in all segments
+  for (const auto &[n, _] : segments_) {
+    if (n < current_segment_) continue;
+
+    LogReader log;
+    bool cache_to_local = true;  // cache qlog to local for fast seek
+    if (!log.load(route_->at(n).qlog.toStdString(), nullptr, cache_to_local, 0, 3)) continue;
+
+    for (const Event *e : log.events) {
+      if (e->mono_time > cur_mono_time_ && e->which == cereal::Event::Which::CONTROLS_STATE) {
+        const auto cs = e->event.getControlsState();
+        if (flag == FindFlag::nextEngagement && cs.getEnabled()) {
+          return e->mono_time;
+        } else if (flag == FindFlag::nextDisEngagement && !cs.getEnabled()) {
+          return e->mono_time;
+        }
+      }
+    }
+  }
+  return std::nullopt;
+}
+
 void Replay::pause(bool pause) {
   updateEvents([=]() {
     qInfo() << (pause ? "paused..." : "resuming");

selfdrive/ui/replay/replay.h

@@ -21,6 +21,11 @@ enum REPLAY_FLAGS {
   REPLAY_FLAG_NO_VIPC = 0x0400,
 };
 
+enum class FindFlag {
+  nextEngagement,
+  nextDisEngagement
+};
+
 class Replay : public QObject {
   Q_OBJECT
 
@@ -39,14 +44,17 @@ class Replay : public QObject {
 signals:
   void segmentChanged();
   void seekTo(int seconds, bool relative);
+  void seekToFlag(FindFlag flag);
 
 protected slots:
   void queueSegment();
   void doSeek(int seconds, bool relative);
+  void doSeekToFlag(FindFlag flag);
   void segmentLoadFinished(bool sucess);
 
 protected:
   typedef std::map<int, std::unique_ptr<Segment>> SegmentMap;
+  std::optional<uint64_t> find(FindFlag flag);
   void startStream(const Segment *cur_segment);
   void stream();
   void setCurrentSegment(int n);