diff --git a/selfdrive/ui/replay/main.cc b/selfdrive/ui/replay/main.cc index 062837885b1c67..ebd0d0f0aca009 100644 --- a/selfdrive/ui/replay/main.cc +++ b/selfdrive/ui/replay/main.cc @@ -59,6 +59,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 index 8155eb3456744f..f5744227cf19d6 100644 --- 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>(); new_events_ = std::make_unique>(); + qRegisterMetaType("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,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 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 index a3e6efaadbcb39..80a6f5945720bf 100644 --- 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 @@ -35,14 +40,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> SegmentMap; + std::optional find(FindFlag flag); void startStream(const Segment *cur_segment); void stream(); void setCurrentSegment(int n);