diff --git a/SConstruct b/SConstruct index 50f8f1c040..bec87eda54 100644 --- a/SConstruct +++ b/SConstruct @@ -15,6 +15,8 @@ arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() if platform.system() == "Darwin": arch = "Darwin" +webcam = bool(ARGUMENTS.get("use_webcam", 0)) + if arch == "aarch64": lenv = { "LD_LIBRARY_PATH": '/data/data/com.termux/files/usr/lib', @@ -117,7 +119,7 @@ env = Environment( "#phonelibs/json11", "#phonelibs/eigen", "#phonelibs/curl/include", - "#phonelibs/opencv/include", + #"#phonelibs/opencv/include", # use opencv4 instead "#phonelibs/libgralloc/include", "#phonelibs/android_frameworks_native/include", "#phonelibs/android_hardware_libhardware/include", @@ -178,7 +180,7 @@ def abspath(x): #zmq = 'zmq' # still needed for apks zmq = FindFile("libzmq.a", libpath) -Export('env', 'arch', 'zmq', 'SHARED') +Export('env', 'arch', 'zmq', 'SHARED', 'webcam') # cereal and messaging are shared with the system SConscript(['cereal/SConscript']) diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript index 12e1afe5ac..336fc784db 100644 --- a/selfdrive/camerad/SConscript +++ b/selfdrive/camerad/SConscript @@ -1,4 +1,4 @@ -Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc', 'cereal') +Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc', 'cereal', 'webcam') libs = ['m', 'pthread', common, 'jpeg', 'json', cereal, 'OpenCL', messaging, 'czmq', 'zmq', 'capnp', 'kj', 'capnp_c', visionipc, gpucommon] @@ -6,8 +6,16 @@ if arch == "aarch64": libs += ['gsl', 'CB', 'adreno_utils', 'EGL', 'GLESv3', 'cutils', 'ui'] cameras = ['cameras/camera_qcom.c'] else: - libs += [] - cameras = ['cameras/camera_frame_stream.cc'] + if webcam: + libs += ['opencv_core', 'opencv_highgui', 'opencv_imgproc', 'opencv_videoio'] + cameras = ['cameras/camera_webcam.cc'] + env = env.Clone() + env.Append(CXXFLAGS = '-DWEBCAM') + env.Append(CFLAGS = '-DWEBCAM') + env.Append(CPPPATH = '/usr/local/include/opencv4') + else: + libs += [] + cameras = ['cameras/camera_frame_stream.cc'] env.SharedLibrary('snapshot/visionipc', ["#selfdrive/common/visionipc.c", "#selfdrive/common/ipc.c"]) diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 187952e22d..6d1017fe2b 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -10,7 +10,9 @@ #define CAMERA_ID_OV8865 3 #define CAMERA_ID_IMX298_FLIPPED 4 #define CAMERA_ID_OV10640 5 -#define CAMERA_ID_MAX 6 +#define CAMERA_ID_LGC920 6 +#define CAMERA_ID_LGC615 7 +#define CAMERA_ID_MAX 8 #ifdef __cplusplus extern "C" { diff --git a/selfdrive/camerad/cameras/camera_frame_stream.cc b/selfdrive/camerad/cameras/camera_frame_stream.cc index 1a9f8a931f..0061fe6185 100644 --- a/selfdrive/camerad/cameras/camera_frame_stream.cc +++ b/selfdrive/camerad/cameras/camera_frame_stream.cc @@ -115,7 +115,7 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = { .frame_width = 1632, .frame_height = 1224, .frame_stride = 2040, // seems right - .bayer = false, + .bayer = true, .bayer_flip = 3, .hdr = false }, diff --git a/selfdrive/camerad/cameras/camera_webcam.cc b/selfdrive/camerad/cameras/camera_webcam.cc new file mode 100644 index 0000000000..9378ac5999 --- /dev/null +++ b/selfdrive/camerad/cameras/camera_webcam.cc @@ -0,0 +1,269 @@ +#include "camera_webcam.h" + +#include +#include +#include +#include + +#include "common/util.h" +#include "common/timing.h" +#include "common/swaglog.h" +#include "buffering.h" + +#include +#include +#include +#include + +extern volatile sig_atomic_t do_exit; + +#define FRAME_WIDTH 1164 +#define FRAME_HEIGHT 874 +#define FRAME_WIDTH_FRONT 1152 +#define FRAME_HEIGHT_FRONT 864 + +namespace { +void camera_open(CameraState *s, VisionBuf *camera_bufs, bool rear) { + assert(camera_bufs); + s->camera_bufs = camera_bufs; +} + +void camera_close(CameraState *s) { + tbuffer_stop(&s->camera_tb); +} + +void camera_release_buffer(void *cookie, int buf_idx) { + CameraState *s = static_cast(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); +} + +static void* rear_thread(void *arg) { + int err; + + set_thread_name("webcam_rear_thread"); + CameraState* s = (CameraState*)arg; + + cv::VideoCapture cap_rear(1); // road + cap_rear.set(cv::CAP_PROP_FRAME_WIDTH, 1280); + cap_rear.set(cv::CAP_PROP_FRAME_HEIGHT, 720); + cap_rear.set(cv::CAP_PROP_FPS, s->fps); + cap_rear.set(cv::CAP_PROP_AUTOFOCUS, 0); // off + cap_rear.set(cv::CAP_PROP_FOCUS, 0); // 0 - 255? + // cv::Rect roi_rear(160, 0, 960, 720); + + cv::Size size; + size.height = s->ci.frame_height; + size.width = s->ci.frame_width; + + // transforms calculation see tools/webcam/warp_vis.py + float ts[9] = {1.00220264, 0.0, -59.40969163, + 0.0, 1.00220264, 76.20704846, + 0.0, 0.0, 1.0}; + const cv::Mat transform = cv::Mat(3, 3, CV_32F, ts); + + if (!cap_rear.isOpened()) { + err = 1; + } + + uint32_t frame_id = 0; + TBuffer* tb = &s->camera_tb; + + while (!do_exit) { + cv::Mat frame_mat; + cv::Mat transformed_mat; + + cap_rear >> frame_mat; + + // int rows = frame_mat.rows; + // int cols = frame_mat.cols; + // printf("Raw Rear, R=%d, C=%d\n", rows, cols); + + cv::warpPerspective(frame_mat, transformed_mat, transform, size, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0); + + int transformed_size = transformed_mat.total() * transformed_mat.elemSize(); + + const int buf_idx = tbuffer_select(tb); + s->camera_bufs_metadata[buf_idx] = { + .frame_id = frame_id, + }; + + cl_command_queue q = s->camera_bufs[buf_idx].copy_q; + cl_mem yuv_cl = s->camera_bufs[buf_idx].buf_cl; + cl_event map_event; + void *yuv_buf = (void *)clEnqueueMapBuffer(q, yuv_cl, CL_TRUE, + CL_MAP_WRITE, 0, transformed_size, + 0, NULL, &map_event, &err); + assert(err == 0); + clWaitForEvents(1, &map_event); + clReleaseEvent(map_event); + memcpy(yuv_buf, transformed_mat.data, transformed_size); + + clEnqueueUnmapMemObject(q, yuv_cl, yuv_buf, 0, NULL, &map_event); + clWaitForEvents(1, &map_event); + clReleaseEvent(map_event); + tbuffer_dispatch(tb, buf_idx); + + frame_id += 1; + frame_mat.release(); + transformed_mat.release(); + } + + cap_rear.release(); + return NULL; +} + +void front_thread(CameraState *s) { + int err; + + cv::VideoCapture cap_front(2); // driver + cap_front.set(cv::CAP_PROP_FRAME_WIDTH, 1280); + cap_front.set(cv::CAP_PROP_FRAME_HEIGHT, 720); + cap_front.set(cv::CAP_PROP_FPS, s->fps); + // cv::Rect roi_front(320, 0, 960, 720); + + cv::Size size; + size.height = s->ci.frame_height; + size.width = s->ci.frame_width; + + // transforms calculation see tools/webcam/warp_vis.py + float ts[9] = {0.94713656, 0.0, -30.16740088, + 0.0, 0.94713656, 91.030837, + 0.0, 0.0, 1.0}; + const cv::Mat transform = cv::Mat(3, 3, CV_32F, ts); + + if (!cap_front.isOpened()) { + err = 1; + } + + uint32_t frame_id = 0; + TBuffer* tb = &s->camera_tb; + + while (!do_exit) { + cv::Mat frame_mat; + cv::Mat transformed_mat; + + cap_front >> frame_mat; + + // int rows = frame_mat.rows; + // int cols = frame_mat.cols; + // printf("Raw Front, R=%d, C=%d\n", rows, cols); + + cv::warpPerspective(frame_mat, transformed_mat, transform, size, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0); + + int transformed_size = transformed_mat.total() * transformed_mat.elemSize(); + + const int buf_idx = tbuffer_select(tb); + s->camera_bufs_metadata[buf_idx] = { + .frame_id = frame_id, + }; + + cl_command_queue q = s->camera_bufs[buf_idx].copy_q; + cl_mem yuv_cl = s->camera_bufs[buf_idx].buf_cl; + cl_event map_event; + void *yuv_buf = (void *)clEnqueueMapBuffer(q, yuv_cl, CL_TRUE, + CL_MAP_WRITE, 0, transformed_size, + 0, NULL, &map_event, &err); + assert(err == 0); + clWaitForEvents(1, &map_event); + clReleaseEvent(map_event); + memcpy(yuv_buf, transformed_mat.data, transformed_size); + + clEnqueueUnmapMemObject(q, yuv_cl, yuv_buf, 0, NULL, &map_event); + clWaitForEvents(1, &map_event); + clReleaseEvent(map_event); + tbuffer_dispatch(tb, buf_idx); + + frame_id += 1; + frame_mat.release(); + transformed_mat.release(); + } + + cap_front.release(); + return; +} + +} // namespace + +CameraInfo cameras_supported[CAMERA_ID_MAX] = { + // road facing + [CAMERA_ID_LGC920] = { + .frame_width = FRAME_WIDTH, + .frame_height = FRAME_HEIGHT, + .frame_stride = FRAME_WIDTH*3, + .bayer = false, + .bayer_flip = false, + }, + // driver facing + [CAMERA_ID_LGC615] = { + .frame_width = FRAME_WIDTH_FRONT, + .frame_height = FRAME_HEIGHT_FRONT, + .frame_stride = FRAME_WIDTH_FRONT*3, + .bayer = false, + .bayer_flip = false, + }, +}; + +void cameras_init(DualCameraState *s) { + memset(s, 0, sizeof(*s)); + + camera_init(&s->rear, CAMERA_ID_LGC920, 20); + s->rear.transform = (mat3){{ + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + }}; + + camera_init(&s->front, CAMERA_ID_LGC615, 10); + s->front.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, VisionBuf *camera_bufs_rear, + VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, + VisionBuf *camera_bufs_front) { + assert(camera_bufs_rear); + assert(camera_bufs_front); + int err; + + // LOG("*** open front ***"); + camera_open(&s->front, camera_bufs_front, false); + + // LOG("*** open rear ***"); + camera_open(&s->rear, camera_bufs_rear, true); +} + +void cameras_close(DualCameraState *s) { + camera_close(&s->rear); + camera_close(&s->front); +} + +void cameras_run(DualCameraState *s) { + set_thread_name("webcam_thread"); + + int err; + pthread_t rear_thread_handle; + err = pthread_create(&rear_thread_handle, NULL, + rear_thread, &s->rear); + assert(err == 0); + + front_thread(&s->front); + + err = pthread_join(rear_thread_handle, NULL); + assert(err == 0); + cameras_close(s); +} \ No newline at end of file diff --git a/selfdrive/camerad/cameras/camera_webcam.h b/selfdrive/camerad/cameras/camera_webcam.h new file mode 100644 index 0000000000..28c3e65209 --- /dev/null +++ b/selfdrive/camerad/cameras/camera_webcam.h @@ -0,0 +1,58 @@ +#ifndef CAMERA_WEBCAM +#define CAMERA_WEBCAM + +#include + +#ifdef __APPLE__ +#include +#else +#include +#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; + + VisionBuf *camera_bufs; + FrameMetadata camera_bufs_metadata[FRAME_BUF_COUNT]; + TBuffer camera_tb; + + int fps; + float digital_gain; + + float cur_gain_frac; + + mat3 transform; +} CameraState; + + +typedef struct DualCameraState { + int ispif_fd; + + CameraState rear; + CameraState front; +} DualCameraState; + +void cameras_init(DualCameraState *s); +void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front); +void cameras_run(DualCameraState *s); +void cameras_close(DualCameraState *s); +void camera_autoexposure(CameraState *s, float grey_frac); +#ifdef __cplusplus +} // extern "C" +#endif + +#endif diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index 1561168170..bdce18de24 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -4,6 +4,8 @@ #ifdef QCOM #include "cameras/camera_qcom.h" +#elif WEBCAM +#include "cameras/camera_webcam.h" #else #include "cameras/camera_frame_stream.h" #endif @@ -160,31 +162,37 @@ void* frontview_thread(void *arg) { } int ui_idx = tbuffer_select(&s->ui_front_tb); + int rgb_idx = ui_idx; FrameMetadata frame_data = s->cameras.front.camera_bufs_metadata[buf_idx]; double t1 = millis_since_boot(); - err = clSetKernelArg(s->krnl_debayer_front, 0, sizeof(cl_mem), &s->front_camera_bufs_cl[buf_idx]); - assert(err == 0); - err = clSetKernelArg(s->krnl_debayer_front, 1, sizeof(cl_mem), &s->rgb_front_bufs_cl[ui_idx]); - assert(err == 0); - float digital_gain = 1.0; - err = clSetKernelArg(s->krnl_debayer_front, 2, sizeof(float), &digital_gain); - assert(err == 0); - cl_event debayer_event; - const size_t debayer_work_size = s->rgb_front_height; - const size_t debayer_local_work_size = 128; - err = clEnqueueNDRangeKernel(q, s->krnl_debayer_front, 1, NULL, - &debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event); - assert(err == 0); + if (s->cameras.front.ci.bayer) { + err = clSetKernelArg(s->krnl_debayer_front, 0, sizeof(cl_mem), &s->front_camera_bufs_cl[buf_idx]); + cl_check_error(err); + err = clSetKernelArg(s->krnl_debayer_front, 1, sizeof(cl_mem), &s->rgb_front_bufs_cl[rgb_idx]); + cl_check_error(err); + float digital_gain = 1.0; + err = clSetKernelArg(s->krnl_debayer_front, 2, sizeof(float), &digital_gain); + assert(err == 0); + + const size_t debayer_work_size = s->rgb_front_height; // doesn't divide evenly, is this okay? + const size_t debayer_local_work_size = 128; + err = clEnqueueNDRangeKernel(q, s->krnl_debayer_front, 1, NULL, + &debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event); + assert(err == 0); + } else { + assert(s->rgb_front_buf_size >= s->cameras.front.frame_size); + assert(s->rgb_front_stride == s->cameras.front.ci.frame_stride); + err = clEnqueueCopyBuffer(q, s->front_camera_bufs_cl[buf_idx], s->rgb_front_bufs_cl[rgb_idx], + 0, 0, s->rgb_front_buf_size, 0, 0, &debayer_event); + assert(err == 0); + } clWaitForEvents(1, &debayer_event); clReleaseEvent(debayer_event); - tbuffer_release(&s->cameras.front.camera_tb, buf_idx); - visionbuf_sync(&s->rgb_front_bufs[ui_idx], VISIONBUF_SYNC_FROM_DEVICE); - // set front camera metering target Message *msg = monitoring_sock->receive(true); if (msg != NULL) { @@ -288,27 +296,29 @@ void* frontview_thread(void *arg) { // send frame event { - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - - auto framed = event.initFrontFrame(); - framed.setFrameId(frame_data.frame_id); - framed.setEncodeId(cnt); - framed.setTimestampEof(frame_data.timestamp_eof); - framed.setFrameLength(frame_data.frame_length); - framed.setIntegLines(frame_data.integ_lines); - framed.setGlobalGain(frame_data.global_gain); - framed.setLensPos(frame_data.lens_pos); - framed.setLensSag(frame_data.lens_sag); - framed.setLensErr(frame_data.lens_err); - framed.setLensTruePos(frame_data.lens_true_pos); - framed.setGainFrac(frame_data.gain_frac); - framed.setFrameType(cereal::FrameData::FrameType::FRONT); + if (s->front_frame_sock != NULL) { + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto framed = event.initFrontFrame(); + framed.setFrameId(frame_data.frame_id); + framed.setEncodeId(cnt); + framed.setTimestampEof(frame_data.timestamp_eof); + framed.setFrameLength(frame_data.frame_length); + framed.setIntegLines(frame_data.integ_lines); + framed.setGlobalGain(frame_data.global_gain); + framed.setLensPos(frame_data.lens_pos); + framed.setLensSag(frame_data.lens_sag); + framed.setLensErr(frame_data.lens_err); + framed.setLensTruePos(frame_data.lens_true_pos); + framed.setGainFrac(frame_data.gain_frac); + framed.setFrameType(cereal::FrameData::FrameType::FRONT); - auto words = capnp::messageToFlatArray(msg); - auto bytes = words.asBytes(); - s->front_frame_sock->send((char*)bytes.begin(), bytes.size()); + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + s->front_frame_sock->send((char*)bytes.begin(), bytes.size()); + } } /*FILE *f = fopen("/tmp/test2", "wb"); @@ -421,37 +431,38 @@ void* processing_thread(void *arg) { // send frame event { - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); + if (s->frame_sock != NULL) { + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto framed = event.initFrame(); + framed.setFrameId(frame_data.frame_id); + framed.setEncodeId(cnt); + framed.setTimestampEof(frame_data.timestamp_eof); + framed.setFrameLength(frame_data.frame_length); + framed.setIntegLines(frame_data.integ_lines); + framed.setGlobalGain(frame_data.global_gain); + framed.setLensPos(frame_data.lens_pos); + framed.setLensSag(frame_data.lens_sag); + framed.setLensErr(frame_data.lens_err); + framed.setLensTruePos(frame_data.lens_true_pos); + framed.setGainFrac(frame_data.gain_frac); - auto framed = event.initFrame(); - framed.setFrameId(frame_data.frame_id); - framed.setEncodeId(cnt); - framed.setTimestampEof(frame_data.timestamp_eof); - framed.setFrameLength(frame_data.frame_length); - framed.setIntegLines(frame_data.integ_lines); - framed.setGlobalGain(frame_data.global_gain); - framed.setLensPos(frame_data.lens_pos); - framed.setLensSag(frame_data.lens_sag); - framed.setLensErr(frame_data.lens_err); - framed.setLensTruePos(frame_data.lens_true_pos); - framed.setGainFrac(frame_data.gain_frac); #ifdef QCOM - kj::ArrayPtr focus_vals(&s->cameras.rear.focus[0], NUM_FOCUS); - kj::ArrayPtr focus_confs(&s->cameras.rear.confidence[0], NUM_FOCUS); - framed.setFocusVal(focus_vals); - framed.setFocusConf(focus_confs); + kj::ArrayPtr focus_vals(&s->cameras.rear.focus[0], NUM_FOCUS); + kj::ArrayPtr focus_confs(&s->cameras.rear.confidence[0], NUM_FOCUS); + framed.setFocusVal(focus_vals); + framed.setFocusConf(focus_confs); #endif #ifndef QCOM - framed.setImage(kj::arrayPtr((const uint8_t*)s->yuv_ion[yuv_idx].addr, s->yuv_buf_size)); + framed.setImage(kj::arrayPtr((const uint8_t*)s->yuv_ion[yuv_idx].addr, s->yuv_buf_size)); #endif - kj::ArrayPtr transform_vs(&s->yuv_transform.v[0], 9); - framed.setTransform(transform_vs); + kj::ArrayPtr transform_vs(&s->yuv_transform.v[0], 9); + framed.setTransform(transform_vs); - if (s->frame_sock != NULL) { auto words = capnp::messageToFlatArray(msg); auto bytes = words.asBytes(); s->frame_sock->send((char*)bytes.begin(), bytes.size()); @@ -939,8 +950,14 @@ void init_buffers(VisionState *s) { tbuffer_init(&s->ui_tb, UI_BUF_COUNT, "rgb"); //assert(s->cameras.front.ci.bayer); - s->rgb_front_width = s->cameras.front.ci.frame_width/2; - s->rgb_front_height = s->cameras.front.ci.frame_height/2; + if (s->cameras.front.ci.bayer) { + s->rgb_front_width = s->cameras.front.ci.frame_width/2; + s->rgb_front_height = s->cameras.front.ci.frame_height/2; + } else { + s->rgb_front_width = s->cameras.front.ci.frame_width; + s->rgb_front_height = s->cameras.front.ci.frame_height; + } + for (int i=0; irgb_front_width, s->rgb_front_height, &s->rgb_front_bufs[i]); diff --git a/selfdrive/camerad/transforms/rgb_to_yuv.c b/selfdrive/camerad/transforms/rgb_to_yuv.c index 2626e6eebe..b9e66b36db 100644 --- a/selfdrive/camerad/transforms/rgb_to_yuv.c +++ b/selfdrive/camerad/transforms/rgb_to_yuv.c @@ -8,6 +8,7 @@ void rgb_to_yuv_init(RGBToYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride) { int err = 0; memset(s, 0, sizeof(*s)); + printf("width %d, height %d, rgb_stride %d\n", width, height, rgb_stride); assert(width % 2 == 0); assert(height % 2 == 0); s->width = width; diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 78590b514c..121b4aba16 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -539,7 +539,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not sm['pathPlan'].mpcSolutionValid: events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if not sm['pathPlan'].sensorValid: + if not sm['pathPlan'].sensorValid and os.getenv("NOSENSOR") is None: events.append(create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT])) if not sm['pathPlan'].paramsValid: events.append(create_event('vehicleModelInvalid', [ET.WARNING])) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 51c7adfe68..c3f65545df 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -11,6 +11,7 @@ import datetime from common.basedir import BASEDIR, PARAMS from common.android import ANDROID +WEBCAM = os.getenv("WEBCAM") is not None sys.path.append(os.path.join(BASEDIR, "pyextra")) os.environ['BASEDIR'] = BASEDIR @@ -211,6 +212,10 @@ car_started_processes = [ 'ubloxd', 'locationd', ] +if WEBCAM: + car_started_processes += [ + 'dmonitoringmodeld', + ] if ANDROID: car_started_processes += [ 'sensord', diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index 787608f91a..d3457c72a0 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -10,8 +10,19 @@ #define MODEL_HEIGHT 320 #define FULL_W 426 +#ifdef QCOM +#define input_lambda(x) (x - 128.f) * 0.0078125f +#else +#define input_lambda(x) x // for non SNPE running platforms, assume keras model instead has lambda layer +#endif + void dmonitoring_init(DMonitoringModelState* s) { - s->m = new DefaultRunModel("../../models/dmonitoring_model_q.dlc", (float*)&s->output, OUTPUT_SIZE, USE_DSP_RUNTIME); +#ifdef QCOM + const char* model_path = "../../models/dmonitoring_model_q.dlc"; +#else + const char* model_path = "../../models/dmonitoring_model.dlc"; +#endif + s->m = new DefaultRunModel(model_path, (float*)&s->output, OUTPUT_SIZE, USE_DSP_RUNTIME); } DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height) { @@ -84,27 +95,28 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ for (int r = 0; r < MODEL_HEIGHT/2; r++) { for (int c = 0; c < MODEL_WIDTH/2; c++) { // Y_ul - net_input_buf[(c*MODEL_HEIGHT/2) + r] = (resized_buf[(2*r*resized_width) + (2*c)] - 128.f) * 0.0078125f; + net_input_buf[(c*MODEL_HEIGHT/2) + r] = input_lambda(resized_buf[(2*r*resized_width) + (2*c)]); // Y_ur - net_input_buf[(c*MODEL_HEIGHT/2) + r + ((MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(2*r*resized_width) + (2*c+1)] - 128.f) * 0.0078125f; + net_input_buf[(c*MODEL_HEIGHT/2) + r + ((MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width) + (2*c+1)]); // Y_dl - net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(2*r*resized_width+1) + (2*c)] - 128.f) * 0.0078125f; + net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c)]); // Y_dr - net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(2*r*resized_width+1) + (2*c+1)] - 128.f) * 0.0078125f; + net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c+1)]); // U - net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(resized_width*resized_height) + (r*resized_width/2) + c] - 128.f) * 0.0078125f; + net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + (r*resized_width/2) + c]); // V - net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + (r*resized_width/2) + c] - 128.f) * 0.0078125f; + net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + (r*resized_width/2) + c]); } } - // FILE *dump_yuv_file = fopen("/sdcard/rawdump.yuv", "wb"); - // fwrite(raw_buf, height*width*3/2, sizeof(uint8_t), dump_yuv_file); - // fclose(dump_yuv_file); + //printf("preprocess completed. %d \n", yuv_buf_len); + //FILE *dump_yuv_file = fopen("/tmp/rawdump.yuv", "wb"); + //fwrite(raw_buf, height*width*3/2, sizeof(uint8_t), dump_yuv_file); + //fclose(dump_yuv_file); - // FILE *dump_yuv_file2 = fopen("/sdcard/inputdump.yuv", "wb"); - // fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file2); - // fclose(dump_yuv_file2); + //FILE *dump_yuv_file2 = fopen("/tmp/inputdump.yuv", "wb"); + //fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file2); + //fclose(dump_yuv_file2); delete[] cropped_buf; delete[] resized_buf; diff --git a/selfdrive/modeld/runners/keras_runner.py b/selfdrive/modeld/runners/keras_runner.py index b213c44e7e..4dfd197673 100755 --- a/selfdrive/modeld/runners/keras_runner.py +++ b/selfdrive/modeld/runners/keras_runner.py @@ -34,6 +34,11 @@ def run_loop(m): if __name__ == "__main__": print(tf.__version__, file=sys.stderr) + # limit gram alloc + gpus = tf.config.experimental.list_physical_devices('GPU') + if len(gpus) > 0: + tf.config.experimental.set_virtual_device_configuration(gpus[0], [tf.config.experimental.VirtualDeviceConfiguration(memory_limit=2048)]) + m = load_model(sys.argv[1]) print(m, file=sys.stderr) bs = [int(np.product(ii.shape[1:])) for ii in m.inputs] diff --git a/selfdrive/modeld/runners/tfmodel.cc b/selfdrive/modeld/runners/tfmodel.cc index 502ac4ba63..0f50614b8f 100644 --- a/selfdrive/modeld/runners/tfmodel.cc +++ b/selfdrive/modeld/runners/tfmodel.cc @@ -91,8 +91,12 @@ void TFModel::addDesire(float *state, int state_size) { void TFModel::execute(float *net_input_buf, int buf_size) { // order must be this pwrite(net_input_buf, buf_size); - pwrite(desire_input_buf, desire_state_size); - pwrite(rnn_input_buf, rnn_state_size); + if (desire_input_buf != NULL) { + pwrite(desire_input_buf, desire_state_size); + } + if (rnn_input_buf != NULL) { + pwrite(rnn_input_buf, rnn_state_size); + } pread(output, output_size); } diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index 6a62df40d5..6979e44f81 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -19,6 +19,7 @@ from selfdrive.car.honda.values import CruiseButtons parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot.') parser.add_argument('--autopilot', action='store_true') parser.add_argument('--joystick', action='store_true') +parser.add_argument('--realmonitoring', action='store_true') args = parser.parse_args() pm = messaging.PubMaster(['frame', 'sensorEvents', 'can']) @@ -68,6 +69,8 @@ def health_function(): rk.keep_time() def fake_driver_monitoring(): + if args.realmonitoring: + return pm = messaging.PubMaster(['driverState']) while 1: dat = messaging.new_message('driverState') @@ -198,7 +201,7 @@ def go(q): vel = vehicle.get_velocity() speed = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) * 3.6 - can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=cruise_button) + can_function(pm, speed, fake_wheel.angle, rk.frame, cruise_button=cruise_button, is_engaged=is_openpilot_engaged) if rk.frame%1 == 0: # 20Hz? throttle_op, brake_op, steer_torque_op = sendcan_function(sendcan) diff --git a/tools/sim/lib/can.py b/tools/sim/lib/can.py index 42e0fb87b5..509bb30198 100755 --- a/tools/sim/lib/can.py +++ b/tools/sim/lib/can.py @@ -20,7 +20,7 @@ SR = 7.5 def angle_to_sangle(angle): return - math.degrees(angle) * SR -def can_function(pm, speed, angle, idx, cruise_button=0): +def can_function(pm, speed, angle, idx, cruise_button=0, is_engaged=False): msg = [] msg.append(packer.make_can_msg("ENGINE_DATA", 0, {"XMISSION_SPEED": speed}, idx)) msg.append(packer.make_can_msg("WHEEL_SPEEDS", 0, @@ -50,6 +50,7 @@ def can_function(pm, speed, angle, idx, cruise_button=0): msg.append(packer.make_can_msg("CRUISE_PARAMS", 0, {}, idx)) msg.append(packer.make_can_msg("CRUISE", 0, {}, idx)) msg.append(packer.make_can_msg("SCM_FEEDBACK", 0, {"MAIN_ON": 1}, idx)) + msg.append(packer.make_can_msg("POWERTRAIN_DATA", 0, {"ACC_STATUS": int(is_engaged)}, idx)) #print(msg) # cam bus diff --git a/tools/webcam/README.md b/tools/webcam/README.md new file mode 100644 index 0000000000..0b3eccac3e --- /dev/null +++ b/tools/webcam/README.md @@ -0,0 +1,43 @@ +Run openpilot with webcam on PC/laptop +===================== +What's needed: +- Ubuntu 16.04 +- Python 3.7.3 +- GPU (recommended) +- Two USB webcams, at least 720p and 78 degrees FOV (e.g. Logitech C920/C615) +- [Car harness](https://comma.ai/shop/products/comma-car-harness) w/ black panda (or the outdated grey panda/giraffe combo) to connect to your car +- [Panda paw](https://comma.ai/shop/products/panda-paw) (or USB-A to USB-A cable) to connect panda to your computer +- Tape, Charger, ... +That's it! + +## Clone openpilot and install the requirements +``` +cd ~ +git clone https://github.com/commaai/openpilot.git +# Follow [this readme](https://github.com/commaai/openpilot/tree/master/tools) to install the requirements +# Add line "export PYTHONPATH=$HOME/openpilot" to your ~/.bashrc +# You may also need to install tensorflow-gpu 2.0.0 +# Install [OpenCV4](https://www.pyimagesearch.com/2018/08/15/how-to-install-opencv-4-on-ubuntu/) +``` +## Build openpilot for webcam +``` +cd ~/openpilot +scons use_webcam=1 +touch prebuilt +``` +## Connect the hardwares +``` +# Connect the road facing camera first, then the driver facing camera +# (default indexes are 1 and 2; can be modified in selfdrive/camerad/cameras/camera_webcam.cc) +# Connect your computer to panda +``` +## GO +``` +cd ~/openpilot/tools/webcam +./accept_terms.py # accept the user terms so that thermald can detect the car started +cd ~/openpilot/selfdrive +PASSIVE=0 NOSENSOR=1 ./manager.py +# Start the car, then the UI should show the road webcam's view +# Adjust and secure the webcams (you can run tools/webcam/front_mount_helper.py to help mount the driver camera) +# Finish calibration and engage! +``` diff --git a/tools/webcam/accept_terms.py b/tools/webcam/accept_terms.py new file mode 100755 index 0000000000..e7757511d8 --- /dev/null +++ b/tools/webcam/accept_terms.py @@ -0,0 +1,9 @@ +#!/usr/bin/env python +from common.params import Params +from selfdrive.version import terms_version, training_version + +if __name__ == '__main__': + params = Params() + params.put("HasAcceptedTerms", str(terms_version, 'utf-8')) + params.put("CompletedTrainingVersion", str(training_version, 'utf-8')) + print("Terms Accepted!") \ No newline at end of file diff --git a/tools/webcam/front_mount_helper.py b/tools/webcam/front_mount_helper.py new file mode 100755 index 0000000000..1fdca3225a --- /dev/null +++ b/tools/webcam/front_mount_helper.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python +import numpy as np + +# copied from common.transformations/camera.py +eon_dcam_focal_length = 860.0 # pixels +webcam_focal_length = 908.0 # pixels + +eon_dcam_intrinsics = np.array([ + [eon_dcam_focal_length, 0, 1152/2.], + [ 0, eon_dcam_focal_length, 864/2.], + [ 0, 0, 1]]) + +webcam_intrinsics = np.array([ + [webcam_focal_length, 0., 1280/2.], + [ 0., webcam_focal_length, 720/2.], + [ 0., 0., 1.]]) + +cam_id = 2 + +if __name__ == "__main__": + import cv2 + + trans_webcam_to_eon_front = np.dot(eon_dcam_intrinsics,np.linalg.inv(webcam_intrinsics)) + + cap = cv2.VideoCapture(cam_id) + cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) + + while (True): + ret, img = cap.read() + if ret: + img = cv2.warpPerspective(img, trans_webcam_to_eon_front, (1152,864), borderMode=cv2.BORDER_CONSTANT, borderValue=0) + img = img[:,-864//2:,:] + cv2.imshow('preview', img) + cv2.waitKey(10) \ No newline at end of file diff --git a/tools/webcam/warp_vis.py b/tools/webcam/warp_vis.py new file mode 100755 index 0000000000..2d7e425c5c --- /dev/null +++ b/tools/webcam/warp_vis.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python +import numpy as np + +# copied from common.transformations/camera.py +eon_focal_length = 910.0 # pixels +eon_dcam_focal_length = 860.0 # pixels + +webcam_focal_length = 908.0 # pixels + +eon_intrinsics = np.array([ + [eon_focal_length, 0., 1164/2.], + [ 0., eon_focal_length, 874/2.], + [ 0., 0., 1.]]) + +eon_dcam_intrinsics = np.array([ + [eon_dcam_focal_length, 0, 1152/2.], + [ 0, eon_dcam_focal_length, 864/2.], + [ 0, 0, 1]]) + +webcam_intrinsics = np.array([ + [webcam_focal_length, 0., 1280/2.], + [ 0., webcam_focal_length, 720/2.], + [ 0., 0., 1.]]) + +if __name__ == "__main__": + import cv2 + trans_webcam_to_eon_rear = np.dot(eon_intrinsics,np.linalg.inv(webcam_intrinsics)) + trans_webcam_to_eon_front = np.dot(eon_dcam_intrinsics,np.linalg.inv(webcam_intrinsics)) + print("trans_webcam_to_eon_rear:\n", trans_webcam_to_eon_rear) + print("trans_webcam_to_eon_front:\n", trans_webcam_to_eon_front) + + cap = cv2.VideoCapture(1) + cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) + + while (True): + ret, img = cap.read() + if ret: + # img = cv2.warpPerspective(img, trans_webcam_to_eon_rear, (1164,874), borderMode=cv2.BORDER_CONSTANT, borderValue=0) + img = cv2.warpPerspective(img, trans_webcam_to_eon_front, (1164,874), borderMode=cv2.BORDER_CONSTANT, borderValue=0) + print(img.shape, end='\r') + cv2.imshow('preview', img) + cv2.waitKey(10) + +