seek engament/disengament in qlogs

commaai · Dec 16, 2021 · 04a1f6c · 04a1f6c
1 parent a793b94
commit 04a1f6c
Show file tree

Hide file tree

Showing 3 changed files with 64 additions and 0 deletions.
diff --git a/selfdrive/ui/replay/main.cc b/selfdrive/ui/replay/main.cc
@@ -69,6 +69,10 @@ void keyboardThread(Replay *replay_) {
       replay_->seekTo(-10, true);
     } else if (c == 'G') {
       replay_->seekTo(0, true);
+    } else if (c == 'e') {
+      replay_->seekToFlag(FindFlag::nextEngagement);
+    } else if (c == 'd') {
+      replay_->seekToFlag(FindFlag::nextDisEngagement);
     } else if (c == ' ') {
       replay_->pause(!replay_->isPaused());
     }

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);
 }
 
@@ -111,6 +113,54 @@ void Replay::doSeek(int seconds, bool relative) {
   queueSegment();
 }
 
+void Replay::doSeekToFlag(FindFlag flag) {
+  if (flag == FindFlag::nextEngagement) {
+    qInfo() << "seeking to next engagement...";  
+  } else {
+    qInfo() << "seeking to next disengagement...";  
+  }
+
+  updateEvents([&]() {
+    auto next = find(flag);
+    if (next) {
+      cur_mono_time_ = *next;
+      current_segment_ = currentSeconds() / 60;
+      return isSegmentMerged(current_segment_);
+    }
+    qWarning() << "seeking failed";
+    return true;
+  });
+
+  queueSegment();
+}
+
+std::optional<uint64_t> Replay::find(FindFlag flag) {
+  for (auto &[n, seg] : segments_) {
+    if (n < current_segment_) continue;
+
+    LogReader log;
+    if (log.load(route_->at(n).qlog.toStdString(), nullptr, true, 0, 3)) {
+      for (auto evt : log.events) {
+        if (evt->mono_time > cur_mono_time_) {
+          if (flag == FindFlag::nextEngagement) {
+            if (evt->which == cereal::Event::Which::CONTROLS_STATE && evt->event.getControlsState().getEnabled()) {
+              return evt->mono_time - 2 * 1e9;
+            }
+          } else if (flag == FindFlag::nextDisEngagement) {
+            if (evt->which == cereal::Event::Which::CONTROLS_STATE && !evt->event.getControlsState().getEnabled()) {
+              return evt->mono_time - 2 * 1e9;
+            }
+          }
+        }
+      }
+    }
+  }
+  return std::nullopt;
+}
+
+void Replay::nextDisengagement() {
+}
+
 void Replay::pause(bool pause) {
   updateEvents([=]() {
     qInfo() << (pause ? "paused..." : "resuming");

diff --git a/selfdrive/ui/replay/replay.h b/selfdrive/ui/replay/replay.h
@@ -19,6 +19,11 @@ enum REPLAY_FLAGS {
   REPLAY_FLAG_NO_CUDA = 0x0100,
 };
 
+enum class FindFlag {
+  nextEngagement,
+  nextDisEngagement
+};
+
 class Replay : public QObject {
   Q_OBJECT
 
@@ -31,18 +36,23 @@ class Replay : public QObject {
   void stop();
   void pause(bool pause);
   bool isPaused() const { return paused_; }
+  void nextEngagement();
+  void nextDisengagement();
 
 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);