Micro bounty: visiond PC version

selfdrive/visiond/build_from_src.mk

@@ -38,34 +38,35 @@ ifeq ($(UNAME_S),Darwin)
              $(PHONELIBS)/zmq/mac/lib/libzmq.a
 
   OPENCL_LIBS = -framework OpenCL
+
+  PLATFORM_OBJS = camera_fake.o \
+                  ../common/visionbuf_cl.o
 else
-  LIBYUV_FLAGS = -I$(PHONELIBS)/libyuv/x64/include
-  LIBYUV_LIBS = $(PHONELIBS)/libyuv/x64/lib/libyuv.a
+  # assuem x86_64 linux
 
-  ZMQ_FLAGS = -I$(EXTERNAL)/zmq/include
-  ZMQ_LIBS = -L$(EXTERNAL)/zmq/lib \
-             -l:libczmq.a -l:libzmq.a
+  ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
+  ZMQ_LIBS = -l:libczmq.a -l:libzmq.a -luuid
 
   OPENCL_LIBS = -lOpenCL
-endif
 
-  CURL_FLAGS = -I/usr/include/curl
-  CURL_LIBS = -lcurl -lz
+  SNPE_FLAGS = -I$(SNPE_ROOT)/include/zdl
+  SNPE_LIBS = -L$(SNPE_ROOT)/lib/x86_64-linux-clang/ \
+              -lSNPE -lsymphony-cpu
+
+  CXXFLAGS += -I../common
+
+  PLATFORM_OBJS = camera_eon_stream.o
+endif
 
   SSL_FLAGS = -I/usr/include/openssl/
   SSL_LIBS = -lssl -lcrypto
 
-  OPENCV_FLAGS =
-  OPENCV_LIBS = -lopencv_video \
-                -lopencv_imgproc \
-                -lopencv_core
-  OTHER_LIBS = -lz -lm -lpthread
+  OTHER_LIBS = -lz -lm -lpthread -lavcodec -lavutil
 
-  PLATFORM_OBJS = camera_fake.o \
-                  ../common/visionbuf_cl.o
 
   CFLAGS += -D_GNU_SOURCE \
             -DCLU_NO_CACHE
+  OBJS = visiond_pc.o
 else
 	# assume phone
 
@@ -102,13 +103,15 @@ else
   OTHER_LIBS = -lz -lcutils -lm -llog -lui -ladreno_utils
 
   PLATFORM_OBJS = camera_qcom.o \
-                  ../common/visionbuf_ion.o
+                  ../common/visionbuf_ion.o \
+                  ../common/visionimg.o
+
 
   CFLAGS += -DQCOM
   CXXFLAGS += -DQCOM
+  OBJS = visiond.o
 endif
 
-OBJS = visiond.o
 OUTPUT = visiond
 
 .PHONY: all
@@ -120,7 +123,6 @@ OBJS += $(PLATFORM_OBJS) \
         ../common/swaglog.o \
         ../common/ipc.o \
         ../common/visionipc.o \
-        ../common/visionimg.o \
         ../common/util.o \
         ../common/params.o \
         ../common/efd.o \

selfdrive/visiond/camera_eon_stream.cc

