camerad: added replay camera (#21241)

* start refactor

* remove camera_frame_stream from files_common

* rename camera_pc to camera_replay

* continue

* loop one segment

* rename cam_frame_id to stream_frame_id

* apply review

* continue

* more

* publish camera state

* cleanup

* cleanup

* better comment

* delete s->pm in cameras_close()

* add function getFrameCount

* refactor loop

* fix typo

* restore freame stream

* disable roadcam

* dd

* move file

* merge master

* fix test case

* add todo

* white space

* remove from release files

* add files back to relase

* move framereader back to ui/replay

* merge master

test_replay

SConstruct

@@ -60,6 +60,7 @@ if arch == "aarch64" and TICI:
   arch = "larch64"
 
 USE_WEBCAM = os.getenv("USE_WEBCAM") is not None
+USE_FRAME_STREAM = os.getenv("USE_FRAME_STREAM") is not None
 
 lenv = {
   "PATH": os.environ['PATH'],
@@ -348,7 +349,7 @@ if GetOption("clazy"):
   qt_env['ENV']['CLAZY_IGNORE_DIRS'] = qt_dirs[0]
   qt_env['ENV']['CLAZY_CHECKS'] = ','.join(checks)
 
-Export('env', 'qt_env', 'arch', 'real_arch', 'SHARED', 'USE_WEBCAM')
+Export('env', 'qt_env', 'arch', 'real_arch', 'SHARED', 'USE_WEBCAM', 'USE_FRAME_STREAM')
 
 SConscript(['selfdrive/common/SConscript'])
 Import('_common', '_gpucommon', '_gpu_libs')

release/files_common

@@ -350,6 +350,9 @@ selfdrive/ui/qt/widgets/*.h
 selfdrive/ui/qt/spinner_aarch64
 selfdrive/ui/qt/text_aarch64
 
+selfdrive/ui/replay/framereader.cc
+selfdrive/ui/replay/framereader.h
+
 selfdrive/camerad/SConscript
 selfdrive/camerad/main.cc
 
@@ -361,6 +364,8 @@ selfdrive/camerad/cameras/camera_frame_stream.cc
 selfdrive/camerad/cameras/camera_frame_stream.h
 selfdrive/camerad/cameras/camera_qcom.cc
 selfdrive/camerad/cameras/camera_qcom.h
+selfdrive/camerad/cameras/camera_replay.cc
+selfdrive/camerad/cameras/camera_replay.h
 selfdrive/camerad/cameras/debayer.cl
 selfdrive/camerad/cameras/sensor_i2c.h
 selfdrive/camerad/cameras/sensor2_i2c.h

selfdrive/camerad/SConscript

@@ -1,4 +1,4 @@
-Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'USE_WEBCAM')
+Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'USE_WEBCAM', 'USE_FRAME_STREAM')
 
 libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon]
 
@@ -9,6 +9,7 @@ elif arch == "larch64":
   libs += ['atomic']
   cameras = ['cameras/camera_qcom2.cc']
 else:
+  env['CXXFLAGS'] += ["-Wno-deprecated-declarations"]
   if USE_WEBCAM:
     libs += ['opencv_core', 'opencv_highgui', 'opencv_imgproc', 'opencv_videoio']
     cameras = ['cameras/camera_webcam.cc']
@@ -17,7 +18,11 @@ else:
     env.Append(CFLAGS = '-DWEBCAM')
     env.Append(CPPPATH = '/usr/local/include/opencv4')
   else:
-    cameras = ['cameras/camera_frame_stream.cc']
+    if USE_FRAME_STREAM:
+      cameras = ['cameras/camera_frame_stream.cc']
+    else:
+      libs += ['avutil', 'avcodec', 'avformat', 'swscale']
+      cameras = ['cameras/camera_replay.cc', env.Object('camera-framereader', '#/selfdrive/ui/replay/framereader.cc')]
 
   if arch == "Darwin":
     del libs[libs.index('OpenCL')]

selfdrive/camerad/cameras/camera_common.cc

@@ -25,7 +25,7 @@
 #elif WEBCAM
 #include "selfdrive/camerad/cameras/camera_webcam.h"
 #else
-#include "selfdrive/camerad/cameras/camera_frame_stream.h"
+#include "selfdrive/camerad/cameras/camera_replay.h"
 #endif
 
 const int YUV_COUNT = 100;

selfdrive/camerad/cameras/camera_replay.cc

@@ -0,0 +1,125 @@
+#include "selfdrive/camerad/cameras/camera_replay.h"
+
+#include <cassert>
+#include <thread>
+
+#include "selfdrive/common/clutil.h"
+#include "selfdrive/common/util.h"
+
+extern ExitHandler do_exit;
+
+void camera_autoexposure(CameraState *s, float grey_frac) {}
+
+namespace {
+
+const char *BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/";
+
+const std::string road_camera_route = "0c94aa1e1296d7c6|2021-05-05--19-48-37";
+// const std::string driver_camera_route = "534ccd8a0950a00c|2021-06-08--12-15-37";
+
+std::string get_url(std::string route_name, const std::string &camera, int segment_num) {
+  std::replace(route_name.begin(), route_name.end(), '|', '/');
+  return util::string_format("%s%s/%d/%s.hevc", BASE_URL, route_name.c_str(), segment_num, camera.c_str());
+}
+
+void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type, const std::string &url) {
+  // TODO: cache url file
+  s->frame = new FrameReader();
+  if (!s->frame->load(url)) {
+    printf("failed to load stream from %s", url.c_str());
+    assert(0);
+  }
+
+  CameraInfo ci = {
+      .frame_width = s->frame->width,
+      .frame_height = s->frame->height,
+      .frame_stride = s->frame->width * 3,
+  };
+  s->ci = ci;
+  s->camera_num = camera_id;
+  s->fps = fps;
+  s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type);
+}
+
+void camera_close(CameraState *s) {
+  delete s->frame;
+}
+
+void run_camera(CameraState *s) {
+  uint32_t stream_frame_id = 0, frame_id = 0;
+  size_t buf_idx = 0;
+  while (!do_exit) {
+    if (stream_frame_id == s->frame->getFrameCount()) {
+      // loop stream
+      stream_frame_id = 0;
+    }
+    uint8_t *dat = s->frame->get(stream_frame_id++);
+    if (dat) {
+      s->buf.camera_bufs_metadata[buf_idx] = {.frame_id = frame_id};
+      auto &buf = s->buf.camera_bufs[buf_idx];
+      CL_CHECK(clEnqueueWriteBuffer(buf.copy_q, buf.buf_cl, CL_TRUE, 0, s->frame->getRGBSize(), dat, 0, NULL, NULL));
+      s->buf.queue(buf_idx);
+      ++frame_id;
+      buf_idx = (buf_idx + 1) % FRAME_BUF_COUNT;
+    }
+    util::sleep_for(1000 / s->fps);
+  }
+}
+
+void road_camera_thread(CameraState *s) {
+  set_thread_name("replay_road_camera_thread");
+  run_camera(s);
+}
+
+// void driver_camera_thread(CameraState *s) {
+//   set_thread_name("replay_driver_camera_thread");
+//   run_camera(s);
+// }
+
+void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
+  const CameraBuf *b = &c->buf;
+  MessageBuilder msg;
+  auto framed = msg.initEvent().initRoadCameraState();
+  fill_frame_data(framed, b->cur_frame_data);
+  framed.setImage(kj::arrayPtr((const uint8_t *)b->cur_yuv_buf->addr, b->cur_yuv_buf->len));
+  framed.setTransform(b->yuv_transform.v);
+  s->pm->send("roadCameraState", msg);
+}
+
+// void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) {
+//   MessageBuilder msg;
+//   auto framed = msg.initEvent().initDriverCameraState();
+//   framed.setFrameType(cereal::FrameData::FrameType::FRONT);
+//   fill_frame_data(framed, c->buf.cur_frame_data);
+//   s->pm->send("driverCameraState", msg);
+// }
+
+}  // namespace
+
+void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
+  camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx,
+              VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK, get_url(road_camera_route, "fcamera", 0));
+  // camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx,
+  //             VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT, get_url(driver_camera_route, "dcamera", 0));
+  s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
+}
+
+void cameras_open(MultiCameraState *s) {}
+
+void cameras_close(MultiCameraState *s) {
+  camera_close(&s->road_cam);
+  camera_close(&s->driver_cam);
+  delete s->pm;
+}
+
+void cameras_run(MultiCameraState *s) {
+  std::vector<std::thread> threads;
+  threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera));
+  // threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera));
+  // threads.push_back(std::thread(driver_camera_thread, &s->driver_cam));
+  road_camera_thread(&s->road_cam);
+
+  for (auto &t : threads) t.join();
+
+  cameras_close(s);
+}

selfdrive/camerad/cameras/camera_replay.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include "selfdrive/camerad/cameras/camera_common.h"
+#include "selfdrive/ui/replay/framereader.h"
+
+#define FRAME_BUF_COUNT 16
+
+typedef struct CameraState {
+  int camera_num;
+  CameraInfo ci;
+
+  int fps;
+  float digital_gain = 0;
+
+  CameraBuf buf;
+  FrameReader *frame = nullptr;
+} CameraState;
+
+typedef struct MultiCameraState {
+  CameraState road_cam;
+  CameraState driver_cam;
+
+  SubMaster *sm = nullptr;
+  PubMaster *pm = nullptr;
+} MultiCameraState;

selfdrive/camerad/main.cc

@@ -22,7 +22,7 @@
 #elif WEBCAM
 #include "selfdrive/camerad/cameras/camera_webcam.h"
 #else
-#include "selfdrive/camerad/cameras/camera_frame_stream.h"
+#include "selfdrive/camerad/cameras/camera_replay.h"
 #endif
 
 ExitHandler do_exit;