replay: Add next engagement / disengagement jump capabilities (#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
commaai · Jan 4, 2022 · d2948f2 · d2948f2
1 parent 9cb0ebe
commit d2948f2
Show file tree

Hide file tree

Showing 3 changed files with 63 additions and 0 deletions.
diff --git a/selfdrive/ui/replay/main.cc b/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') {

diff --git a/selfdrive/ui/replay/replay.cc b/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");

diff --git a/selfdrive/ui/replay/replay.h b/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);