@@ -0,0 +1,228 @@
+#include "camera_eon_stream.h"
+
+#include <string>
+#include <unistd.h>
+#include <vector>
+#include <string.h>
+
+#include <czmq.h>
+#include <libyuv.h>
+#include <capnp/serialize.h>
+#include "cereal/gen/cpp/log.capnp.h"
+
+#include "common/util.h"
+#include "common/timing.h"
+#include "common/swaglog.h"
+#include "buffering.h"
+
+extern "C" {
+#include <libavcodec/avcodec.h>
+}
+
+extern volatile int do_exit;
+
+#define FRAME_WIDTH 1164
+#define FRAME_HEIGHT 874
+
+namespace {
+void camera_open(CameraState *s, cl_mem *yuv_cls, bool rear, cl_device_id device_id, cl_context context, cl_command_queue q) {
+  assert(yuv_cls);
+  s->yuv_cls = yuv_cls;
+  s->device_id = device_id;
+  s->context = context;
+  s->q = q;
+}
+
+void camera_close(CameraState *s) {
+  tbuffer_stop(&s->camera_tb);
+}
+
+void camera_release_buffer(void *cookie, int buf_idx) {
+  CameraState *s = static_cast<CameraState *>(cookie);
+}
+
+void camera_init(CameraState *s, int camera_id, unsigned int fps) {
+  assert(camera_id < ARRAYSIZE(cameras_supported));
+  s->ci = cameras_supported[camera_id];
+  assert(s->ci.frame_width != 0);
+
+  s->frame_size = s->ci.frame_height * s->ci.frame_stride;
+  s->fps = fps;
+
+  tbuffer_init2(&s->camera_tb, FRAME_BUF_COUNT, "frame", camera_release_buffer,
+                s);
+}
+
+void run_eon_stream(DualCameraState *s) {
+  int err;
+  uint8_t stream_start[74] =  
+{0x00,0x00,0x00,0x01,0x40,0x01,0x0c,0x01,0xff,0xff,0x01,0x60,0x00,0x00,0x03,0x00,0xb0,0x00,0x00,0x03,0x00,0x00,0x03,0x00,0x5d,0xac,0x59,0x00,0x00,0x00,0x01,0x42,0x01,0x01,0x01,0x60,0x00,0x00,0x03,0x00,0xb0,0x00,0x00,0x03,0x00,0x00,0x03,0x00,0x5d,0xa0,0x02,0x50,0x80,0x38,0x1c,0x5c,0x66,0x5a,0xee,0x4c,0x92,0xec,0x80,0x00,0x00,0x00,0x01,0x44,0x01,0xc0,0xf1,0x80,0x04,0x20};
+
+  avcodec_register_all();
+  const AVCodec *codec = avcodec_find_decoder_by_name("hevc");
+  if(!codec) {
+    LOG("hevc decoder not found\n");
+    return;
+  }
+  AVCodecParserContext *parser = av_parser_init(codec->id);
+  if (!parser) {
+    LOG("parser not found\n");
+    return;
+  }
+  AVCodecContext *dec_ctx = avcodec_alloc_context3(codec);
+  assert(dec_ctx);
+  err = avcodec_open2(dec_ctx, codec, NULL);
+  assert(err >= 0);
+  AVFrame *frame = av_frame_alloc();
+  assert(frame);
+
+  std::string zmq_uri(">tcp://");
+  const char *eon_ip = getenv("EON_IP");
+  if(eon_ip)
+    zmq_uri += eon_ip;
+  else
+    zmq_uri += "192.168.1.105";
+  zmq_uri += ":9002";
+  LOG("Connecting to Eon stream: %s", zmq_uri.c_str());
+  zsock_t *frame_sock = zsock_new_sub(zmq_uri.c_str(), "");
+  assert(frame_sock);
+  void *frame_sock_raw = zsock_resolve(frame_sock);
+
+  CameraState *const rear_camera = &s->rear;
+
+  auto *tb = &rear_camera->camera_tb;
+  AVPacket avpkt;
+  av_init_packet(&avpkt);
+  // send stream start bytes
+  avpkt.size = sizeof(stream_start);
+  avpkt.data = &stream_start[0];
+  while (avpkt.size > 0) {
+    int got_frame = 0;
+    int len = avcodec_decode_video2(dec_ctx, frame, &got_frame, &avpkt);
+    if (len < 0) {
+      LOGD("Error while decoding frame\n");
+      return;
+    }
+    avpkt.size -= len;
+    avpkt.data += len;
+  }
+  while (!do_exit) {
+    zmq_msg_t t_msg;
+    err = zmq_msg_init(&t_msg);
+    assert(err == 0);
+
+    zmq_msg_t frame_msg;
+    err = zmq_msg_init(&frame_msg);
+    assert(err == 0);
+
+    err = zmq_msg_recv(&t_msg, frame_sock_raw, 0);
+    if(err == -1)
+	    break;
+    err = zmq_msg_recv(&frame_msg, frame_sock_raw, 0);
+    if(err == -1)
+	    break;
+
+    av_init_packet(&avpkt);
+    avpkt.size = zmq_msg_size(&frame_msg);
+    if (avpkt.size == 0) {
+      LOGD("Empty frame msg recved.\n");
+      continue;
+    }
+    avpkt.data = (uint8_t *)zmq_msg_data(&frame_msg);;
+    while (avpkt.size > 0) {
+      int got_frame = 0;
+      int len = avcodec_decode_video2(dec_ctx, frame, &got_frame, &avpkt);
+      assert(len >= 0);
+      if (got_frame) {
+        assert(frame->width == FRAME_WIDTH);
+        assert(frame->height == FRAME_HEIGHT);
+        const int buf_idx = tbuffer_select(tb);
+        rear_camera->camera_bufs_metadata[buf_idx] = {
+          .frame_id = (uint32_t)dec_ctx->frame_number,
+          .timestamp_eof = nanos_since_boot(),
+          .frame_length = 0,
+          .integ_lines = 0,
+          .global_gain = 0,
+        };
+        cl_mem yuv_cl = rear_camera->yuv_cls[buf_idx];
+        cl_event map_event;
+        void *yuv_buf = (void *)clEnqueueMapBuffer(rear_camera->q, yuv_cl, CL_TRUE,
+                                                    CL_MAP_WRITE, 0, frame->width * frame->height * 3 / 2,
+                                                    0, NULL, &map_event, &err);
+        assert(err == 0);
+        clWaitForEvents(1, &map_event);
+        clReleaseEvent(map_event);
+        uint8_t *write_ptr = (uint8_t *)yuv_buf;
+        for(int line_idx = 0; line_idx < frame->height; line_idx++) {
+          memcpy(write_ptr, frame->data[0] + line_idx * frame->linesize[0], frame->width);
+          write_ptr += frame->width;
+        }
+        for(int line_idx = 0; line_idx < frame->height / 2; line_idx++) {
+          memcpy(write_ptr, frame->data[1] + line_idx * frame->linesize[1], frame->width / 2);
+          write_ptr += frame->width / 2;
+        }
+        for(int line_idx = 0; line_idx < frame->height / 2;line_idx++) {
+          memcpy(write_ptr, frame->data[2] + line_idx * frame->linesize[2], frame->width / 2);
+          write_ptr += frame->width / 2;
+        }
+        clEnqueueUnmapMemObject(rear_camera->q, yuv_cl, yuv_buf, 0, NULL, &map_event);
+        clWaitForEvents(1, &map_event);
+        clReleaseEvent(map_event);
+        tbuffer_dispatch(tb, buf_idx);
+
+      }
+      avpkt.size -= len;
+      avpkt.data += len;
+    }
+    err = zmq_msg_close(&frame_msg);
+    assert(err == 0);
+    err = zmq_msg_close(&t_msg);
+    assert(err == 0);
+  }
+  zsock_destroy(&frame_sock);
+  av_parser_close(parser);
+  avcodec_free_context(&dec_ctx);
+  av_frame_free(&frame);
+}
+
+}  // namespace
+
+CameraInfo cameras_supported[CAMERA_ID_MAX] = {
+  [CAMERA_ID_IMX298] = {
+      .frame_width = FRAME_WIDTH,
+      .frame_height = FRAME_HEIGHT,
+      .frame_stride = FRAME_WIDTH*3,
+      .bayer = false,
+      .bayer_flip = false,
+  },
+};
+
+void cameras_init(DualCameraState *s) {
+  memset(s, 0, sizeof(*s));
+
+  camera_init(&s->rear, CAMERA_ID_IMX298, 20);
+  s->rear.transform = (mat3){{
+    1.0,  0.0, 0.0,
+    0.0, 1.0, 0.0,
+    0.0,  0.0, 1.0,
+  }};
+}
+
+void camera_autoexposure(CameraState *s, float grey_frac) {}
+
+void cameras_open(DualCameraState *s, cl_mem *yuv_cls_rear, cl_device_id device_id, cl_context context, cl_command_queue q) {
+  assert(yuv_cls_rear);
+  int err;
+
+  camera_open(&s->rear, yuv_cls_rear, true, device_id, context, q);
+}
+
+void cameras_close(DualCameraState *s) {
+  camera_close(&s->rear);
+}
+
+void cameras_run(DualCameraState *s) {
+  set_thread_name("Eon streaming");
+  run_eon_stream(s);
+  cameras_close(s);
+}

