dragonpilot v2023.07.05

version: dragonpilot v0.9.4 release
date: 2023-07-05T18:59:41
dp-dev(priv) master commit: 7b0489feab40283a422d2201ef95a9cb8c06f6cd

tools/replay/camera.h

@@ -0,0 +1,38 @@
+#pragma once
+
+#include <unistd.h>
+#include "cereal/visionipc/visionipc_server.h"
+#include "common/queue.h"
+#include "tools/replay/framereader.h"
+#include "tools/replay/logreader.h"
+
+class CameraServer {
+public:
+  CameraServer(std::pair<int, int> camera_size[MAX_CAMERAS] = nullptr);
+  ~CameraServer();
+  void pushFrame(CameraType type, FrameReader* fr, const cereal::EncodeIndex::Reader& eidx);
+  void waitForSent();
+
+protected:
+  struct Camera {
+    CameraType type;
+    VisionStreamType stream_type;
+    int width;
+    int height;
+    std::thread thread;
+    SafeQueue<std::pair<FrameReader*, const cereal::EncodeIndex::Reader>> queue;
+    int cached_id = -1;
+    int cached_seg = -1;
+    VisionBuf * cached_buf;
+  };
+  void startVipcServer();
+  void cameraThread(Camera &cam);
+
+  Camera cameras_[MAX_CAMERAS] = {
+      {.type = RoadCam, .stream_type = VISION_STREAM_ROAD},
+      {.type = DriverCam, .stream_type = VISION_STREAM_DRIVER},
+      {.type = WideRoadCam, .stream_type = VISION_STREAM_WIDE_ROAD},
+  };
+  std::atomic<int> publishing_ = 0;
+  std::unique_ptr<VisionIpcServer> vipc_server_;
+};

tools/replay/consoleui.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <array>
+#include <QBasicTimer>
+#include <QObject>
+#include <QSocketNotifier>
+#include <QTimer>
+#include <QTimerEvent>
+
+#include "tools/replay/replay.h"
+#include <ncurses.h>
+
+class ConsoleUI : public QObject {
+  Q_OBJECT
+
+public:
+  ConsoleUI(Replay *replay, QObject *parent = 0);
+  ~ConsoleUI();
+
+private:
+  void initWindows();
+  void handleKey(char c);
+  void displayHelp();
+  void displayTimelineDesc();
+  void updateTimeline();
+  void updateSummary();
+  void updateStatus();
+  void pauseReplay(bool pause);
+
+  enum Status { Waiting, Playing, Paused };
+  enum Win { Title, Stats, Log, LogBorder, DownloadBar, Timeline, TimelineDesc, Help, CarState, Max};
+  std::array<WINDOW*, Win::Max> w{};
+  SubMaster sm;
+  Replay *replay;
+  QBasicTimer getch_timer;
+  QTimer sm_timer;
+  QSocketNotifier notifier{0, QSocketNotifier::Read, this};
+  int max_width, max_height;
+  Status status = Status::Waiting;
+
+signals:
+  void updateProgressBarSignal(uint64_t cur, uint64_t total, bool success);
+  void logMessageSignal(ReplyMsgType type, const QString &msg);
+
+private slots:
+  void readyRead();
+  void timerEvent(QTimerEvent *ev);
+  void updateProgressBar(uint64_t cur, uint64_t total, bool success);
+  void logMessage(ReplyMsgType type, const QString &msg);
+};

tools/replay/filereader.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include <atomic>
+#include <string>
+
+class FileReader {
+public:
+  FileReader(bool cache_to_local, size_t chunk_size = 0, int retries = 3)
+      : cache_to_local_(cache_to_local), chunk_size_(chunk_size), max_retries_(retries) {}
+  virtual ~FileReader() {}
+  std::string read(const std::string &file, std::atomic<bool> *abort = nullptr);
+
+private:
+  std::string download(const std::string &url, std::atomic<bool> *abort);
+  size_t chunk_size_;
+  int max_retries_;
+  bool cache_to_local_;
+};
+
+std::string cacheFilePath(const std::string &url);

