diff --git a/selfdrive/ui/replay/main.cc b/selfdrive/ui/replay/main.cc index 062837885b1c67b..7ede23d0a5b22a6 100644 --- 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 index 8155eb3456744f4..a49eb6a47eea7ac 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,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 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 index a3e6efaadbcb39e..5cedab98b6015a3 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 @@ -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> SegmentMap; + std::optional find(FindFlag flag); void startStream(const Segment *cur_segment); void stream(); void setCurrentSegment(int n);