diff --git a/selfdrive/ui/replay/logreader.cc b/selfdrive/ui/replay/logreader.cc index e15faef88d..233c35a257 100644 --- a/selfdrive/ui/replay/logreader.cc +++ b/selfdrive/ui/replay/logreader.cc @@ -38,27 +38,20 @@ bool LogReader::load(const std::string &file) { return false; } - auto insertEidx = [&](CameraType type, const cereal::EncodeIndex::Reader &e) { - eidx[type][e.getFrameId()] = {e.getSegmentNum(), e.getSegmentId()}; - }; - kj::ArrayPtr words((const capnp::word *)raw_.data(), raw_.size() / sizeof(capnp::word)); while (words.size() > 0) { try { std::unique_ptr evt = std::make_unique(words); - switch (evt->which) { - case cereal::Event::ROAD_ENCODE_IDX: - insertEidx(RoadCam, evt->event.getRoadEncodeIdx()); - break; - case cereal::Event::DRIVER_ENCODE_IDX: - insertEidx(DriverCam, evt->event.getDriverEncodeIdx()); - break; - case cereal::Event::WIDE_ROAD_ENCODE_IDX: - insertEidx(WideRoadCam, evt->event.getWideRoadEncodeIdx()); - break; - default: - break; + + // Add encodeIdx packet again as a frame packet for the video stream + if (evt->which == cereal::Event::ROAD_ENCODE_IDX || + evt->which == cereal::Event::DRIVER_ENCODE_IDX || + evt->which == cereal::Event::WIDE_ROAD_ENCODE_IDX) { + + std::unique_ptr frame_evt = std::make_unique(words, true); + events.push_back(frame_evt.release()); } + words = kj::arrayPtr(evt->reader.getEnd(), words.end()); events.push_back(evt.release()); } catch (const kj::Exception &e) { diff --git a/selfdrive/ui/replay/logreader.h b/selfdrive/ui/replay/logreader.h index cca8ef6216..86962f6ee0 100644 --- a/selfdrive/ui/replay/logreader.h +++ b/selfdrive/ui/replay/logreader.h @@ -2,6 +2,7 @@ #include #include +#include #include #include "cereal/gen/cpp/log.capnp.h" @@ -9,10 +10,7 @@ const CameraType ALL_CAMERAS[] = {RoadCam, DriverCam, WideRoadCam}; const int MAX_CAMERAS = std::size(ALL_CAMERAS); -struct EncodeIdx { - int segmentNum; - uint32_t frameEncodeId; -}; + class Event { public: Event(cereal::Event::Which which, uint64_t mono_time) : reader(kj::ArrayPtr{}) { @@ -20,11 +18,35 @@ public: this->which = which; this->mono_time = mono_time; } - Event(const kj::ArrayPtr &amsg) : reader(amsg) { + Event(const kj::ArrayPtr &amsg, bool frame=false) : reader(amsg), frame(frame) { words = kj::ArrayPtr(amsg.begin(), reader.getEnd()); event = reader.getRoot(); which = event.which(); mono_time = event.getLogMonoTime(); + + // 1) Send video data at t=timestampEof/timestampSof + // 2) Send encodeIndex packet at t=logMonoTime + if (frame) { + cereal::EncodeIndex::Reader idx; + if (which == cereal::Event::ROAD_ENCODE_IDX) { + idx = event.getRoadEncodeIdx(); + } else if (which == cereal::Event::DRIVER_ENCODE_IDX) { + idx = event.getDriverEncodeIdx(); + } else if (which == cereal::Event::WIDE_ROAD_ENCODE_IDX) { + idx = event.getWideRoadEncodeIdx(); + } else { + assert(false); + } + + // C2 only has eof set, and some older routes have neither + uint64_t sof = idx.getTimestampSof(); + uint64_t eof = idx.getTimestampEof(); + if (sof > 0) { + mono_time = sof; + } else if (eof > 0) { + mono_time = eof; + } + } } inline kj::ArrayPtr bytes() const { return words.asBytes(); } @@ -39,6 +61,7 @@ public: cereal::Event::Reader event; capnp::FlatArrayMessageReader reader; kj::ArrayPtr words; + bool frame; }; class LogReader { @@ -48,7 +71,6 @@ public: bool load(const std::string &file); std::vector events; - std::unordered_map eidx[MAX_CAMERAS] = {}; private: std::vector raw_; diff --git a/selfdrive/ui/replay/replay.cc b/selfdrive/ui/replay/replay.cc index ee40a4abac..e8bdedd062 100644 --- a/selfdrive/ui/replay/replay.cc +++ b/selfdrive/ui/replay/replay.cc @@ -92,7 +92,7 @@ void Replay::seekTo(int seconds, bool relative) { cur_mono_time_ = route_start_ts_ + seconds * 1e9; setCurrentSegment(segment); - bool segment_loaded = std::find(segments_merged_.begin(), segments_merged_.end(), segment) != segments_merged_.end(); + bool segment_loaded = std::find(segments_merged_.begin(), segments_merged_.end(), segment) != segments_merged_.end(); // return false if segment changed and not loaded yet return !segment_changed || segment_loaded; }); @@ -150,19 +150,14 @@ void Replay::mergeSegments(int cur_seg, int end_idx) { // merge & sort events std::vector *new_events = new std::vector(); - std::unordered_map *new_eidx = new std::unordered_map[MAX_CAMERAS]; for (int n : segments_need_merge) { auto &log = segments_[n]->log; auto middle = new_events->insert(new_events->end(), log->events.begin(), log->events.end()); std::inplace_merge(new_events->begin(), middle, new_events->end(), Event::lessThan()); - for (CameraType cam_type : ALL_CAMERAS) { - new_eidx[cam_type].insert(log->eidx[cam_type].begin(), log->eidx[cam_type].end()); - } } // update events auto prev_events = events_; - auto prev_eidx = eidx_; updateEvents([&]() { if (route_start_ts_ == 0) { // get route start time from initData @@ -174,12 +169,10 @@ void Replay::mergeSegments(int cur_seg, int end_idx) { } events_ = new_events; - eidx_ = new_eidx; segments_merged_ = segments_need_merge; return true; }); delete prev_events; - delete[] prev_eidx; } // free segments out of current semgnt window. @@ -239,14 +232,15 @@ void Replay::stream() { } // publish frame - // TODO: publish all frames - if (evt->which == cereal::Event::ROAD_CAMERA_STATE) { - auto it_ = eidx_[RoadCam].find(evt->event.getRoadCameraState().getFrameId()); - if (it_ != eidx_[RoadCam].end()) { - EncodeIdx &e = it_->second; - auto &seg = segments_[e.segmentNum]; - if (seg && seg->isLoaded()) { + if (evt->frame) { + // TODO: publish all frames + if (evt->which == cereal::Event::ROAD_ENCODE_IDX) { + auto idx = evt->event.getRoadEncodeIdx(); + auto &seg = segments_[idx.getSegmentNum()]; + + if (seg && seg->isLoaded() && idx.getType() == cereal::EncodeIndex::Type::FULL_H_E_V_C) { auto &frm = seg->frames[RoadCam]; + if (vipc_server == nullptr) { cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); @@ -257,7 +251,7 @@ void Replay::stream() { vipc_server->start_listener(); } - uint8_t *dat = frm->get(e.frameEncodeId); + uint8_t *dat = frm->get(idx.getSegmentId()); if (dat) { VisionIpcBufExtra extra = {}; VisionBuf *buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_RGB_BACK); @@ -266,14 +260,15 @@ void Replay::stream() { } } } - } // publish msg - if (sm == nullptr) { - auto bytes = evt->bytes(); - pm->send(type.c_str(), (capnp::byte *)bytes.begin(), bytes.size()); } else { - sm->update_msgs(nanos_since_boot(), {{type, evt->event}}); + if (sm == nullptr) { + auto bytes = evt->bytes(); + pm->send(type.c_str(), (capnp::byte *)bytes.begin(), bytes.size()); + } else { + sm->update_msgs(nanos_since_boot(), {{type, evt->event}}); + } } } } diff --git a/selfdrive/ui/replay/replay.h b/selfdrive/ui/replay/replay.h index 7987fe6a7a..3e6f5839ee 100644 --- a/selfdrive/ui/replay/replay.h +++ b/selfdrive/ui/replay/replay.h @@ -48,7 +48,6 @@ protected: uint64_t route_start_ts_ = 0; uint64_t cur_mono_time_ = 0; std::vector *events_ = nullptr; - std::unordered_map *eidx_ = nullptr; std::vector> segments_; std::vector segments_merged_;