tools/replay/framereader.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "tools/replay/filereader.h"
+
+extern "C" {
+#include <libavcodec/avcodec.h>
+#include <libavformat/avformat.h>
+}
+
+struct AVFrameDeleter {
+  void operator()(AVFrame* frame) const { av_frame_free(&frame); }
+};
+
+class FrameReader {
+public:
+  FrameReader();
+  ~FrameReader();
+  bool load(const std::string &url, bool no_hw_decoder = false, std::atomic<bool> *abort = nullptr, bool local_cache = false,
+            int chunk_size = -1, int retries = 0);
+  bool load(const std::byte *data, size_t size, bool no_hw_decoder = false, std::atomic<bool> *abort = nullptr);
+  bool get(int idx, uint8_t *yuv);
+  int getYUVSize() const { return width * height * 3 / 2; }
+  size_t getFrameCount() const { return packets.size(); }
+  bool valid() const { return valid_; }
+
+  int width = 0, height = 0;
+  int aligned_width = 0, aligned_height = 0;
+
+private:
+  bool initHardwareDecoder(AVHWDeviceType hw_device_type);
+  bool decode(int idx, uint8_t *yuv);
+  AVFrame * decodeFrame(AVPacket *pkt);
+  bool copyBuffers(AVFrame *f, uint8_t *yuv);
+
+  std::vector<AVPacket*> packets;
+  std::unique_ptr<AVFrame, AVFrameDeleter>av_frame_, hw_frame;
+  AVFormatContext *input_ctx = nullptr;
+  AVCodecContext *decoder_ctx = nullptr;
+  int key_frames_count_ = 0;
+  bool valid_ = false;
+  AVIOContext *avio_ctx_ = nullptr;
+
+  AVPixelFormat hw_pix_fmt = AV_PIX_FMT_NONE;
+  AVBufferRef *hw_device_ctx = nullptr;
+  int prev_idx = -1;
+  inline static std::atomic<bool> has_hw_decoder = true;
+};

tools/replay/logreader.h

@@ -0,0 +1,67 @@
+#pragma once
+
+#if __has_include(<memory_resource>)
+#define HAS_MEMORY_RESOURCE 1
+#include <memory_resource>
+#endif
+
+#include <set>
+
+#include "cereal/gen/cpp/log.capnp.h"
+#include "system/camerad/cameras/camera_common.h"
+#include "tools/replay/filereader.h"
+
+const CameraType ALL_CAMERAS[] = {RoadCam, DriverCam, WideRoadCam};
+const int MAX_CAMERAS = std::size(ALL_CAMERAS);
+const int DEFAULT_EVENT_MEMORY_POOL_BLOCK_SIZE = 65000;
+
+class Event {
+public:
+  Event(cereal::Event::Which which, uint64_t mono_time) : reader(kj::ArrayPtr<capnp::word>{}) {
+    // construct a dummy Event for binary search, e.g std::upper_bound
+    this->which = which;
+    this->mono_time = mono_time;
+  }
+  Event(const kj::ArrayPtr<const capnp::word> &amsg, bool frame = false);
+  inline kj::ArrayPtr<const capnp::byte> bytes() const { return words.asBytes(); }
+
+  struct lessThan {
+    inline bool operator()(const Event *l, const Event *r) {
+      return l->mono_time < r->mono_time || (l->mono_time == r->mono_time && l->which < r->which);
+    }
+  };
+
+#if HAS_MEMORY_RESOURCE
+  void *operator new(size_t size, std::pmr::monotonic_buffer_resource *mbr) {
+    return mbr->allocate(size);
+  }
+  void operator delete(void *ptr) {
+    // No-op. memory used by EventMemoryPool increases monotonically until the logReader is destroyed.
+  }
+#endif
+
+  uint64_t mono_time;
+  cereal::Event::Which which;
+  cereal::Event::Reader event;
+  capnp::FlatArrayMessageReader reader;
+  kj::ArrayPtr<const capnp::word> words;
+  bool frame;
+};
+
+class LogReader {
+public:
+  LogReader(size_t memory_pool_block_size = DEFAULT_EVENT_MEMORY_POOL_BLOCK_SIZE);
+  ~LogReader();
+  bool load(const std::string &url, std::atomic<bool> *abort = nullptr, const std::set<cereal::Event::Which> &allow = {},
+            bool local_cache = false, int chunk_size = -1, int retries = 0);
+  bool load(const std::byte *data, size_t size, std::atomic<bool> *abort = nullptr);
+  std::vector<Event*> events;
+
+private:
+  bool parse(const std::set<cereal::Event::Which> &allow, std::atomic<bool> *abort);
+  std::string raw_;
+#ifdef HAS_MEMORY_RESOURCE
+  std::pmr::monotonic_buffer_resource *mbr_ = nullptr;
+  void *pool_buffer_ = nullptr;
+#endif
+};

