From b7f3d92c9b88046ca0b9a1e360c2b46afa009db5 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Mon, 20 Sep 2021 05:43:54 +0800 Subject: [PATCH] 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 old-commit-hash: 9b302488f98f7fcaf52a943e98de54196375b5b8 --- SConstruct | 3 +- release/files_common | 5 + selfdrive/camerad/SConscript | 9 +- selfdrive/camerad/cameras/camera_common.cc | 2 +- selfdrive/camerad/cameras/camera_replay.cc | 125 +++++++++++++++++++++ selfdrive/camerad/cameras/camera_replay.h | 25 +++++ selfdrive/camerad/main.cc | 2 +- 7 files changed, 166 insertions(+), 5 deletions(-) create mode 100644 selfdrive/camerad/cameras/camera_replay.cc create mode 100644 selfdrive/camerad/cameras/camera_replay.h diff --git a/SConstruct b/SConstruct index 263f213f6e..5e457bb2df 100644 --- a/SConstruct +++ b/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') diff --git a/release/files_common b/release/files_common index 47c1e255b0..94f2bb4ba7 100644 --- a/release/files_common +++ b/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 diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript index 4e20c16dc4..37ccba8f2f 100644 --- a/selfdrive/camerad/SConscript +++ b/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')] diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index 1cd608addd..9c895c0a23 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/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; diff --git a/selfdrive/camerad/cameras/camera_replay.cc b/selfdrive/camerad/cameras/camera_replay.cc new file mode 100644 index 0000000000..ff632b4b67 --- /dev/null +++ b/selfdrive/camerad/cameras/camera_replay.cc @@ -0,0 +1,125 @@ +#include "selfdrive/camerad/cameras/camera_replay.h" + +#include +#include + +#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 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); +} diff --git a/selfdrive/camerad/cameras/camera_replay.h b/selfdrive/camerad/cameras/camera_replay.h new file mode 100644 index 0000000000..7c41af0ab4 --- /dev/null +++ b/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; diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index b4cf162d2c..b4f5ff796d 100644 --- a/selfdrive/camerad/main.cc +++ b/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;