selfdrive/visiond/camera_eon_stream.h

@@ -0,0 +1,60 @@
+#ifndef FAKE_CAMERA_H
+#define FAKE_CAMERA_H
+
+#include <stdbool.h>
+
+#ifdef __APPLE__
+#include <OpenCL/cl.h>
+#else
+#include <CL/cl.h>
+#endif
+
+#include "common/mat.h"
+
+#include "buffering.h"
+#include "common/visionbuf.h"
+#include "camera_common.h"
+
+#define FRAME_BUF_COUNT 16
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct CameraState {
+  int camera_id;
+  CameraInfo ci;
+  int frame_size;
+
+  cl_mem *yuv_cls;
+  cl_device_id device_id;
+  cl_context context;
+  cl_command_queue q;
+
+  FrameMetadata camera_bufs_metadata[FRAME_BUF_COUNT];
+  TBuffer camera_tb;
+
+  int fps;
+  float digital_gain;
+
+  mat3 transform;
+} CameraState;
+
+
+typedef struct DualCameraState {
+  int ispif_fd;
+
+  CameraState rear;
+  CameraState front;
+} DualCameraState;
+
+void cameras_init(DualCameraState *s);
+void cameras_open(DualCameraState *s, cl_mem *yuv_cls_rear, cl_device_id device_id, cl_context context, cl_command_queue q);
+void cameras_run(DualCameraState *s);
+void cameras_close(DualCameraState *s);
+void camera_autoexposure(CameraState *s, float grey_frac);
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+
+#endif

selfdrive/visiond/model.cc

@@ -1,3 +1,7 @@
+#include <string.h>
+#include <assert.h>
+#include <fcntl.h>
+#include <unistd.h>
 #include "common/timing.h"
 #include "model.h"
 
@@ -92,5 +96,6 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
 void model_free(ModelState* s) {
   model_input_free(&s->in);
   delete s->m;
+  free(s->output);
 }
 

selfdrive/visiond/monitoring.cc

@@ -1,3 +1,4 @@
+#include <string.h>
 #include "monitoring.h"
 #include "common/mat.h"