tools/replay/replay.h

@@ -0,0 +1,141 @@
+#pragma once
+
+#include <optional>
+
+#include <QThread>
+
+#include "tools/replay/camera.h"
+#include "tools/replay/route.h"
+
+const QString DEMO_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36";
+
+// one segment uses about 100M of memory
+constexpr int MIN_SEGMENTS_CACHE = 5;
+
+enum REPLAY_FLAGS {
+  REPLAY_FLAG_NONE = 0x0000,
+  REPLAY_FLAG_DCAM = 0x0002,
+  REPLAY_FLAG_ECAM = 0x0004,
+  REPLAY_FLAG_NO_LOOP = 0x0010,
+  REPLAY_FLAG_NO_FILE_CACHE = 0x0020,
+  REPLAY_FLAG_QCAMERA = 0x0040,
+  REPLAY_FLAG_NO_HW_DECODER = 0x0100,
+  REPLAY_FLAG_FULL_SPEED = 0x0200,
+  REPLAY_FLAG_NO_VIPC = 0x0400,
+  REPLAY_FLAG_ALL_SERVICES = 0x0800,
+};
+
+enum class FindFlag {
+  nextEngagement,
+  nextDisEngagement,
+  nextUserFlag,
+  nextInfo,
+  nextWarning,
+  nextCritical
+};
+
+enum class TimelineType { None, Engaged, AlertInfo, AlertWarning, AlertCritical, UserFlag };
+typedef bool (*replayEventFilter)(const Event *, void *);
+
+class Replay : public QObject {
+  Q_OBJECT
+
+public:
+  Replay(QString route, QStringList allow, QStringList block, QStringList base_blacklist, SubMaster *sm = nullptr,
+          uint32_t flags = REPLAY_FLAG_NONE, QString data_dir = "", QObject *parent = 0);
+  ~Replay();
+  bool load();
+  void start(int seconds = 0);
+  void stop();
+  void pause(bool pause);
+  void seekToFlag(FindFlag flag);
+  void seekTo(double seconds, bool relative);
+  inline bool isPaused() const { return paused_; }
+  // the filter is called in streaming thread.try to return quickly from it to avoid blocking streaming.
+  // the filter function must return true if the event should be filtered.
+  // otherwise it must return false.
+  inline void installEventFilter(replayEventFilter filter, void *opaque) {
+    filter_opaque = opaque;
+    event_filter = filter;
+  }
+  inline int segmentCacheLimit() const { return segment_cache_limit; }
+  inline void setSegmentCacheLimit(int n) { segment_cache_limit = std::max(MIN_SEGMENTS_CACHE, n); }
+  inline bool hasFlag(REPLAY_FLAGS flag) const { return flags_ & flag; }
+  inline void addFlag(REPLAY_FLAGS flag) { flags_ |= flag; }
+  inline void removeFlag(REPLAY_FLAGS flag) { flags_ &= ~flag; }
+  inline const Route* route() const { return route_.get(); }
+  inline double currentSeconds() const { return double(cur_mono_time_ - route_start_ts_) / 1e9; }
+  inline QDateTime currentDateTime() const { return route_->datetime().addSecs(currentSeconds()); }
+  inline uint64_t routeStartTime() const { return route_start_ts_; }
+  inline int toSeconds(uint64_t mono_time) const { return (mono_time - route_start_ts_) / 1e9; }
+  inline int totalSeconds() const { return (!segments_.empty()) ? (segments_.rbegin()->first + 1) * 60 : 0; }
+  inline void setSpeed(float speed) { speed_ = speed; }
+  inline float getSpeed() const { return speed_; }
+  inline const std::vector<Event *> *events() const { return events_.get(); }
+  inline const std::map<int, std::unique_ptr<Segment>> &segments() const { return segments_; };
+  inline const std::string &carFingerprint() const { return car_fingerprint_; }
+  inline const std::vector<std::tuple<int, int, TimelineType>> getTimeline() {
+    std::lock_guard lk(timeline_lock);
+    return timeline;
+  }
+
+signals:
+  void streamStarted();
+  void segmentsMerged();
+  void seekedTo(double sec);
+
+protected slots:
+  void segmentLoadFinished(bool success);
+
+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);
+  void queueSegment();
+  void mergeSegments(const SegmentMap::iterator &begin, const SegmentMap::iterator &end);
+  void updateEvents(const std::function<bool()>& lambda);
+  void publishMessage(const Event *e);
+  void publishFrame(const Event *e);
+  void buildTimeline();
+  inline bool isSegmentMerged(int n) {
+    return std::find(segments_merged_.begin(), segments_merged_.end(), n) != segments_merged_.end();
+  }
+
+  QThread *stream_thread_ = nullptr;
+
+  // logs
+  std::mutex stream_lock_;
+  std::condition_variable stream_cv_;
+  std::atomic<bool> updating_events_ = false;
+  std::atomic<int> current_segment_ = 0;
+  SegmentMap segments_;
+  // the following variables must be protected with stream_lock_
+  std::atomic<bool> exit_ = false;
+  bool paused_ = false;
+  bool events_updated_ = false;
+  uint64_t route_start_ts_ = 0;
+  std::atomic<uint64_t> cur_mono_time_ = 0;
+  std::unique_ptr<std::vector<Event *>> events_;
+  std::unique_ptr<std::vector<Event *>> new_events_;
+  std::vector<int> segments_merged_;
+
+  // messaging
+  SubMaster *sm = nullptr;
+  std::unique_ptr<PubMaster> pm;
+  std::vector<const char*> sockets_;
+  std::unique_ptr<Route> route_;
+  std::unique_ptr<CameraServer> camera_server_;
+  std::atomic<uint32_t> flags_ = REPLAY_FLAG_NONE;
+
+  std::mutex timeline_lock;
+  QFuture<void> timeline_future;
+  std::vector<std::tuple<int, int, TimelineType>> timeline;
+  std::set<cereal::Event::Which> allow_list;
+  std::string car_fingerprint_;
+  float speed_ = 1.0;
+  replayEventFilter event_filter = nullptr;
+  void *filter_opaque = nullptr;
+  int segment_cache_limit = MIN_SEGMENTS_CACHE;
+};

tools/replay/route.h

@@ -0,0 +1,72 @@
+#pragma once
+
+#include <QDateTime>
+#include <QFutureSynchronizer>
+
+#include "tools/replay/framereader.h"
+#include "tools/replay/logreader.h"
+#include "tools/replay/util.h"
+
+struct RouteIdentifier {
+  QString dongle_id;
+  QString timestamp;
+  int segment_id;
+  QString str;
+};
+
+struct SegmentFile {
+  QString rlog;
+  QString qlog;
+  QString road_cam;
+  QString driver_cam;
+  QString wide_road_cam;
+  QString qcamera;
+};
+
+class Route {
+public:
+  Route(const QString &route, const QString &data_dir = {});
+  bool load();
+  inline const QString &name() const { return route_.str; }
+  inline const QDateTime datetime() const { return date_time_; }
+  inline const QString &dir() const { return data_dir_; }
+  inline const RouteIdentifier &identifier() const { return route_; }
+  inline const std::map<int, SegmentFile> &segments() const { return segments_; }
+  inline const SegmentFile &at(int n) { return segments_.at(n); }
+  static RouteIdentifier parseRoute(const QString &str);
+
+protected:
+  bool loadFromLocal();
+  bool loadFromServer();
+  bool loadFromJson(const QString &json);
+  void addFileToSegment(int seg_num, const QString &file);
+  RouteIdentifier route_ = {};
+  QString data_dir_;
+  std::map<int, SegmentFile> segments_;
+  QDateTime date_time_;
+};
+
+class Segment : public QObject {
+  Q_OBJECT
+
+public:
+  Segment(int n, const SegmentFile &files, uint32_t flags, const std::set<cereal::Event::Which> &allow = {});
+  ~Segment();
+  inline bool isLoaded() const { return !loading_ && !abort_; }
+
+  const int seg_num = 0;
+  std::unique_ptr<LogReader> log;
+  std::unique_ptr<FrameReader> frames[MAX_CAMERAS] = {};
+
+signals:
+  void loadFinished(bool success);
+
+protected:
+  void loadFile(int id, const std::string file);
+
+  std::atomic<bool> abort_ = false;
+  std::atomic<int> loading_ = 0;
+  QFutureSynchronizer<void> synchronizer_;
+  uint32_t flags;
+  std::set<cereal::Event::Which> allow;
+};

tools/replay/util.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <atomic>
+#include <functional>
+#include <string>
+
+enum class ReplyMsgType {
+  Info,
+  Debug,
+  Warning,
+  Critical
+};
+
+typedef std::function<void(ReplyMsgType type, const std::string msg)> ReplayMessageHandler;
+void installMessageHandler(ReplayMessageHandler);
+void logMessage(ReplyMsgType type, const char* fmt, ...);
+
+#define rInfo(fmt, ...) ::logMessage(ReplyMsgType::Info, fmt,  ## __VA_ARGS__)
+#define rDebug(fmt, ...) ::logMessage(ReplyMsgType::Debug, fmt,  ## __VA_ARGS__)
+#define rWarning(fmt, ...) ::logMessage(ReplyMsgType::Warning, fmt,  ## __VA_ARGS__)
+#define rError(fmt, ...) ::logMessage(ReplyMsgType::Critical , fmt,  ## __VA_ARGS__)
+
+std::string sha256(const std::string &str);
+void precise_nano_sleep(long sleep_ns);
+std::string decompressBZ2(const std::string &in, std::atomic<bool> *abort = nullptr);
+std::string decompressBZ2(const std::byte *in, size_t in_size, std::atomic<bool> *abort = nullptr);
+std::string getUrlWithoutQuery(const std::string &url);
+size_t getRemoteFileSize(const std::string &url, std::atomic<bool> *abort = nullptr);
+std::string httpGet(const std::string &url, size_t chunk_size = 0, std::atomic<bool> *abort = nullptr);
+
+typedef std::function<void(uint64_t cur, uint64_t total, bool success)> DownloadProgressHandler;
+void installDownloadProgressHandler(DownloadProgressHandler);
+bool httpDownload(const std::string &url, const std::string &file, size_t chunk_size = 0, std::atomic<bool> *abort = nullptr);
+std::string formattedDataSize(size_t size);