From da079d47d7d9496723117700ae1bb51c1f1abc38 Mon Sep 17 00:00:00 2001 From: George Hotz Date: Fri, 17 Jan 2020 11:20:17 -0800 Subject: [PATCH] logcatd, loggerd, mapd, modeld, proclogd --- selfdrive/logcatd/.gitignore | 1 + selfdrive/logcatd/SConscript | 2 + selfdrive/logcatd/logcatd.cc | 71 ++ selfdrive/loggerd/.gitignore | 1 + selfdrive/loggerd/SConscript | 12 + selfdrive/loggerd/__init__.py | 0 selfdrive/loggerd/config.py | 29 + selfdrive/loggerd/deleter.py | 39 + selfdrive/loggerd/encoder.c | 796 +++++++++++++++++ selfdrive/loggerd/encoder.h | 86 ++ selfdrive/loggerd/ethernetsniffer.py | 19 + selfdrive/loggerd/frame_logger.h | 81 ++ selfdrive/loggerd/include/msm_media_info.h | 819 ++++++++++++++++++ selfdrive/loggerd/logger.c | 217 +++++ selfdrive/loggerd/logger.h | 59 ++ selfdrive/loggerd/loggerd.cc | 738 ++++++++++++++++ selfdrive/loggerd/raw_logger.cc | 163 ++++ selfdrive/loggerd/raw_logger.h | 43 + selfdrive/loggerd/tests/Makefile | 35 + selfdrive/loggerd/tests/__init__.py | 0 selfdrive/loggerd/tests/fill_eon.py | 26 + .../loggerd/tests/loggerd_tests_common.py | 81 ++ selfdrive/loggerd/tests/test_deleter.py | 108 +++ selfdrive/loggerd/tests/test_uploader.py | 116 +++ selfdrive/loggerd/tests/testraw.cc | 57 ++ selfdrive/loggerd/uploader.py | 271 ++++++ selfdrive/mapd/__init__.py | 0 selfdrive/mapd/default_speeds.json | 106 +++ selfdrive/mapd/default_speeds_generator.py | 240 +++++ selfdrive/mapd/mapd.py | 296 +++++++ selfdrive/mapd/mapd_helpers.py | 364 ++++++++ selfdrive/modeld/SConscript | 36 + selfdrive/modeld/__init__.py | 0 selfdrive/modeld/constants.py | 6 + selfdrive/modeld/modeld | 4 + selfdrive/modeld/modeld.cc | 247 ++++++ selfdrive/modeld/models/commonmodel.c | 68 ++ selfdrive/modeld/models/commonmodel.h | 42 + selfdrive/modeld/models/driving.cc | 287 ++++++ selfdrive/modeld/models/driving.h | 81 ++ selfdrive/modeld/models/monitoring.cc | 163 ++++ selfdrive/modeld/models/monitoring.h | 44 + selfdrive/modeld/monitoringd | 5 + selfdrive/modeld/monitoringd.cc | 80 ++ selfdrive/modeld/runners/run.h | 18 + selfdrive/modeld/runners/runmodel.h | 12 + selfdrive/modeld/runners/snpemodel.cc | 126 +++ selfdrive/modeld/runners/snpemodel.h | 51 ++ selfdrive/modeld/runners/tfmodel.cc | 160 ++++ selfdrive/modeld/runners/tfmodel.h | 39 + selfdrive/modeld/test/polyfit/.gitignore | 1 + selfdrive/modeld/test/polyfit/Makefile | 15 + selfdrive/modeld/test/polyfit/data.h | 8 + selfdrive/modeld/test/polyfit/main.cc | 52 ++ selfdrive/modeld/test/polyfit/test.py | 24 + .../modeld/test/snpe_benchmark/.gitignore | 1 + .../modeld/test/snpe_benchmark/benchmark.cc | 190 ++++ .../modeld/test/snpe_benchmark/benchmark.sh | 3 + selfdrive/modeld/test/tf_test/build.sh | 2 + selfdrive/modeld/test/tf_test/main.cc | 69 ++ selfdrive/modeld/test/tf_test/pb_loader.py | 9 + selfdrive/modeld/transforms/loadyuv.c | 82 ++ selfdrive/modeld/transforms/loadyuv.cl | 43 + selfdrive/modeld/transforms/loadyuv.h | 34 + selfdrive/modeld/transforms/transform.c | 149 ++++ selfdrive/modeld/transforms/transform.cl | 54 ++ selfdrive/modeld/transforms/transform.h | 38 + selfdrive/modeld/visiontest.c | 160 ++++ selfdrive/modeld/visiontest.mk | 105 +++ selfdrive/modeld/visiontest.py | 135 +++ selfdrive/proclogd/SConscript | 2 + selfdrive/proclogd/proclogd.cc | 247 ++++++ 72 files changed, 7768 insertions(+) create mode 100644 selfdrive/logcatd/.gitignore create mode 100644 selfdrive/logcatd/SConscript create mode 100644 selfdrive/logcatd/logcatd.cc create mode 100644 selfdrive/loggerd/.gitignore create mode 100644 selfdrive/loggerd/SConscript create mode 100644 selfdrive/loggerd/__init__.py create mode 100644 selfdrive/loggerd/config.py create mode 100644 selfdrive/loggerd/deleter.py create mode 100644 selfdrive/loggerd/encoder.c create mode 100644 selfdrive/loggerd/encoder.h create mode 100755 selfdrive/loggerd/ethernetsniffer.py create mode 100644 selfdrive/loggerd/frame_logger.h create mode 100644 selfdrive/loggerd/include/msm_media_info.h create mode 100644 selfdrive/loggerd/logger.c create mode 100644 selfdrive/loggerd/logger.h create mode 100644 selfdrive/loggerd/loggerd.cc create mode 100644 selfdrive/loggerd/raw_logger.cc create mode 100644 selfdrive/loggerd/raw_logger.h create mode 100644 selfdrive/loggerd/tests/Makefile create mode 100644 selfdrive/loggerd/tests/__init__.py create mode 100755 selfdrive/loggerd/tests/fill_eon.py create mode 100644 selfdrive/loggerd/tests/loggerd_tests_common.py create mode 100644 selfdrive/loggerd/tests/test_deleter.py create mode 100644 selfdrive/loggerd/tests/test_uploader.py create mode 100644 selfdrive/loggerd/tests/testraw.cc create mode 100644 selfdrive/loggerd/uploader.py create mode 100644 selfdrive/mapd/__init__.py create mode 100644 selfdrive/mapd/default_speeds.json create mode 100755 selfdrive/mapd/default_speeds_generator.py create mode 100755 selfdrive/mapd/mapd.py create mode 100644 selfdrive/mapd/mapd_helpers.py create mode 100644 selfdrive/modeld/SConscript create mode 100644 selfdrive/modeld/__init__.py create mode 100644 selfdrive/modeld/constants.py create mode 100755 selfdrive/modeld/modeld create mode 100644 selfdrive/modeld/modeld.cc create mode 100644 selfdrive/modeld/models/commonmodel.c create mode 100644 selfdrive/modeld/models/commonmodel.h create mode 100644 selfdrive/modeld/models/driving.cc create mode 100644 selfdrive/modeld/models/driving.h create mode 100644 selfdrive/modeld/models/monitoring.cc create mode 100644 selfdrive/modeld/models/monitoring.h create mode 100755 selfdrive/modeld/monitoringd create mode 100644 selfdrive/modeld/monitoringd.cc create mode 100644 selfdrive/modeld/runners/run.h create mode 100644 selfdrive/modeld/runners/runmodel.h create mode 100644 selfdrive/modeld/runners/snpemodel.cc create mode 100644 selfdrive/modeld/runners/snpemodel.h create mode 100644 selfdrive/modeld/runners/tfmodel.cc create mode 100644 selfdrive/modeld/runners/tfmodel.h create mode 100644 selfdrive/modeld/test/polyfit/.gitignore create mode 100644 selfdrive/modeld/test/polyfit/Makefile create mode 100644 selfdrive/modeld/test/polyfit/data.h create mode 100644 selfdrive/modeld/test/polyfit/main.cc create mode 100644 selfdrive/modeld/test/polyfit/test.py create mode 100644 selfdrive/modeld/test/snpe_benchmark/.gitignore create mode 100644 selfdrive/modeld/test/snpe_benchmark/benchmark.cc create mode 100755 selfdrive/modeld/test/snpe_benchmark/benchmark.sh create mode 100755 selfdrive/modeld/test/tf_test/build.sh create mode 100644 selfdrive/modeld/test/tf_test/main.cc create mode 100755 selfdrive/modeld/test/tf_test/pb_loader.py create mode 100644 selfdrive/modeld/transforms/loadyuv.c create mode 100644 selfdrive/modeld/transforms/loadyuv.cl create mode 100644 selfdrive/modeld/transforms/loadyuv.h create mode 100644 selfdrive/modeld/transforms/transform.c create mode 100644 selfdrive/modeld/transforms/transform.cl create mode 100644 selfdrive/modeld/transforms/transform.h create mode 100644 selfdrive/modeld/visiontest.c create mode 100644 selfdrive/modeld/visiontest.mk create mode 100644 selfdrive/modeld/visiontest.py create mode 100644 selfdrive/proclogd/SConscript create mode 100644 selfdrive/proclogd/proclogd.cc diff --git a/selfdrive/logcatd/.gitignore b/selfdrive/logcatd/.gitignore new file mode 100644 index 000000000..c66f7622d --- /dev/null +++ b/selfdrive/logcatd/.gitignore @@ -0,0 +1 @@ +logcatd diff --git a/selfdrive/logcatd/SConscript b/selfdrive/logcatd/SConscript new file mode 100644 index 000000000..1040c3af7 --- /dev/null +++ b/selfdrive/logcatd/SConscript @@ -0,0 +1,2 @@ +Import('env', 'messaging') +env.Program('logcatd.cc', LIBS=[messaging, 'cutils', 'zmq', 'czmq', 'capnp', 'kj']) diff --git a/selfdrive/logcatd/logcatd.cc b/selfdrive/logcatd/logcatd.cc new file mode 100644 index 000000000..4baa97677 --- /dev/null +++ b/selfdrive/logcatd/logcatd.cc @@ -0,0 +1,71 @@ +#include +#include +#include +#include + +//#include +#include +#include + +#include +#include "common/timing.h" +#include "cereal/gen/cpp/log.capnp.h" +#include "messaging.hpp" + +int main() { + int err; + + struct logger_list *logger_list = android_logger_list_alloc(ANDROID_LOG_RDONLY, 0, 0); + assert(logger_list); + struct logger *main_logger = android_logger_open(logger_list, LOG_ID_MAIN); + assert(main_logger); + struct logger *radio_logger = android_logger_open(logger_list, LOG_ID_RADIO); + assert(radio_logger); + struct logger *system_logger = android_logger_open(logger_list, LOG_ID_SYSTEM); + assert(system_logger); + struct logger *crash_logger = android_logger_open(logger_list, LOG_ID_CRASH); + assert(crash_logger); +// struct logger *kernel_logger = android_logger_open(logger_list, LOG_ID_KERNEL); +// assert(kernel_logger); + + Context * c = Context::create(); + PubSocket * androidLog = PubSocket::create(c, "androidLog"); + assert(androidLog != NULL); + + while (1) { + log_msg log_msg; + err = android_logger_list_read(logger_list, &log_msg); + if (err <= 0) { + break; + } + + AndroidLogEntry entry; + err = android_log_processLogBuffer(&log_msg.entry_v1, &entry); + if (err < 0) { + continue; + } + + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto androidEntry = event.initAndroidLogEntry(); + androidEntry.setId(log_msg.id()); + androidEntry.setTs(entry.tv_sec * 1000000000ULL + entry.tv_nsec); + androidEntry.setPriority(entry.priority); + androidEntry.setPid(entry.pid); + androidEntry.setTid(entry.tid); + androidEntry.setTag(entry.tag); + androidEntry.setMessage(entry.message); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + androidLog->send((char*)bytes.begin(), bytes.size()); + } + + android_logger_list_close(logger_list); + + delete c; + delete androidLog; + + return 0; +} diff --git a/selfdrive/loggerd/.gitignore b/selfdrive/loggerd/.gitignore new file mode 100644 index 000000000..55081595a --- /dev/null +++ b/selfdrive/loggerd/.gitignore @@ -0,0 +1 @@ +loggerd diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript new file mode 100644 index 000000000..b319c773c --- /dev/null +++ b/selfdrive/loggerd/SConscript @@ -0,0 +1,12 @@ +Import('env', 'arch', 'messaging', 'common', 'visionipc') + +src = ['loggerd.cc', 'logger.c'] +libs = ['zmq', 'czmq', 'capnp', 'kj', 'yaml-cpp', 'z', + 'avformat', 'avcodec', 'swscale', 'avutil', + 'yuv', 'bz2', common, 'json', messaging, visionipc] + +if arch == "aarch64": + src += ['encoder.c', 'raw_logger.cc'] + libs += ['OmxVenc', 'OmxCore', 'cutils'] + +env.Program(src, LIBS=libs) diff --git a/selfdrive/loggerd/__init__.py b/selfdrive/loggerd/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/loggerd/config.py b/selfdrive/loggerd/config.py new file mode 100644 index 000000000..0ee6fa671 --- /dev/null +++ b/selfdrive/loggerd/config.py @@ -0,0 +1,29 @@ +import os + +if os.environ.get('LOGGERD_ROOT', False): + ROOT = os.environ['LOGGERD_ROOT'] + print("Custom loggerd root: ", ROOT) +else: + ROOT = '/data/media/0/realdata/' + +SEGMENT_LENGTH = 60 + + +def get_available_percent(default=None): + try: + statvfs = os.statvfs(ROOT) + available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks + except OSError: + available_percent = default + + return available_percent + + +def get_available_bytes(default=None): + try: + statvfs = os.statvfs(ROOT) + available_bytes = statvfs.f_bavail * statvfs.f_frsize + except OSError: + available_bytes = default + + return available_bytes diff --git a/selfdrive/loggerd/deleter.py b/selfdrive/loggerd/deleter.py new file mode 100644 index 000000000..1c687ff4b --- /dev/null +++ b/selfdrive/loggerd/deleter.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python3 +import os +import shutil +import threading +from selfdrive.swaglog import cloudlog +from selfdrive.loggerd.config import ROOT, get_available_bytes +from selfdrive.loggerd.uploader import listdir_by_creation + + +def deleter_thread(exit_event): + while not exit_event.is_set(): + available_bytes = get_available_bytes() + + if available_bytes is not None and available_bytes < (5 * 1024 * 1024 * 1024): + # remove the earliest directory we can + dirs = listdir_by_creation(ROOT) + for delete_dir in dirs: + delete_path = os.path.join(ROOT, delete_dir) + + if any(name.endswith(".lock") for name in os.listdir(delete_path)): + continue + + try: + cloudlog.info("deleting %s" % delete_path) + shutil.rmtree(delete_path) + break + except OSError: + cloudlog.exception("issue deleting %s" % delete_path) + exit_event.wait(.1) + else: + exit_event.wait(30) + + +def main(gctx=None): + deleter_thread(threading.Event()) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/loggerd/encoder.c b/selfdrive/loggerd/encoder.c new file mode 100644 index 000000000..747c7a195 --- /dev/null +++ b/selfdrive/loggerd/encoder.c @@ -0,0 +1,796 @@ +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + +#include + +#include + +#include + +#include "common/mutex.h" +#include "common/swaglog.h" + +#include "encoder.h" + +#define ALOG(...) __android_log_print(ANDROID_LOG_VERBOSE, "omxapp", ##__VA_ARGS__) + +// encoder: lossey codec using hardware hevc +static void wait_for_state(EncoderState *s, OMX_STATETYPE state) { + pthread_mutex_lock(&s->state_lock); + while (s->state != state) { + pthread_cond_wait(&s->state_cv, &s->state_lock); + } + pthread_mutex_unlock(&s->state_lock); +} + +static OMX_ERRORTYPE event_handler(OMX_HANDLETYPE component, OMX_PTR app_data, OMX_EVENTTYPE event, + OMX_U32 data1, OMX_U32 data2, OMX_PTR event_data) { + EncoderState *s = app_data; + + switch (event) { + case OMX_EventCmdComplete: + assert(data1 == OMX_CommandStateSet); + LOG("set state event 0x%x", data2); + pthread_mutex_lock(&s->state_lock); + s->state = data2; + pthread_cond_broadcast(&s->state_cv); + pthread_mutex_unlock(&s->state_lock); + break; + case OMX_EventError: + LOGE("OMX error 0x%08x", data1); + // assert(false); + break; + default: + LOGE("unhandled event %d", event); + assert(false); + break; + } + + pthread_mutex_unlock(&s->state_lock); + + return OMX_ErrorNone; +} + +static OMX_ERRORTYPE empty_buffer_done(OMX_HANDLETYPE component, OMX_PTR app_data, + OMX_BUFFERHEADERTYPE *buffer) { + EncoderState *s = app_data; + + // printf("empty_buffer_done\n"); + + queue_push(&s->free_in, (void*)buffer); + + return OMX_ErrorNone; +} + + +static OMX_ERRORTYPE fill_buffer_done(OMX_HANDLETYPE component, OMX_PTR app_data, + OMX_BUFFERHEADERTYPE *buffer) { + EncoderState *s = app_data; + + // printf("fill_buffer_done\n"); + + queue_push(&s->done_out, (void*)buffer); + + return OMX_ErrorNone; +} + +static OMX_CALLBACKTYPE omx_callbacks = { + .EventHandler = event_handler, + .EmptyBufferDone = empty_buffer_done, + .FillBufferDone = fill_buffer_done, +}; + +#define PORT_INDEX_IN 0 +#define PORT_INDEX_OUT 1 + +static const char* omx_color_fomat_name(uint32_t format) { + switch (format) { + case OMX_COLOR_FormatUnused: return "OMX_COLOR_FormatUnused"; + case OMX_COLOR_FormatMonochrome: return "OMX_COLOR_FormatMonochrome"; + case OMX_COLOR_Format8bitRGB332: return "OMX_COLOR_Format8bitRGB332"; + case OMX_COLOR_Format12bitRGB444: return "OMX_COLOR_Format12bitRGB444"; + case OMX_COLOR_Format16bitARGB4444: return "OMX_COLOR_Format16bitARGB4444"; + case OMX_COLOR_Format16bitARGB1555: return "OMX_COLOR_Format16bitARGB1555"; + case OMX_COLOR_Format16bitRGB565: return "OMX_COLOR_Format16bitRGB565"; + case OMX_COLOR_Format16bitBGR565: return "OMX_COLOR_Format16bitBGR565"; + case OMX_COLOR_Format18bitRGB666: return "OMX_COLOR_Format18bitRGB666"; + case OMX_COLOR_Format18bitARGB1665: return "OMX_COLOR_Format18bitARGB1665"; + case OMX_COLOR_Format19bitARGB1666: return "OMX_COLOR_Format19bitARGB1666"; + case OMX_COLOR_Format24bitRGB888: return "OMX_COLOR_Format24bitRGB888"; + case OMX_COLOR_Format24bitBGR888: return "OMX_COLOR_Format24bitBGR888"; + case OMX_COLOR_Format24bitARGB1887: return "OMX_COLOR_Format24bitARGB1887"; + case OMX_COLOR_Format25bitARGB1888: return "OMX_COLOR_Format25bitARGB1888"; + case OMX_COLOR_Format32bitBGRA8888: return "OMX_COLOR_Format32bitBGRA8888"; + case OMX_COLOR_Format32bitARGB8888: return "OMX_COLOR_Format32bitARGB8888"; + case OMX_COLOR_FormatYUV411Planar: return "OMX_COLOR_FormatYUV411Planar"; + case OMX_COLOR_FormatYUV411PackedPlanar: return "OMX_COLOR_FormatYUV411PackedPlanar"; + case OMX_COLOR_FormatYUV420Planar: return "OMX_COLOR_FormatYUV420Planar"; + case OMX_COLOR_FormatYUV420PackedPlanar: return "OMX_COLOR_FormatYUV420PackedPlanar"; + case OMX_COLOR_FormatYUV420SemiPlanar: return "OMX_COLOR_FormatYUV420SemiPlanar"; + case OMX_COLOR_FormatYUV422Planar: return "OMX_COLOR_FormatYUV422Planar"; + case OMX_COLOR_FormatYUV422PackedPlanar: return "OMX_COLOR_FormatYUV422PackedPlanar"; + case OMX_COLOR_FormatYUV422SemiPlanar: return "OMX_COLOR_FormatYUV422SemiPlanar"; + case OMX_COLOR_FormatYCbYCr: return "OMX_COLOR_FormatYCbYCr"; + case OMX_COLOR_FormatYCrYCb: return "OMX_COLOR_FormatYCrYCb"; + case OMX_COLOR_FormatCbYCrY: return "OMX_COLOR_FormatCbYCrY"; + case OMX_COLOR_FormatCrYCbY: return "OMX_COLOR_FormatCrYCbY"; + case OMX_COLOR_FormatYUV444Interleaved: return "OMX_COLOR_FormatYUV444Interleaved"; + case OMX_COLOR_FormatRawBayer8bit: return "OMX_COLOR_FormatRawBayer8bit"; + case OMX_COLOR_FormatRawBayer10bit: return "OMX_COLOR_FormatRawBayer10bit"; + case OMX_COLOR_FormatRawBayer8bitcompressed: return "OMX_COLOR_FormatRawBayer8bitcompressed"; + case OMX_COLOR_FormatL2: return "OMX_COLOR_FormatL2"; + case OMX_COLOR_FormatL4: return "OMX_COLOR_FormatL4"; + case OMX_COLOR_FormatL8: return "OMX_COLOR_FormatL8"; + case OMX_COLOR_FormatL16: return "OMX_COLOR_FormatL16"; + case OMX_COLOR_FormatL24: return "OMX_COLOR_FormatL24"; + case OMX_COLOR_FormatL32: return "OMX_COLOR_FormatL32"; + case OMX_COLOR_FormatYUV420PackedSemiPlanar: return "OMX_COLOR_FormatYUV420PackedSemiPlanar"; + case OMX_COLOR_FormatYUV422PackedSemiPlanar: return "OMX_COLOR_FormatYUV422PackedSemiPlanar"; + case OMX_COLOR_Format18BitBGR666: return "OMX_COLOR_Format18BitBGR666"; + case OMX_COLOR_Format24BitARGB6666: return "OMX_COLOR_Format24BitARGB6666"; + case OMX_COLOR_Format24BitABGR6666: return "OMX_COLOR_Format24BitABGR6666"; + + case OMX_COLOR_FormatAndroidOpaque: return "OMX_COLOR_FormatAndroidOpaque"; + case OMX_TI_COLOR_FormatYUV420PackedSemiPlanar: return "OMX_TI_COLOR_FormatYUV420PackedSemiPlanar"; + case OMX_QCOM_COLOR_FormatYVU420SemiPlanar: return "OMX_QCOM_COLOR_FormatYVU420SemiPlanar"; + case OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka: return "OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka"; + case OMX_SEC_COLOR_FormatNV12Tiled: return "OMX_SEC_COLOR_FormatNV12Tiled"; + case OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar32m: return "OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar32m"; + + // case QOMX_COLOR_FormatYVU420SemiPlanar: return "QOMX_COLOR_FormatYVU420SemiPlanar"; + case QOMX_COLOR_FormatYVU420PackedSemiPlanar32m4ka: return "QOMX_COLOR_FormatYVU420PackedSemiPlanar32m4ka"; + case QOMX_COLOR_FormatYUV420PackedSemiPlanar16m2ka: return "QOMX_COLOR_FormatYUV420PackedSemiPlanar16m2ka"; + // case QOMX_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka: return "QOMX_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka"; + // case QOMX_COLOR_FORMATYUV420PackedSemiPlanar32m: return "QOMX_COLOR_FORMATYUV420PackedSemiPlanar32m"; + case QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mMultiView: return "QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mMultiView"; + case QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mCompressed: return "QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mCompressed"; + case QOMX_COLOR_Format32bitRGBA8888: return "QOMX_COLOR_Format32bitRGBA8888"; + case QOMX_COLOR_Format32bitRGBA8888Compressed: return "QOMX_COLOR_Format32bitRGBA8888Compressed"; + + default: + return "unkn"; + } +} + +void encoder_init(EncoderState *s, const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale) { + int err; + + memset(s, 0, sizeof(*s)); + s->filename = filename; + s->width = width; + s->height = height; + s->fps = fps; + mutex_init_reentrant(&s->lock); + + if (!h265) { + s->remuxing = true; + } + + if (downscale) { + s->downscale = true; + s->y_ptr2 = malloc(s->width*s->height); + s->u_ptr2 = malloc(s->width*s->height/4); + s->v_ptr2 = malloc(s->width*s->height/4); + } + + s->segment = -1; + + s->state = OMX_StateLoaded; + + s->codec_config = NULL; + + queue_init(&s->free_in); + queue_init(&s->done_out); + + pthread_mutex_init(&s->state_lock, NULL); + pthread_cond_init(&s->state_cv, NULL); + + if (h265) { + err = OMX_GetHandle(&s->handle, (OMX_STRING)"OMX.qcom.video.encoder.hevc", + s, &omx_callbacks); + } else { + err = OMX_GetHandle(&s->handle, (OMX_STRING)"OMX.qcom.video.encoder.avc", + s, &omx_callbacks); + } + assert(err == OMX_ErrorNone); + // printf("handle: %p\n", s->handle); + + // setup input port + + OMX_PARAM_PORTDEFINITIONTYPE in_port = {0}; + in_port.nSize = sizeof(in_port); + in_port.nPortIndex = (OMX_U32) PORT_INDEX_IN; + err = OMX_GetParameter(s->handle, OMX_IndexParamPortDefinition, + (OMX_PTR) &in_port); + assert(err == OMX_ErrorNone); + + in_port.format.video.nFrameWidth = s->width; + in_port.format.video.nFrameHeight = s->height; + in_port.format.video.nStride = VENUS_Y_STRIDE(COLOR_FMT_NV12, s->width); + in_port.format.video.nSliceHeight = s->height; + // in_port.nBufferSize = (s->width * s->height * 3) / 2; + in_port.nBufferSize = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, s->width, s->height); + in_port.format.video.xFramerate = (s->fps * 65536); + in_port.format.video.eCompressionFormat = OMX_VIDEO_CodingUnused; + // in_port.format.video.eColorFormat = OMX_COLOR_FormatYUV420SemiPlanar; + in_port.format.video.eColorFormat = QOMX_COLOR_FORMATYUV420PackedSemiPlanar32m; + + err = OMX_SetParameter(s->handle, OMX_IndexParamPortDefinition, + (OMX_PTR) &in_port); + assert(err == OMX_ErrorNone); + + + err = OMX_GetParameter(s->handle, OMX_IndexParamPortDefinition, + (OMX_PTR) &in_port); + assert(err == OMX_ErrorNone); + s->num_in_bufs = in_port.nBufferCountActual; + + // printf("in width: %d, stride: %d\n", + // in_port.format.video.nFrameWidth, in_port.format.video.nStride); + + // setup output port + + OMX_PARAM_PORTDEFINITIONTYPE out_port = {0}; + out_port.nSize = sizeof(out_port); + out_port.nPortIndex = (OMX_U32) PORT_INDEX_OUT; + err = OMX_GetParameter(s->handle, OMX_IndexParamPortDefinition, + (OMX_PTR)&out_port); + assert(err == OMX_ErrorNone); + out_port.format.video.nFrameWidth = s->width; + out_port.format.video.nFrameHeight = s->height; + out_port.format.video.xFramerate = 0; + out_port.format.video.nBitrate = bitrate; + if (h265) { + out_port.format.video.eCompressionFormat = OMX_VIDEO_CodingHEVC; + } else { + out_port.format.video.eCompressionFormat = OMX_VIDEO_CodingAVC; + } + out_port.format.video.eColorFormat = OMX_COLOR_FormatUnused; + + err = OMX_SetParameter(s->handle, OMX_IndexParamPortDefinition, + (OMX_PTR) &out_port); + assert(err == OMX_ErrorNone); + + err = OMX_GetParameter(s->handle, OMX_IndexParamPortDefinition, + (OMX_PTR) &out_port); + assert(err == OMX_ErrorNone); + s->num_out_bufs = out_port.nBufferCountActual; + + OMX_VIDEO_PARAM_BITRATETYPE bitrate_type = {0}; + bitrate_type.nSize = sizeof(bitrate_type); + bitrate_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT; + err = OMX_GetParameter(s->handle, OMX_IndexParamVideoBitrate, + (OMX_PTR) &bitrate_type); + assert(err == OMX_ErrorNone); + + bitrate_type.eControlRate = OMX_Video_ControlRateVariable; + bitrate_type.nTargetBitrate = bitrate; + + err = OMX_SetParameter(s->handle, OMX_IndexParamVideoBitrate, + (OMX_PTR) &bitrate_type); + assert(err == OMX_ErrorNone); + + if (h265) { + // setup HEVC + OMX_VIDEO_PARAM_HEVCTYPE hecv_type = {0}; + hecv_type.nSize = sizeof(hecv_type); + hecv_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT; + err = OMX_GetParameter(s->handle, (OMX_INDEXTYPE)OMX_IndexParamVideoHevc, + (OMX_PTR) &hecv_type); + assert(err == OMX_ErrorNone); + + hecv_type.eProfile = OMX_VIDEO_HEVCProfileMain; + hecv_type.eLevel = OMX_VIDEO_HEVCHighTierLevel5; + + err = OMX_SetParameter(s->handle, (OMX_INDEXTYPE)OMX_IndexParamVideoHevc, + (OMX_PTR) &hecv_type); + assert(err == OMX_ErrorNone); + } else { + // setup h264 + OMX_VIDEO_PARAM_AVCTYPE avc = { 0 }; + avc.nSize = sizeof(avc); + avc.nPortIndex = (OMX_U32) PORT_INDEX_OUT; + err = OMX_GetParameter(s->handle, OMX_IndexParamVideoAvc, &avc); + assert(err == OMX_ErrorNone); + + avc.nBFrames = 0; + avc.nPFrames = 15; + + avc.eProfile = OMX_VIDEO_AVCProfileBaseline; + avc.eLevel = OMX_VIDEO_AVCLevel31; + + avc.nAllowedPictureTypes |= OMX_VIDEO_PictureTypeB; + avc.eLoopFilterMode = OMX_VIDEO_AVCLoopFilterEnable; + + err = OMX_SetParameter(s->handle, OMX_IndexParamVideoAvc, &avc); + assert(err == OMX_ErrorNone); + } + + + // for (int i = 0; ; i++) { + // OMX_VIDEO_PARAM_PORTFORMATTYPE video_port_format = {0}; + // video_port_format.nSize = sizeof(video_port_format); + // video_port_format.nIndex = i; + // video_port_format.nPortIndex = PORT_INDEX_IN; + // if (OMX_GetParameter(s->handle, OMX_IndexParamVideoPortFormat, &video_port_format) != OMX_ErrorNone) + // break; + // printf("in %d: compression 0x%x format 0x%x %s\n", i, + // video_port_format.eCompressionFormat, video_port_format.eColorFormat, + // omx_color_fomat_name(video_port_format.eColorFormat)); + // } + + // for (int i=0; i<32; i++) { + // OMX_VIDEO_PARAM_PROFILELEVELTYPE params = {0}; + // params.nSize = sizeof(params); + // params.nPortIndex = PORT_INDEX_OUT; + // params.nProfileIndex = i; + // if (OMX_GetParameter(s->handle, OMX_IndexParamVideoProfileLevelQuerySupported, ¶ms) != OMX_ErrorNone) + // break; + // printf("profile %d level 0x%x\n", params.eProfile, params.eLevel); + // } + + err = OMX_SendCommand(s->handle, OMX_CommandStateSet, OMX_StateIdle, NULL); + assert(err == OMX_ErrorNone); + + s->in_buf_headers = calloc(s->num_in_bufs, sizeof(OMX_BUFFERHEADERTYPE*)); + for (int i=0; inum_in_bufs; i++) { + err = OMX_AllocateBuffer(s->handle, &s->in_buf_headers[i], PORT_INDEX_IN, s, + in_port.nBufferSize); + assert(err == OMX_ErrorNone); + } + + s->out_buf_headers = calloc(s->num_out_bufs, sizeof(OMX_BUFFERHEADERTYPE*)); + for (int i=0; inum_out_bufs; i++) { + err = OMX_AllocateBuffer(s->handle, &s->out_buf_headers[i], PORT_INDEX_OUT, s, + out_port.nBufferSize); + assert(err == OMX_ErrorNone); + } + + wait_for_state(s, OMX_StateIdle); + + err = OMX_SendCommand(s->handle, OMX_CommandStateSet, OMX_StateExecuting, NULL); + assert(err == OMX_ErrorNone); + + wait_for_state(s, OMX_StateExecuting); + + // give omx all the output buffers + for (int i = 0; i < s->num_out_bufs; i++) { + // printf("fill %p\n", s->out_buf_headers[i]); + err = OMX_FillThisBuffer(s->handle, s->out_buf_headers[i]); + assert(err == OMX_ErrorNone); + } + + // fill the input free queue + for (int i = 0; i < s->num_in_bufs; i++) { + queue_push(&s->free_in, (void*)s->in_buf_headers[i]); + } +} + +static void handle_out_buf(EncoderState *s, OMX_BUFFERHEADERTYPE *out_buf) { + int err; + uint8_t *buf_data = out_buf->pBuffer + out_buf->nOffset; + + if (out_buf->nFlags & OMX_BUFFERFLAG_CODECCONFIG) { + if (s->codec_config_len < out_buf->nFilledLen) { + s->codec_config = realloc(s->codec_config, out_buf->nFilledLen); + } + s->codec_config_len = out_buf->nFilledLen; + memcpy(s->codec_config, buf_data, out_buf->nFilledLen); + } + + if (s->stream_sock_raw) { + uint64_t current_time = nanos_since_boot(); + uint64_t diff = current_time - out_buf->nTimeStamp*1000LL; + double msdiff = (double) diff / 1000000.0; + // printf("encoded latency to tsEof: %f\n", msdiff); + zmq_send(s->stream_sock_raw, &out_buf->nTimeStamp, sizeof(out_buf->nTimeStamp), ZMQ_SNDMORE); + zmq_send(s->stream_sock_raw, buf_data, out_buf->nFilledLen, 0); + } + + if (s->of) { + //printf("write %d flags 0x%x\n", out_buf->nFilledLen, out_buf->nFlags); + fwrite(buf_data, out_buf->nFilledLen, 1, s->of); + } + + if (s->remuxing) { + if (!s->wrote_codec_config && s->codec_config_len > 0) { + if (s->codec_ctx->extradata_size < s->codec_config_len) { + s->codec_ctx->extradata = realloc(s->codec_ctx->extradata, s->codec_config_len + AV_INPUT_BUFFER_PADDING_SIZE); + } + s->codec_ctx->extradata_size = s->codec_config_len; + memcpy(s->codec_ctx->extradata, s->codec_config, s->codec_config_len); + memset(s->codec_ctx->extradata + s->codec_ctx->extradata_size, 0, AV_INPUT_BUFFER_PADDING_SIZE); + + err = avcodec_parameters_from_context(s->out_stream->codecpar, s->codec_ctx); + assert(err >= 0); + err = avformat_write_header(s->ofmt_ctx, NULL); + assert(err >= 0); + + s->wrote_codec_config = true; + } + + if (out_buf->nTimeStamp > 0) { + // input timestamps are in microseconds + AVRational in_timebase = {1, 1000000}; + + AVPacket pkt; + av_init_packet(&pkt); + pkt.data = buf_data; + pkt.size = out_buf->nFilledLen; + pkt.pts = pkt.dts = av_rescale_q_rnd(out_buf->nTimeStamp, in_timebase, s->ofmt_ctx->streams[0]->time_base, AV_ROUND_NEAR_INF|AV_ROUND_PASS_MINMAX); + pkt.duration = av_rescale_q(50*1000, in_timebase, s->ofmt_ctx->streams[0]->time_base); + + if (out_buf->nFlags & OMX_BUFFERFLAG_SYNCFRAME) { + pkt.flags |= AV_PKT_FLAG_KEY; + } + + err = av_write_frame(s->ofmt_ctx, &pkt); + if (err < 0) { LOGW("ts encoder write issue"); } + + av_free_packet(&pkt); + } + } + + // give omx back the buffer + err = OMX_FillThisBuffer(s->handle, out_buf); + assert(err == OMX_ErrorNone); +} + +int encoder_encode_frame(EncoderState *s, + const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, + int in_width, int in_height, + int *frame_segment, VIPCBufExtra *extra) { + int err; + pthread_mutex_lock(&s->lock); + + if (s->opening) { + encoder_open(s, s->next_path); + s->opening = false; + } + + if (!s->open) { + pthread_mutex_unlock(&s->lock); + return -1; + } + + // this sometimes freezes... put it outside the encoder lock so we can still trigger rotates... + // THIS IS A REALLY BAD IDEA, but apparently the race has to happen 30 times to trigger this + pthread_mutex_unlock(&s->lock); + OMX_BUFFERHEADERTYPE* in_buf = queue_pop(&s->free_in); + pthread_mutex_lock(&s->lock); + + if (s->rotating) { + encoder_close(s); + encoder_open(s, s->next_path); + s->segment = s->next_segment; + s->rotating = false; + } + + int ret = s->counter; + + uint8_t *in_buf_ptr = in_buf->pBuffer; + // printf("in_buf ptr %p\n", in_buf_ptr); + + uint8_t *in_y_ptr = in_buf_ptr; + int in_y_stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, s->width); + int in_uv_stride = VENUS_UV_STRIDE(COLOR_FMT_NV12, s->width); + // uint8_t *in_uv_ptr = in_buf_ptr + (s->width * s->height); + uint8_t *in_uv_ptr = in_buf_ptr + (in_y_stride * VENUS_Y_SCANLINES(COLOR_FMT_NV12, s->height)); + + if (s->downscale) { + I420Scale(y_ptr, in_width, + u_ptr, in_width/2, + v_ptr, in_width/2, + in_width, in_height, + s->y_ptr2, s->width, + s->u_ptr2, s->width/2, + s->v_ptr2, s->width/2, + s->width, s->height, + kFilterNone); + y_ptr = s->y_ptr2; + u_ptr = s->u_ptr2; + v_ptr = s->v_ptr2; + } + err = I420ToNV12(y_ptr, s->width, + u_ptr, s->width/2, + v_ptr, s->width/2, + in_y_ptr, in_y_stride, + in_uv_ptr, in_uv_stride, + s->width, s->height); + assert(err == 0); + + // in_buf->nFilledLen = (s->width*s->height) + (s->width*s->height/2); + in_buf->nFilledLen = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, s->width, s->height); + in_buf->nFlags = OMX_BUFFERFLAG_ENDOFFRAME; + in_buf->nOffset = 0; + in_buf->nTimeStamp = extra->timestamp_eof/1000LL; // OMX_TICKS, in microseconds + + err = OMX_EmptyThisBuffer(s->handle, in_buf); + assert(err == OMX_ErrorNone); + + // pump output + while (true) { + OMX_BUFFERHEADERTYPE *out_buf = queue_try_pop(&s->done_out); + if (!out_buf) { + break; + } + handle_out_buf(s, out_buf); + } + + s->dirty = true; + + s->counter++; + + if (frame_segment) { + *frame_segment = s->segment; + } + + if (s->closing) { + encoder_close(s); + s->closing = false; + } + + pthread_mutex_unlock(&s->lock); + return ret; +} + +void encoder_open(EncoderState *s, const char* path) { + int err; + + pthread_mutex_lock(&s->lock); + + snprintf(s->vid_path, sizeof(s->vid_path), "%s/%s", path, s->filename); + + if (s->remuxing) { + avformat_alloc_output_context2(&s->ofmt_ctx, NULL, NULL, s->vid_path); + assert(s->ofmt_ctx); + + s->out_stream = avformat_new_stream(s->ofmt_ctx, NULL); + assert(s->out_stream); + + // set codec correctly + av_register_all(); + + AVCodec *codec = NULL; + codec = avcodec_find_encoder(AV_CODEC_ID_H264); + assert(codec); + + s->codec_ctx = avcodec_alloc_context3(codec); + assert(s->codec_ctx); + s->codec_ctx->width = s->width; + s->codec_ctx->height = s->height; + s->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P; + s->codec_ctx->time_base = (AVRational){ 1, s->fps }; + + err = avio_open(&s->ofmt_ctx->pb, s->vid_path, AVIO_FLAG_WRITE); + assert(err >= 0); + + s->wrote_codec_config = false; + } else { + s->of = fopen(s->vid_path, "wb"); + assert(s->of); + if (s->codec_config_len > 0) { + fwrite(s->codec_config, s->codec_config_len, 1, s->of); + } + } + + // create camera lock file + snprintf(s->lock_path, sizeof(s->lock_path), "%s/%s.lock", path, s->filename); + int lock_fd = open(s->lock_path, O_RDWR | O_CREAT, 0777); + assert(lock_fd >= 0); + close(lock_fd); + + s->open = true; + s->counter = 0; + + pthread_mutex_unlock(&s->lock); +} + +void encoder_close(EncoderState *s) { + int err; + + pthread_mutex_lock(&s->lock); + + if (s->open) { + if (s->dirty) { + // drain output only if there could be frames in the encoder + + OMX_BUFFERHEADERTYPE* in_buf = queue_pop(&s->free_in); + in_buf->nFilledLen = 0; + in_buf->nOffset = 0; + in_buf->nFlags = OMX_BUFFERFLAG_EOS; + in_buf->nTimeStamp = 0; + + err = OMX_EmptyThisBuffer(s->handle, in_buf); + assert(err == OMX_ErrorNone); + + while (true) { + OMX_BUFFERHEADERTYPE *out_buf = queue_pop(&s->done_out); + + handle_out_buf(s, out_buf); + + if (out_buf->nFlags & OMX_BUFFERFLAG_EOS) { + break; + } + } + s->dirty = false; + } + + if (s->remuxing) { + av_write_trailer(s->ofmt_ctx); + avio_closep(&s->ofmt_ctx->pb); + avformat_free_context(s->ofmt_ctx); + } else { + fclose(s->of); + } + unlink(s->lock_path); + } + s->open = false; + + pthread_mutex_unlock(&s->lock); +} + +void encoder_rotate(EncoderState *s, const char* new_path, int new_segment) { + pthread_mutex_lock(&s->lock); + snprintf(s->next_path, sizeof(s->next_path), "%s", new_path); + s->next_segment = new_segment; + if (s->open) { + if (s->next_segment == -1) { + s->closing = true; + } else { + s->rotating = true; + } + } else { + s->segment = s->next_segment; + s->opening = true; + } + pthread_mutex_unlock(&s->lock); +} + +void encoder_destroy(EncoderState *s) { + int err; + + assert(!s->open); + + err = OMX_SendCommand(s->handle, OMX_CommandStateSet, OMX_StateIdle, NULL); + assert(err == OMX_ErrorNone); + + wait_for_state(s, OMX_StateIdle); + + err = OMX_SendCommand(s->handle, OMX_CommandStateSet, OMX_StateLoaded, NULL); + assert(err == OMX_ErrorNone); + + for (int i=0; inum_in_bufs; i++) { + err = OMX_FreeBuffer(s->handle, PORT_INDEX_IN, s->in_buf_headers[i]); + assert(err == OMX_ErrorNone); + } + free(s->in_buf_headers); + + for (int i=0; inum_out_bufs; i++) { + err = OMX_FreeBuffer(s->handle, PORT_INDEX_OUT, s->out_buf_headers[i]); + assert(err == OMX_ErrorNone); + } + free(s->out_buf_headers); + + wait_for_state(s, OMX_StateLoaded); + + err = OMX_FreeHandle(s->handle); + assert(err == OMX_ErrorNone); + + if (s->downscale) { + free(s->y_ptr2); + free(s->u_ptr2); + free(s->v_ptr2); + } +} + +#if 0 + +// cd one/selfdrive/visiond +// clang +// -fPIC -pie +// -std=gnu11 -O2 -g +// -o encoder +// -I ~/one/selfdrive +// -I ~/one/phonelibs/openmax/include +// -I ~/one/phonelibs/libyuv/include +// -lOmxVenc -lOmxCore -llog +// encoder.c +// buffering.c +// -L ~/one/phonelibs/libyuv/lib -l:libyuv.a + +int main() { + int err; + + EncoderState state; + EncoderState *s = &state; + memset(s, 0, sizeof(*s)); + + int w = 1164; + int h = 874; + + encoder_init(s, w, h, 20); + printf("inited\n"); + + encoder_open(s, "/sdcard/t1"); + + // uint8_t *tmpy = malloc(640*480); + // uint8_t *tmpu = malloc((640/2)*(480/2)); + // uint8_t *tmpv = malloc((640/2)*(480/2)); + + // memset(tmpy, 0, 640*480); + // // memset(tmpu, 0xff, (640/2)*(480/2)); + // memset(tmpv, 0, (640/2)*(480/2)); + +// #if 0 + // FILE *infile = fopen("/sdcard/camera_t2.yuv", "rb"); + uint8_t *inbuf = malloc(w*h*3/2); + memset(inbuf, 0, w*h*3/2); + + for (int i=0; i<20*3+5; i++) { + + // fread(inbuf, w*h*3/2, 1, infile); + + uint8_t *tmpy = inbuf; + uint8_t *tmpu = inbuf + w*h; + uint8_t *tmpv = inbuf + w*h + (w/2)*(h/2); + + for (int y=0; yof); + // s->of = fopen("/sdcard/tmpout2.hevc", "wb"); + // if (s->codec_config) { + // fwrite(s->codec_config, s->codec_config_len, 1, s->of); + // } + // encoder_open(s, "/sdcard/t1"); + + encoder_rotate(s, "/sdcard/t2"); + + for (int i=0; i<20*3+5; i++) { + + // fread(inbuf, w*h*3/2, 1, infile); + + uint8_t *tmpy = inbuf; + uint8_t *tmpu = inbuf + w*h; + uint8_t *tmpv = inbuf + w*h + (w/2)*(h/2); + + for (int y=0; y +#include +#include + +#include + +#include +#include + +#include "common/cqueue.h" +#include "common/visionipc.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct EncoderState { + pthread_mutex_t lock; + int width, height, fps; + const char* path; + char vid_path[1024]; + char lock_path[1024]; + bool open; + bool dirty; + int counter; + int segment; + + bool rotating; + bool closing; + bool opening; + char next_path[1024]; + int next_segment; + + const char* filename; + FILE *of; + + size_t codec_config_len; + uint8_t *codec_config; + bool wrote_codec_config; + + pthread_mutex_t state_lock; + pthread_cond_t state_cv; + OMX_STATETYPE state; + + OMX_HANDLETYPE handle; + + int num_in_bufs; + OMX_BUFFERHEADERTYPE** in_buf_headers; + + int num_out_bufs; + OMX_BUFFERHEADERTYPE** out_buf_headers; + + Queue free_in; + Queue done_out; + + void *stream_sock_raw; + + AVFormatContext *ofmt_ctx; + AVCodecContext *codec_ctx; + AVStream *out_stream; + bool remuxing; + + void *zmq_ctx; + + bool downscale; + uint8_t *y_ptr2, *u_ptr2, *v_ptr2; +} EncoderState; + +void encoder_init(EncoderState *s, const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale); +int encoder_encode_frame(EncoderState *s, + const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, + int in_width, int in_height, + int *frame_segment, VIPCBufExtra *extra); +void encoder_open(EncoderState *s, const char* path); +void encoder_rotate(EncoderState *s, const char* new_path, int new_segment); +void encoder_close(EncoderState *s); +void encoder_destroy(EncoderState *s); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/selfdrive/loggerd/ethernetsniffer.py b/selfdrive/loggerd/ethernetsniffer.py new file mode 100755 index 000000000..1b30b9671 --- /dev/null +++ b/selfdrive/loggerd/ethernetsniffer.py @@ -0,0 +1,19 @@ +#!/usr/bin/env python3 +import zmq +import cereal.messaging as messaging +from cereal.services import service_list +import pcap + +def main(gctx=None): + ethernetData = messaging.pub_sock('ethernetData') + + for ts, pkt in pcap.pcap('eth0'): + dat = messaging.new_message() + dat.init('ethernetData', 1) + dat.ethernetData[0].ts = ts + dat.ethernetData[0].pkt = str(pkt) + ethernetData.send(dat.to_bytes()) + +if __name__ == "__main__": + main() + diff --git a/selfdrive/loggerd/frame_logger.h b/selfdrive/loggerd/frame_logger.h new file mode 100644 index 000000000..bfc0681b7 --- /dev/null +++ b/selfdrive/loggerd/frame_logger.h @@ -0,0 +1,81 @@ +#ifndef FRAMELOGGER_H +#define FRAMELOGGER_H + +#include + +#include +#include + +class FrameLogger { +public: + virtual ~FrameLogger() {} + + virtual void Open(const std::string &path) = 0; + virtual void Close() = 0; + + int LogFrame(uint64_t ts, const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, int *frame_segment) { + std::lock_guard guard(lock); + + if (opening) { + Open(next_path); + opening = false; + } + + if (!is_open) return -1; + + if (rotating) { + Close(); + Open(next_path); + segment = next_segment; + rotating = false; + } + + int ret = ProcessFrame(ts, y_ptr, u_ptr, v_ptr); + + if (ret >= 0 && frame_segment) { + *frame_segment = segment; + } + + if (closing) { + Close(); + closing = false; + } + + return ret; + } + + void Rotate(const std::string &new_path, int new_segment) { + std::lock_guard guard(lock); + + next_path = new_path; + next_segment = new_segment; + if (is_open) { + if (next_segment == -1) { + closing = true; + } else { + rotating = true; + } + } else { + segment = next_segment; + opening = true; + } + } + +protected: + + virtual int ProcessFrame(uint64_t ts, const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr) = 0; + + std::recursive_mutex lock; + + bool is_open = false; + int segment = -1; + + std::string vid_path, lock_path; + +private: + int next_segment = -1; + bool opening = false, closing = false, rotating = false; + std::string next_path; +}; + +#endif diff --git a/selfdrive/loggerd/include/msm_media_info.h b/selfdrive/loggerd/include/msm_media_info.h new file mode 100644 index 000000000..39dceb2c4 --- /dev/null +++ b/selfdrive/loggerd/include/msm_media_info.h @@ -0,0 +1,819 @@ +#ifndef __MEDIA_INFO_H__ +#define __MEDIA_INFO_H__ + +#ifndef MSM_MEDIA_ALIGN +#define MSM_MEDIA_ALIGN(__sz, __align) (((__sz) + (__align-1)) & (~(__align-1))) +#endif + +#ifndef MSM_MEDIA_ROUNDUP +#define MSM_MEDIA_ROUNDUP(__sz, __r) (((__sz) + ((__r) - 1)) / (__r)) +#endif + +#ifndef MSM_MEDIA_MAX +#define MSM_MEDIA_MAX(__a, __b) ((__a) > (__b)?(__a):(__b)) +#endif + +enum color_fmts { + /* Venus NV12: + * YUV 4:2:0 image with a plane of 8 bit Y samples followed + * by an interleaved U/V plane containing 8 bit 2x2 subsampled + * colour difference samples. + * + * <-------- Y/UV_Stride --------> + * <------- Width -------> + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . ^ ^ + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . Height | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | Y_Scanlines + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . V | + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . V + * U V U V U V U V U V U V . . . . ^ + * U V U V U V U V U V U V . . . . | + * U V U V U V U V U V U V . . . . | + * U V U V U V U V U V U V . . . . UV_Scanlines + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . V + * . . . . . . . . . . . . . . . . --> Buffer size alignment + * + * Y_Stride : Width aligned to 128 + * UV_Stride : Width aligned to 128 + * Y_Scanlines: Height aligned to 32 + * UV_Scanlines: Height/2 aligned to 16 + * Extradata: Arbitrary (software-imposed) padding + * Total size = align((Y_Stride * Y_Scanlines + * + UV_Stride * UV_Scanlines + * + max(Extradata, Y_Stride * 8), 4096) + */ + COLOR_FMT_NV12, + + /* Venus NV21: + * YUV 4:2:0 image with a plane of 8 bit Y samples followed + * by an interleaved V/U plane containing 8 bit 2x2 subsampled + * colour difference samples. + * + * <-------- Y/UV_Stride --------> + * <------- Width -------> + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . ^ ^ + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . Height | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | Y_Scanlines + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . V | + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . V + * V U V U V U V U V U V U . . . . ^ + * V U V U V U V U V U V U . . . . | + * V U V U V U V U V U V U . . . . | + * V U V U V U V U V U V U . . . . UV_Scanlines + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . V + * . . . . . . . . . . . . . . . . --> Padding & Buffer size alignment + * + * Y_Stride : Width aligned to 128 + * UV_Stride : Width aligned to 128 + * Y_Scanlines: Height aligned to 32 + * UV_Scanlines: Height/2 aligned to 16 + * Extradata: Arbitrary (software-imposed) padding + * Total size = align((Y_Stride * Y_Scanlines + * + UV_Stride * UV_Scanlines + * + max(Extradata, Y_Stride * 8), 4096) + */ + COLOR_FMT_NV21, + /* Venus NV12_MVTB: + * Two YUV 4:2:0 images/views one after the other + * in a top-bottom layout, same as NV12 + * with a plane of 8 bit Y samples followed + * by an interleaved U/V plane containing 8 bit 2x2 subsampled + * colour difference samples. + * + * + * <-------- Y/UV_Stride --------> + * <------- Width -------> + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . ^ ^ ^ + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . Height | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | Y_Scanlines | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . V | | + * . . . . . . . . . . . . . . . . | View_1 + * . . . . . . . . . . . . . . . . | | + * . . . . . . . . . . . . . . . . | | + * . . . . . . . . . . . . . . . . V | + * U V U V U V U V U V U V . . . . ^ | + * U V U V U V U V U V U V . . . . | | + * U V U V U V U V U V U V . . . . | | + * U V U V U V U V U V U V . . . . UV_Scanlines | + * . . . . . . . . . . . . . . . . | | + * . . . . . . . . . . . . . . . . V V + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . ^ ^ ^ + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . Height | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | Y_Scanlines | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . | | | + * Y Y Y Y Y Y Y Y Y Y Y Y . . . . V | | + * . . . . . . . . . . . . . . . . | View_2 + * . . . . . . . . . . . . . . . . | | + * . . . . . . . . . . . . . . . . | | + * . . . . . . . . . . . . . . . . V | + * U V U V U V U V U V U V . . . . ^ | + * U V U V U V U V U V U V . . . . | | + * U V U V U V U V U V U V . . . . | | + * U V U V U V U V U V U V . . . . UV_Scanlines | + * . . . . . . . . . . . . . . . . | | + * . . . . . . . . . . . . . . . . V V + * . . . . . . . . . . . . . . . . --> Buffer size alignment + * + * Y_Stride : Width aligned to 128 + * UV_Stride : Width aligned to 128 + * Y_Scanlines: Height aligned to 32 + * UV_Scanlines: Height/2 aligned to 16 + * View_1 begin at: 0 (zero) + * View_2 begin at: Y_Stride * Y_Scanlines + UV_Stride * UV_Scanlines + * Extradata: Arbitrary (software-imposed) padding + * Total size = align((2*(Y_Stride * Y_Scanlines) + * + 2*(UV_Stride * UV_Scanlines) + Extradata), 4096) + */ + COLOR_FMT_NV12_MVTB, + /* Venus NV12 UBWC: + * Compressed Macro-tile format for NV12. + * Contains 4 planes in the following order - + * (A) Y_Meta_Plane + * (B) Y_UBWC_Plane + * (C) UV_Meta_Plane + * (D) UV_UBWC_Plane + * + * Y_Meta_Plane consists of meta information to decode compressed + * tile data in Y_UBWC_Plane. + * Y_UBWC_Plane consists of Y data in compressed macro-tile format. + * UBWC decoder block will use the Y_Meta_Plane data together with + * Y_UBWC_Plane data to produce loss-less uncompressed 8 bit Y samples. + * + * UV_Meta_Plane consists of meta information to decode compressed + * tile data in UV_UBWC_Plane. + * UV_UBWC_Plane consists of UV data in compressed macro-tile format. + * UBWC decoder block will use UV_Meta_Plane data together with + * UV_UBWC_Plane data to produce loss-less uncompressed 8 bit 2x2 + * subsampled color difference samples. + * + * Each tile in Y_UBWC_Plane/UV_UBWC_Plane is independently decodable + * and randomly accessible. There is no dependency between tiles. + * + * <----- Y_Meta_Stride ----> + * <-------- Width ------> + * M M M M M M M M M M M M . . ^ ^ + * M M M M M M M M M M M M . . | | + * M M M M M M M M M M M M . . Height | + * M M M M M M M M M M M M . . | Meta_Y_Scanlines + * M M M M M M M M M M M M . . | | + * M M M M M M M M M M M M . . | | + * M M M M M M M M M M M M . . | | + * M M M M M M M M M M M M . . V | + * . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . -------> Buffer size aligned to 4k + * . . . . . . . . . . . . . . V + * <--Compressed tile Y Stride---> + * <------- Width -------> + * Y* Y* Y* Y* Y* Y* Y* Y* . . . . ^ ^ + * Y* Y* Y* Y* Y* Y* Y* Y* . . . . | | + * Y* Y* Y* Y* Y* Y* Y* Y* . . . . Height | + * Y* Y* Y* Y* Y* Y* Y* Y* . . . . | Macro_tile_Y_Scanlines + * Y* Y* Y* Y* Y* Y* Y* Y* . . . . | | + * Y* Y* Y* Y* Y* Y* Y* Y* . . . . | | + * Y* Y* Y* Y* Y* Y* Y* Y* . . . . | | + * Y* Y* Y* Y* Y* Y* Y* Y* . . . . V | + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . -------> Buffer size aligned to 4k + * . . . . . . . . . . . . . . . . V + * <----- UV_Meta_Stride ----> + * M M M M M M M M M M M M . . ^ + * M M M M M M M M M M M M . . | + * M M M M M M M M M M M M . . | + * M M M M M M M M M M M M . . M_UV_Scanlines + * . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . V + * . . . . . . . . . . . . . . -------> Buffer size aligned to 4k + * <--Compressed tile UV Stride---> + * U* V* U* V* U* V* U* V* . . . . ^ + * U* V* U* V* U* V* U* V* . . . . | + * U* V* U* V* U* V* U* V* . . . . | + * U* V* U* V* U* V* U* V* . . . . UV_Scanlines + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . V + * . . . . . . . . . . . . . . . . -------> Buffer size aligned to 4k + * + * Y_Stride = align(Width, 128) + * UV_Stride = align(Width, 128) + * Y_Scanlines = align(Height, 32) + * UV_Scanlines = align(Height/2, 16) + * Y_UBWC_Plane_size = align(Y_Stride * Y_Scanlines, 4096) + * UV_UBWC_Plane_size = align(UV_Stride * UV_Scanlines, 4096) + * Y_Meta_Stride = align(roundup(Width, Y_TileWidth), 64) + * Y_Meta_Scanlines = align(roundup(Height, Y_TileHeight), 16) + * Y_Meta_Plane_size = align(Y_Meta_Stride * Y_Meta_Scanlines, 4096) + * UV_Meta_Stride = align(roundup(Width, UV_TileWidth), 64) + * UV_Meta_Scanlines = align(roundup(Height, UV_TileHeight), 16) + * UV_Meta_Plane_size = align(UV_Meta_Stride * UV_Meta_Scanlines, 4096) + * Extradata = 8k + * + * Total size = align( Y_UBWC_Plane_size + UV_UBWC_Plane_size + + * Y_Meta_Plane_size + UV_Meta_Plane_size + * + max(Extradata, Y_Stride * 48), 4096) + */ + COLOR_FMT_NV12_UBWC, + /* Venus NV12 10-bit UBWC: + * Compressed Macro-tile format for NV12. + * Contains 4 planes in the following order - + * (A) Y_Meta_Plane + * (B) Y_UBWC_Plane + * (C) UV_Meta_Plane + * (D) UV_UBWC_Plane + * + * Y_Meta_Plane consists of meta information to decode compressed + * tile data in Y_UBWC_Plane. + * Y_UBWC_Plane consists of Y data in compressed macro-tile format. + * UBWC decoder block will use the Y_Meta_Plane data together with + * Y_UBWC_Plane data to produce loss-less uncompressed 10 bit Y samples. + * + * UV_Meta_Plane consists of meta information to decode compressed + * tile data in UV_UBWC_Plane. + * UV_UBWC_Plane consists of UV data in compressed macro-tile format. + * UBWC decoder block will use UV_Meta_Plane data together with + * UV_UBWC_Plane data to produce loss-less uncompressed 10 bit 2x2 + * subsampled color difference samples. + * + * Each tile in Y_UBWC_Plane/UV_UBWC_Plane is independently decodable + * and randomly accessible. There is no dependency between tiles. + * + * <----- Y_Meta_Stride -----> + * <-------- Width ------> + * M M M M M M M M M M M M . . ^ ^ + * M M M M M M M M M M M M . . | | + * M M M M M M M M M M M M . . Height | + * M M M M M M M M M M M M . . | Meta_Y_Scanlines + * M M M M M M M M M M M M . . | | + * M M M M M M M M M M M M . . | | + * M M M M M M M M M M M M . . | | + * M M M M M M M M M M M M . . V | + * . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . -------> Buffer size aligned to 4k + * . . . . . . . . . . . . . . V + * <--Compressed tile Y Stride---> + * <------- Width -------> + * Y* Y* Y* Y* Y* Y* Y* Y* . . . . ^ ^ + * Y* Y* Y* Y* Y* Y* Y* Y* . . . . | | + * Y* Y* Y* Y* Y* Y* Y* Y* . . . . Height | + * Y* Y* Y* Y* Y* Y* Y* Y* . . . . | Macro_tile_Y_Scanlines + * Y* Y* Y* Y* Y* Y* Y* Y* . . . . | | + * Y* Y* Y* Y* Y* Y* Y* Y* . . . . | | + * Y* Y* Y* Y* Y* Y* Y* Y* . . . . | | + * Y* Y* Y* Y* Y* Y* Y* Y* . . . . V | + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . -------> Buffer size aligned to 4k + * . . . . . . . . . . . . . . . . V + * <----- UV_Meta_Stride ----> + * M M M M M M M M M M M M . . ^ + * M M M M M M M M M M M M . . | + * M M M M M M M M M M M M . . | + * M M M M M M M M M M M M . . M_UV_Scanlines + * . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . V + * . . . . . . . . . . . . . . -------> Buffer size aligned to 4k + * <--Compressed tile UV Stride---> + * U* V* U* V* U* V* U* V* . . . . ^ + * U* V* U* V* U* V* U* V* . . . . | + * U* V* U* V* U* V* U* V* . . . . | + * U* V* U* V* U* V* U* V* . . . . UV_Scanlines + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . V + * . . . . . . . . . . . . . . . . -------> Buffer size aligned to 4k + * + * + * Y_Stride = align(Width * 4/3, 128) + * UV_Stride = align(Width * 4/3, 128) + * Y_Scanlines = align(Height, 32) + * UV_Scanlines = align(Height/2, 16) + * Y_UBWC_Plane_Size = align(Y_Stride * Y_Scanlines, 4096) + * UV_UBWC_Plane_Size = align(UV_Stride * UV_Scanlines, 4096) + * Y_Meta_Stride = align(roundup(Width, Y_TileWidth), 64) + * Y_Meta_Scanlines = align(roundup(Height, Y_TileHeight), 16) + * Y_Meta_Plane_size = align(Y_Meta_Stride * Y_Meta_Scanlines, 4096) + * UV_Meta_Stride = align(roundup(Width, UV_TileWidth), 64) + * UV_Meta_Scanlines = align(roundup(Height, UV_TileHeight), 16) + * UV_Meta_Plane_size = align(UV_Meta_Stride * UV_Meta_Scanlines, 4096) + * Extradata = 8k + * + * Total size = align(Y_UBWC_Plane_size + UV_UBWC_Plane_size + + * Y_Meta_Plane_size + UV_Meta_Plane_size + * + max(Extradata, Y_Stride * 48), 4096) + */ + COLOR_FMT_NV12_BPP10_UBWC, + /* Venus RGBA8888 format: + * Contains 1 plane in the following order - + * (A) RGBA plane + * + * <-------- RGB_Stride --------> + * <------- Width -------> + * R R R R R R R R R R R R . . . . ^ ^ + * R R R R R R R R R R R R . . . . | | + * R R R R R R R R R R R R . . . . Height | + * R R R R R R R R R R R R . . . . | RGB_Scanlines + * R R R R R R R R R R R R . . . . | | + * R R R R R R R R R R R R . . . . | | + * R R R R R R R R R R R R . . . . | | + * R R R R R R R R R R R R . . . . V | + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . V + * + * RGB_Stride = align(Width * 4, 128) + * RGB_Scanlines = align(Height, 32) + * RGB_Plane_size = align(RGB_Stride * RGB_Scanlines, 4096) + * Extradata = 8k + * + * Total size = align(RGB_Plane_size + Extradata, 4096) + */ + COLOR_FMT_RGBA8888, + /* Venus RGBA8888 UBWC format: + * Contains 2 planes in the following order - + * (A) Meta plane + * (B) RGBA plane + * + * <--- RGB_Meta_Stride ----> + * <-------- Width ------> + * M M M M M M M M M M M M . . ^ ^ + * M M M M M M M M M M M M . . | | + * M M M M M M M M M M M M . . Height | + * M M M M M M M M M M M M . . | Meta_RGB_Scanlines + * M M M M M M M M M M M M . . | | + * M M M M M M M M M M M M . . | | + * M M M M M M M M M M M M . . | | + * M M M M M M M M M M M M . . V | + * . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . -------> Buffer size aligned to 4k + * . . . . . . . . . . . . . . V + * <-------- RGB_Stride --------> + * <------- Width -------> + * R R R R R R R R R R R R . . . . ^ ^ + * R R R R R R R R R R R R . . . . | | + * R R R R R R R R R R R R . . . . Height | + * R R R R R R R R R R R R . . . . | RGB_Scanlines + * R R R R R R R R R R R R . . . . | | + * R R R R R R R R R R R R . . . . | | + * R R R R R R R R R R R R . . . . | | + * R R R R R R R R R R R R . . . . V | + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . | + * . . . . . . . . . . . . . . . . -------> Buffer size aligned to 4k + * . . . . . . . . . . . . . . . . V + * + * RGB_Stride = align(Width * 4, 128) + * RGB_Scanlines = align(Height, 32) + * RGB_Plane_size = align(RGB_Stride * RGB_Scanlines, 4096) + * RGB_Meta_Stride = align(roundup(Width, RGB_TileWidth), 64) + * RGB_Meta_Scanline = align(roundup(Height, RGB_TileHeight), 16) + * RGB_Meta_Plane_size = align(RGB_Meta_Stride * + * RGB_Meta_Scanlines, 4096) + * Extradata = 8k + * + * Total size = align(RGB_Meta_Plane_size + RGB_Plane_size + + * Extradata, 4096) + */ + COLOR_FMT_RGBA8888_UBWC, +}; + +static inline unsigned int VENUS_EXTRADATA_SIZE(int width, int height) +{ + (void)height; + (void)width; + + /* + * In the future, calculate the size based on the w/h but just + * hardcode it for now since 16K satisfies all current usecases. + */ + return 16 * 1024; +} + +static inline unsigned int VENUS_Y_STRIDE(int color_fmt, int width) +{ + unsigned int alignment, stride = 0; + if (!width) + goto invalid_input; + + switch (color_fmt) { + case COLOR_FMT_NV21: + case COLOR_FMT_NV12: + case COLOR_FMT_NV12_MVTB: + case COLOR_FMT_NV12_UBWC: + alignment = 128; + stride = MSM_MEDIA_ALIGN(width, alignment); + break; + case COLOR_FMT_NV12_BPP10_UBWC: + alignment = 256; + stride = MSM_MEDIA_ALIGN(width, 192); + stride = MSM_MEDIA_ALIGN(stride * 4/3, alignment); + break; + default: + break; + } +invalid_input: + return stride; +} + +static inline unsigned int VENUS_UV_STRIDE(int color_fmt, int width) +{ + unsigned int alignment, stride = 0; + if (!width) + goto invalid_input; + + switch (color_fmt) { + case COLOR_FMT_NV21: + case COLOR_FMT_NV12: + case COLOR_FMT_NV12_MVTB: + case COLOR_FMT_NV12_UBWC: + alignment = 128; + stride = MSM_MEDIA_ALIGN(width, alignment); + break; + case COLOR_FMT_NV12_BPP10_UBWC: + alignment = 256; + stride = MSM_MEDIA_ALIGN(width, 192); + stride = MSM_MEDIA_ALIGN(stride * 4/3, alignment); + break; + default: + break; + } +invalid_input: + return stride; +} + +static inline unsigned int VENUS_Y_SCANLINES(int color_fmt, int height) +{ + unsigned int alignment, sclines = 0; + if (!height) + goto invalid_input; + + switch (color_fmt) { + case COLOR_FMT_NV21: + case COLOR_FMT_NV12: + case COLOR_FMT_NV12_MVTB: + case COLOR_FMT_NV12_UBWC: + alignment = 32; + break; + case COLOR_FMT_NV12_BPP10_UBWC: + alignment = 16; + break; + default: + return 0; + } + sclines = MSM_MEDIA_ALIGN(height, alignment); +invalid_input: + return sclines; +} + +static inline unsigned int VENUS_UV_SCANLINES(int color_fmt, int height) +{ + unsigned int alignment, sclines = 0; + if (!height) + goto invalid_input; + + switch (color_fmt) { + case COLOR_FMT_NV21: + case COLOR_FMT_NV12: + case COLOR_FMT_NV12_MVTB: + case COLOR_FMT_NV12_BPP10_UBWC: + alignment = 16; + break; + case COLOR_FMT_NV12_UBWC: + alignment = 32; + break; + default: + goto invalid_input; + } + + sclines = MSM_MEDIA_ALIGN(height / 2, alignment); + +invalid_input: + return sclines; +} + +static inline unsigned int VENUS_Y_META_STRIDE(int color_fmt, int width) +{ + int y_tile_width = 0, y_meta_stride = 0; + + if (!width) + goto invalid_input; + + switch (color_fmt) { + case COLOR_FMT_NV12_UBWC: + y_tile_width = 32; + break; + case COLOR_FMT_NV12_BPP10_UBWC: + y_tile_width = 48; + break; + default: + goto invalid_input; + } + + y_meta_stride = MSM_MEDIA_ROUNDUP(width, y_tile_width); + y_meta_stride = MSM_MEDIA_ALIGN(y_meta_stride, 64); + +invalid_input: + return y_meta_stride; +} + +static inline unsigned int VENUS_Y_META_SCANLINES(int color_fmt, int height) +{ + int y_tile_height = 0, y_meta_scanlines = 0; + + if (!height) + goto invalid_input; + + switch (color_fmt) { + case COLOR_FMT_NV12_UBWC: + y_tile_height = 8; + break; + case COLOR_FMT_NV12_BPP10_UBWC: + y_tile_height = 4; + break; + default: + goto invalid_input; + } + + y_meta_scanlines = MSM_MEDIA_ROUNDUP(height, y_tile_height); + y_meta_scanlines = MSM_MEDIA_ALIGN(y_meta_scanlines, 16); + +invalid_input: + return y_meta_scanlines; +} + +static inline unsigned int VENUS_UV_META_STRIDE(int color_fmt, int width) +{ + int uv_tile_width = 0, uv_meta_stride = 0; + + if (!width) + goto invalid_input; + + switch (color_fmt) { + case COLOR_FMT_NV12_UBWC: + uv_tile_width = 16; + break; + case COLOR_FMT_NV12_BPP10_UBWC: + uv_tile_width = 24; + break; + default: + goto invalid_input; + } + + uv_meta_stride = MSM_MEDIA_ROUNDUP(width / 2, uv_tile_width); + uv_meta_stride = MSM_MEDIA_ALIGN(uv_meta_stride, 64); + +invalid_input: + return uv_meta_stride; +} + +static inline unsigned int VENUS_UV_META_SCANLINES(int color_fmt, int height) +{ + int uv_tile_height = 0, uv_meta_scanlines = 0; + + if (!height) + goto invalid_input; + + switch (color_fmt) { + case COLOR_FMT_NV12_UBWC: + uv_tile_height = 8; + break; + case COLOR_FMT_NV12_BPP10_UBWC: + uv_tile_height = 4; + break; + default: + goto invalid_input; + } + + uv_meta_scanlines = MSM_MEDIA_ROUNDUP(height / 2, uv_tile_height); + uv_meta_scanlines = MSM_MEDIA_ALIGN(uv_meta_scanlines, 16); + +invalid_input: + return uv_meta_scanlines; +} + +static inline unsigned int VENUS_RGB_STRIDE(int color_fmt, int width) +{ + unsigned int alignment = 0, stride = 0; + if (!width) + goto invalid_input; + + switch (color_fmt) { + case COLOR_FMT_RGBA8888: + alignment = 128; + break; + case COLOR_FMT_RGBA8888_UBWC: + alignment = 256; + break; + default: + goto invalid_input; + } + + stride = MSM_MEDIA_ALIGN(width * 4, alignment); + +invalid_input: + return stride; +} + +static inline unsigned int VENUS_RGB_SCANLINES(int color_fmt, int height) +{ + unsigned int alignment = 0, scanlines = 0; + + if (!height) + goto invalid_input; + + switch (color_fmt) { + case COLOR_FMT_RGBA8888: + alignment = 32; + break; + case COLOR_FMT_RGBA8888_UBWC: + alignment = 16; + break; + default: + goto invalid_input; + } + + scanlines = MSM_MEDIA_ALIGN(height, alignment); + +invalid_input: + return scanlines; +} + +static inline unsigned int VENUS_RGB_META_STRIDE(int color_fmt, int width) +{ + int rgb_tile_width = 0, rgb_meta_stride = 0; + + if (!width) + goto invalid_input; + + switch (color_fmt) { + case COLOR_FMT_RGBA8888_UBWC: + rgb_tile_width = 16; + break; + default: + goto invalid_input; + } + + rgb_meta_stride = MSM_MEDIA_ROUNDUP(width, rgb_tile_width); + rgb_meta_stride = MSM_MEDIA_ALIGN(rgb_meta_stride, 64); + +invalid_input: + return rgb_meta_stride; +} + +static inline unsigned int VENUS_RGB_META_SCANLINES(int color_fmt, int height) +{ + int rgb_tile_height = 0, rgb_meta_scanlines = 0; + + if (!height) + goto invalid_input; + + switch (color_fmt) { + case COLOR_FMT_RGBA8888_UBWC: + rgb_tile_height = 4; + break; + default: + goto invalid_input; + } + + rgb_meta_scanlines = MSM_MEDIA_ROUNDUP(height, rgb_tile_height); + rgb_meta_scanlines = MSM_MEDIA_ALIGN(rgb_meta_scanlines, 16); + +invalid_input: + return rgb_meta_scanlines; +} + +static inline unsigned int VENUS_BUFFER_SIZE( + int color_fmt, int width, int height) +{ + const unsigned int extra_size = VENUS_EXTRADATA_SIZE(width, height); + unsigned int uv_alignment = 0, size = 0; + unsigned int y_plane, uv_plane, y_stride, + uv_stride, y_sclines, uv_sclines; + unsigned int y_ubwc_plane = 0, uv_ubwc_plane = 0; + unsigned int y_meta_stride = 0, y_meta_scanlines = 0; + unsigned int uv_meta_stride = 0, uv_meta_scanlines = 0; + unsigned int y_meta_plane = 0, uv_meta_plane = 0; + unsigned int rgb_stride = 0, rgb_scanlines = 0; + unsigned int rgb_plane = 0, rgb_ubwc_plane = 0, rgb_meta_plane = 0; + unsigned int rgb_meta_stride = 0, rgb_meta_scanlines = 0; + + if (!width || !height) + goto invalid_input; + + y_stride = VENUS_Y_STRIDE(color_fmt, width); + uv_stride = VENUS_UV_STRIDE(color_fmt, width); + y_sclines = VENUS_Y_SCANLINES(color_fmt, height); + uv_sclines = VENUS_UV_SCANLINES(color_fmt, height); + rgb_stride = VENUS_RGB_STRIDE(color_fmt, width); + rgb_scanlines = VENUS_RGB_SCANLINES(color_fmt, height); + + switch (color_fmt) { + case COLOR_FMT_NV21: + case COLOR_FMT_NV12: + uv_alignment = 4096; + y_plane = y_stride * y_sclines; + uv_plane = uv_stride * uv_sclines + uv_alignment; + size = y_plane + uv_plane + + MSM_MEDIA_MAX(extra_size, 8 * y_stride); + size = MSM_MEDIA_ALIGN(size, 4096); + break; + case COLOR_FMT_NV12_MVTB: + uv_alignment = 4096; + y_plane = y_stride * y_sclines; + uv_plane = uv_stride * uv_sclines + uv_alignment; + size = y_plane + uv_plane; + size = 2 * size + extra_size; + size = MSM_MEDIA_ALIGN(size, 4096); + break; + case COLOR_FMT_NV12_UBWC: + case COLOR_FMT_NV12_BPP10_UBWC: + y_ubwc_plane = MSM_MEDIA_ALIGN(y_stride * y_sclines, 4096); + uv_ubwc_plane = MSM_MEDIA_ALIGN(uv_stride * uv_sclines, 4096); + y_meta_stride = VENUS_Y_META_STRIDE(color_fmt, width); + y_meta_scanlines = VENUS_Y_META_SCANLINES(color_fmt, height); + y_meta_plane = MSM_MEDIA_ALIGN( + y_meta_stride * y_meta_scanlines, 4096); + uv_meta_stride = VENUS_UV_META_STRIDE(color_fmt, width); + uv_meta_scanlines = VENUS_UV_META_SCANLINES(color_fmt, height); + uv_meta_plane = MSM_MEDIA_ALIGN(uv_meta_stride * + uv_meta_scanlines, 4096); + + size = y_ubwc_plane + uv_ubwc_plane + y_meta_plane + + uv_meta_plane + + MSM_MEDIA_MAX(extra_size + 8192, 48 * y_stride); + size = MSM_MEDIA_ALIGN(size, 4096); + break; + case COLOR_FMT_RGBA8888: + rgb_plane = MSM_MEDIA_ALIGN(rgb_stride * rgb_scanlines, 4096); + size = rgb_plane; + size = MSM_MEDIA_ALIGN(size, 4096); + break; + case COLOR_FMT_RGBA8888_UBWC: + rgb_ubwc_plane = MSM_MEDIA_ALIGN(rgb_stride * rgb_scanlines, + 4096); + rgb_meta_stride = VENUS_RGB_META_STRIDE(color_fmt, width); + rgb_meta_scanlines = VENUS_RGB_META_SCANLINES(color_fmt, + height); + rgb_meta_plane = MSM_MEDIA_ALIGN(rgb_meta_stride * + rgb_meta_scanlines, 4096); + size = rgb_ubwc_plane + rgb_meta_plane; + size = MSM_MEDIA_ALIGN(size, 4096); + break; + default: + break; + } +invalid_input: + return size; +} + +static inline unsigned int VENUS_VIEW2_OFFSET( + int color_fmt, int width, int height) +{ + unsigned int offset = 0; + unsigned int y_plane, uv_plane, y_stride, + uv_stride, y_sclines, uv_sclines; + if (!width || !height) + goto invalid_input; + + y_stride = VENUS_Y_STRIDE(color_fmt, width); + uv_stride = VENUS_UV_STRIDE(color_fmt, width); + y_sclines = VENUS_Y_SCANLINES(color_fmt, height); + uv_sclines = VENUS_UV_SCANLINES(color_fmt, height); + switch (color_fmt) { + case COLOR_FMT_NV12_MVTB: + y_plane = y_stride * y_sclines; + uv_plane = uv_stride * uv_sclines; + offset = y_plane + uv_plane; + break; + default: + break; + } +invalid_input: + return offset; +} + +#endif diff --git a/selfdrive/loggerd/logger.c b/selfdrive/loggerd/logger.c new file mode 100644 index 000000000..7dfb610ba --- /dev/null +++ b/selfdrive/loggerd/logger.c @@ -0,0 +1,217 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "common/swaglog.h" + +#include "logger.h" + +static int mkpath(char* file_path) { + assert(file_path && *file_path); + char* p; + for (p=strchr(file_path+1, '/'); p; p=strchr(p+1, '/')) { + *p = '\0'; + if (mkdir(file_path, 0777)==-1) { + if (errno != EEXIST) { + *p = '/'; + return -1; + } + } + *p = '/'; + } + return 0; +} + +void logger_init(LoggerState *s, const char* log_name, const uint8_t* init_data, size_t init_data_len, bool has_qlog) { + memset(s, 0, sizeof(*s)); + if (init_data) { + s->init_data = (uint8_t*)malloc(init_data_len); + assert(s->init_data); + memcpy(s->init_data, init_data, init_data_len); + s->init_data_len = init_data_len; + } + + umask(0); + + pthread_mutex_init(&s->lock, NULL); + + s->part = -1; + s->has_qlog = has_qlog; + + time_t rawtime = time(NULL); + struct tm timeinfo; + localtime_r(&rawtime, &timeinfo); + + strftime(s->route_name, sizeof(s->route_name), + "%Y-%m-%d--%H-%M-%S", &timeinfo); + snprintf(s->log_name, sizeof(s->log_name), "%s", log_name); +} + +static LoggerHandle* logger_open(LoggerState *s, const char* root_path) { + int err; + + LoggerHandle *h = NULL; + for (int i=0; ihandles[i].refcnt == 0) { + h = &s->handles[i]; + break; + } + } + assert(h); + + snprintf(h->segment_path, sizeof(h->segment_path), + "%s/%s--%d", root_path, s->route_name, s->part); + + snprintf(h->log_path, sizeof(h->log_path), "%s/%s.bz2", h->segment_path, s->log_name); + snprintf(h->qlog_path, sizeof(h->qlog_path), "%s/qlog.bz2", h->segment_path); + snprintf(h->lock_path, sizeof(h->lock_path), "%s.lock", h->log_path); + + err = mkpath(h->log_path); + if (err) return NULL; + + FILE* lock_file = fopen(h->lock_path, "wb"); + if (lock_file == NULL) return NULL; + fclose(lock_file); + + h->log_file = fopen(h->log_path, "wb"); + if (h->log_file == NULL) goto fail; + + if (s->has_qlog) { + h->qlog_file = fopen(h->qlog_path, "wb"); + if (h->qlog_file == NULL) goto fail; + } + + int bzerror; + h->bz_file = BZ2_bzWriteOpen(&bzerror, h->log_file, 9, 0, 30); + if (bzerror != BZ_OK) goto fail; + + if (s->has_qlog) { + h->bz_qlog = BZ2_bzWriteOpen(&bzerror, h->qlog_file, 9, 0, 30); + if (bzerror != BZ_OK) goto fail; + } + + if (s->init_data) { + BZ2_bzWrite(&bzerror, h->bz_file, s->init_data, s->init_data_len); + if (bzerror != BZ_OK) goto fail; + + if (s->has_qlog) { + // init data goes in the qlog too + BZ2_bzWrite(&bzerror, h->bz_qlog, s->init_data, s->init_data_len); + if (bzerror != BZ_OK) goto fail; + } + } + + pthread_mutex_init(&h->lock, NULL); + h->refcnt++; + return h; +fail: + LOGE("logger failed to open files"); + if (h->qlog_file) fclose(h->qlog_file); + if (h->log_file) fclose(h->log_file); + return NULL; +} + +int logger_next(LoggerState *s, const char* root_path, + char* out_segment_path, size_t out_segment_path_len, + int* out_part) { + pthread_mutex_lock(&s->lock); + s->part++; + + LoggerHandle* next_h = logger_open(s, root_path); + if (!next_h) { + pthread_mutex_unlock(&s->lock); + return -1; + } + + if (s->cur_handle) { + lh_close(s->cur_handle); + } + s->cur_handle = next_h; + + if (out_segment_path) { + snprintf(out_segment_path, out_segment_path_len, "%s", next_h->segment_path); + } + if (out_part) { + *out_part = s->part; + } + + pthread_mutex_unlock(&s->lock); + return 0; +} + +LoggerHandle* logger_get_handle(LoggerState *s) { + pthread_mutex_lock(&s->lock); + LoggerHandle* h = s->cur_handle; + if (h) { + pthread_mutex_lock(&h->lock); + h->refcnt++; + pthread_mutex_unlock(&h->lock); + } + pthread_mutex_unlock(&s->lock); + return h; +} + +void logger_log(LoggerState *s, uint8_t* data, size_t data_size, bool in_qlog) { + pthread_mutex_lock(&s->lock); + if (s->cur_handle) { + lh_log(s->cur_handle, data, data_size, in_qlog); + } + pthread_mutex_unlock(&s->lock); +} + +void logger_close(LoggerState *s) { + pthread_mutex_lock(&s->lock); + free(s->init_data); + if (s->cur_handle) { + lh_close(s->cur_handle); + } + pthread_mutex_unlock(&s->lock); +} + +void lh_log(LoggerHandle* h, uint8_t* data, size_t data_size, bool in_qlog) { + pthread_mutex_lock(&h->lock); + assert(h->refcnt > 0); + int bzerror; + BZ2_bzWrite(&bzerror, h->bz_file, data, data_size); + + if (in_qlog && h->bz_qlog != NULL) { + BZ2_bzWrite(&bzerror, h->bz_qlog, data, data_size); + } + pthread_mutex_unlock(&h->lock); +} + +void lh_close(LoggerHandle* h) { + pthread_mutex_lock(&h->lock); + assert(h->refcnt > 0); + h->refcnt--; + if (h->refcnt == 0) { + if (h->bz_file){ + int bzerror; + BZ2_bzWriteClose(&bzerror, h->bz_file, 0, NULL, NULL); + h->bz_file = NULL; + } + if (h->bz_qlog){ + int bzerror; + BZ2_bzWriteClose(&bzerror, h->bz_qlog, 0, NULL, NULL); + h->bz_qlog = NULL; + } + if (h->qlog_file) fclose(h->qlog_file); + fclose(h->log_file); + unlink(h->lock_path); + pthread_mutex_unlock(&h->lock); + pthread_mutex_destroy(&h->lock); + return; + } + pthread_mutex_unlock(&h->lock); +} diff --git a/selfdrive/loggerd/logger.h b/selfdrive/loggerd/logger.h new file mode 100644 index 000000000..ab8798481 --- /dev/null +++ b/selfdrive/loggerd/logger.h @@ -0,0 +1,59 @@ +#ifndef LOGGER_H +#define LOGGER_H + +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define LOGGER_MAX_HANDLES 16 + +typedef struct LoggerHandle { + pthread_mutex_t lock; + int refcnt; + char segment_path[4096]; + char log_path[4096]; + char lock_path[4096]; + FILE* log_file; + BZFILE* bz_file; + + FILE* qlog_file; + char qlog_path[4096]; + BZFILE* bz_qlog; +} LoggerHandle; + +typedef struct LoggerState { + pthread_mutex_t lock; + + uint8_t* init_data; + size_t init_data_len; + + int part; + char route_name[64]; + char log_name[64]; + bool has_qlog; + + LoggerHandle handles[LOGGER_MAX_HANDLES]; + LoggerHandle* cur_handle; +} LoggerState; + +void logger_init(LoggerState *s, const char* log_name, const uint8_t* init_data, size_t init_data_len, bool has_qlog); +int logger_next(LoggerState *s, const char* root_path, + char* out_segment_path, size_t out_segment_path_len, + int* out_part); +LoggerHandle* logger_get_handle(LoggerState *s); +void logger_close(LoggerState *s); +void logger_log(LoggerState *s, uint8_t* data, size_t data_size, bool in_qlog); + +void lh_log(LoggerHandle* h, uint8_t* data, size_t data_size, bool in_qlog); +void lh_close(LoggerHandle* h); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc new file mode 100644 index 000000000..1b3a81d7a --- /dev/null +++ b/selfdrive/loggerd/loggerd.cc @@ -0,0 +1,738 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#ifdef QCOM +#include +#endif + +#include "common/version.h" +#include "common/timing.h" +#include "common/params.h" +#include "common/swaglog.h" +#include "common/visionipc.h" +#include "common/utilpp.h" +#include "common/util.h" + +#include "logger.h" +#include "messaging.hpp" + +#ifndef QCOM +// no encoder on PC +#define DISABLE_ENCODER +#endif + + +#ifndef DISABLE_ENCODER +#include "encoder.h" +#include "raw_logger.h" +#endif + +#include "cereal/gen/cpp/log.capnp.h" + +#define CAMERA_FPS 20 +#define SEGMENT_LENGTH 60 +#define LOG_ROOT "/data/media/0/realdata" +#define ENABLE_LIDAR 0 + +#define RAW_CLIP_LENGTH 100 // 5 seconds at 20fps +#define RAW_CLIP_FREQUENCY (randrange(61, 8*60)) // once every ~4 minutes + +namespace { + +double randrange(double a, double b) { + static std::mt19937 gen(millis_since_boot()); + + std::uniform_real_distribution<> dist(a, b); + return dist(gen); +} + + +volatile sig_atomic_t do_exit = 0; +static void set_do_exit(int sig) { + do_exit = 1; +} +struct LoggerdState { + Context *ctx; + LoggerState logger; + + std::mutex lock; + std::condition_variable cv; + char segment_path[4096]; + uint32_t last_frame_id; + uint32_t rotate_last_frame_id; + int rotate_segment; +}; +LoggerdState s; + +#ifndef DISABLE_ENCODER +void encoder_thread(bool is_streaming, bool raw_clips, bool front) { + int err; + + if (front) { + char *value; + const int result = read_db_value(NULL, "RecordFront", &value, NULL); + if (result != 0) return; + if (value[0] != '1') { free(value); return; } + free(value); + LOGW("recording front camera"); + + set_thread_name("FrontCameraEncoder"); + } else { + set_thread_name("RearCameraEncoder"); + } + + VisionStream stream; + + bool encoder_inited = false; + EncoderState encoder; + EncoderState encoder_alt; + bool has_encoder_alt = false; + + int encoder_segment = -1; + int cnt = 0; + + PubSocket *idx_sock = PubSocket::create(s.ctx, front ? "frontEncodeIdx" : "encodeIdx"); + assert(idx_sock != NULL); + + LoggerHandle *lh = NULL; + + while (!do_exit) { + VisionStreamBufs buf_info; + if (front) { + err = visionstream_init(&stream, VISION_STREAM_YUV_FRONT, false, &buf_info); + } else { + err = visionstream_init(&stream, VISION_STREAM_YUV, false, &buf_info); + } + if (err != 0) { + LOGD("visionstream connect fail"); + usleep(100000); + continue; + } + + if (!encoder_inited) { + LOGD("encoder init %dx%d", buf_info.width, buf_info.height); + encoder_init(&encoder, front ? "dcamera.hevc" : "fcamera.hevc", buf_info.width, buf_info.height, CAMERA_FPS, front ? 2500000 : 5000000, true, false); + if (!front) { + encoder_init(&encoder_alt, "qcamera.ts", 480, 360, CAMERA_FPS, 128000, false, true); + has_encoder_alt = true; + } + encoder_inited = true; + if (is_streaming) { + encoder.zmq_ctx = zmq_ctx_new(); + encoder.stream_sock_raw = zmq_socket(encoder.zmq_ctx, ZMQ_PUB); + assert(encoder.stream_sock_raw); + zmq_bind(encoder.stream_sock_raw, "tcp://*:9002"); + } + } + + // dont log a raw clip in the first minute + double rawlogger_start_time = seconds_since_boot()+RAW_CLIP_FREQUENCY; + int rawlogger_clip_cnt = 0; + RawLogger *rawlogger = NULL; + + if (raw_clips) { + rawlogger = new RawLogger("prcamera", buf_info.width, buf_info.height, CAMERA_FPS); + } + + while (!do_exit) { + VIPCBufExtra extra; + VIPCBuf* buf = visionstream_get(&stream, &extra); + if (buf == NULL) { + LOG("visionstream get failed"); + break; + } + + uint64_t current_time = nanos_since_boot(); + uint64_t diff = current_time - extra.timestamp_eof; + double msdiff = (double) diff / 1000000.0; + // printf("logger latency to tsEof: %f\n", msdiff); + + uint8_t *y = (uint8_t*)buf->addr; + uint8_t *u = y + (buf_info.width*buf_info.height); + uint8_t *v = u + (buf_info.width/2)*(buf_info.height/2); + + { + bool should_rotate = false; + std::unique_lock lk(s.lock); + if (!front) { + // wait if log camera is older on back camera + while ( extra.frame_id > s.last_frame_id //if the log camera is older, wait for it to catch up. + && (extra.frame_id-s.last_frame_id) < 8 // but if its too old then there probably was a discontinuity (visiond restarted) + && !do_exit) { + s.cv.wait(lk); + } + should_rotate = extra.frame_id > s.rotate_last_frame_id && encoder_segment < s.rotate_segment; + } else { + // front camera is best effort + should_rotate = encoder_segment < s.rotate_segment; + } + if (do_exit) break; + + // rotate the encoder if the logger is on a newer segment + if (should_rotate) { + LOG("rotate encoder to %s", s.segment_path); + + encoder_rotate(&encoder, s.segment_path, s.rotate_segment); + if (has_encoder_alt) { + encoder_rotate(&encoder_alt, s.segment_path, s.rotate_segment); + } + + if (raw_clips) { + rawlogger->Rotate(s.segment_path, s.rotate_segment); + } + + encoder_segment = s.rotate_segment; + if (lh) { + lh_close(lh); + } + lh = logger_get_handle(&s.logger); + } + } + + { + // encode hevc + int out_segment = -1; + int out_id = encoder_encode_frame(&encoder, + y, u, v, + buf_info.width, buf_info.height, + &out_segment, &extra); + + if (has_encoder_alt) { + int out_segment_alt = -1; + encoder_encode_frame(&encoder_alt, + y, u, v, + buf_info.width, buf_info.height, + &out_segment_alt, &extra); + } + + // publish encode index + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto eidx = event.initEncodeIdx(); + eidx.setFrameId(extra.frame_id); + eidx.setType(front ? cereal::EncodeIndex::Type::FRONT : cereal::EncodeIndex::Type::FULL_H_E_V_C); + eidx.setEncodeId(cnt); + eidx.setSegmentNum(out_segment); + eidx.setSegmentId(out_id); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + if (idx_sock->send((char*)bytes.begin(), bytes.size()) < 0) { + printf("err sending encodeIdx pkt: %s\n", strerror(errno)); + } + if (lh) { + lh_log(lh, bytes.begin(), bytes.size(), false); + } + } + + if (raw_clips) { + double ts = seconds_since_boot(); + if (ts > rawlogger_start_time) { + // encode raw if in clip + int out_segment = -1; + int out_id = rawlogger->LogFrame(cnt, y, u, v, &out_segment); + + if (rawlogger_clip_cnt == 0) { + LOG("starting raw clip in seg %d", out_segment); + } + + // publish encode index + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto eidx = event.initEncodeIdx(); + eidx.setFrameId(extra.frame_id); + eidx.setType(cereal::EncodeIndex::Type::FULL_LOSSLESS_CLIP); + eidx.setEncodeId(cnt); + eidx.setSegmentNum(out_segment); + eidx.setSegmentId(out_id); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + if (lh) { + lh_log(lh, bytes.begin(), bytes.size(), false); + } + + // close rawlogger if clip ended + rawlogger_clip_cnt++; + if (rawlogger_clip_cnt >= RAW_CLIP_LENGTH) { + rawlogger->Close(); + + rawlogger_clip_cnt = 0; + rawlogger_start_time = ts+RAW_CLIP_FREQUENCY; + + LOG("ending raw clip in seg %d, next in %.1f sec", out_segment, rawlogger_start_time-ts); + } + } + } + + cnt++; + } + + if (lh) { + lh_close(lh); + lh = NULL; + } + + if (raw_clips) { + rawlogger->Close(); + delete rawlogger; + } + + visionstream_destroy(&stream); + } + + delete idx_sock; + + if (encoder_inited) { + LOG("encoder destroy"); + encoder_close(&encoder); + encoder_destroy(&encoder); + } + + if (has_encoder_alt) { + LOG("encoder alt destroy"); + encoder_close(&encoder_alt); + encoder_destroy(&encoder_alt); + } +} +#endif + +#if ENABLE_LIDAR + +#include +#include +#include +#include + +#define VELODYNE_DATA_PORT 2368 +#define VELODYNE_TELEMETRY_PORT 8308 + +#define MAX_LIDAR_PACKET 2048 + +int lidar_thread() { + // increase kernel max buffer size + system("sysctl -w net.core.rmem_max=26214400"); + set_thread_name("lidar"); + + int sock; + if ((sock = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + perror("cannot create socket"); + return -1; + } + + int a = 26214400; + if (setsockopt(sock, SOL_SOCKET, SO_RCVBUF, &a, sizeof(int)) == -1) { + perror("cannot set socket opts"); + return -1; + } + + struct sockaddr_in addr; + memset(&addr, 0, sizeof(struct sockaddr_in)); + addr.sin_family = AF_INET; + addr.sin_port = htons(VELODYNE_DATA_PORT); + inet_aton("192.168.5.11", &(addr.sin_addr)); + + if (bind(sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + perror("cannot bind LIDAR socket"); + return -1; + } + + capnp::byte buf[MAX_LIDAR_PACKET]; + + while (!do_exit) { + // receive message + struct sockaddr from; + socklen_t fromlen = sizeof(from); + int cnt = recvfrom(sock, (void *)buf, MAX_LIDAR_PACKET, 0, &from, &fromlen); + if (cnt <= 0) { + printf("bug in lidar recieve!\n"); + continue; + } + + // create message for log + capnp::MallocMessageBuilder msg; + auto event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto lidar_pts = event.initLidarPts(); + + // copy in the buffer + // TODO: can we remove this copy? does it matter? + kj::ArrayPtr bufferPtr = kj::arrayPtr(buf, cnt); + lidar_pts.setPkt(bufferPtr); + + // log it + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + logger_log(&s.logger, bytes.begin(), bytes.size()); + } + return 0; +} +#endif + +} + +void append_property(const char* key, const char* value, void *cookie) { + std::vector > *properties = + (std::vector > *)cookie; + + properties->push_back(std::make_pair(std::string(key), std::string(value))); +} + +kj::Array gen_init_data() { + capnp::MallocMessageBuilder msg; + auto event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto init = event.initInitData(); + + init.setDeviceType(cereal::InitData::DeviceType::NEO); + init.setVersion(capnp::Text::Reader(COMMA_VERSION)); + + std::ifstream cmdline_stream("/proc/cmdline"); + std::vector kernel_args; + std::string buf; + while (cmdline_stream >> buf) { + kernel_args.push_back(buf); + } + + auto lkernel_args = init.initKernelArgs(kernel_args.size()); + for (int i=0; i > properties; + property_list(append_property, (void*)&properties); + + auto lentries = init.initAndroidProperties().initEntries(properties.size()); + for (int i=0; i params; + read_db_all(NULL, ¶ms); + auto lparams = init.initParams().initEntries(params.size()); + int i = 0; + for (auto& kv : params) { + auto lentry = lparams[i]; + lentry.setKey(kv.first); + lentry.setValue(kv.second); + i++; + } + } + + + auto words = capnp::messageToFlatArray(msg); + + if (git_commit) { + free((void*)git_commit); + } + + if (git_branch) { + free((void*)git_branch); + } + + if (git_remote) { + free((void*)git_remote); + } + + if (passive) { + free((void*)passive); + } + + return words; +} + +static int clear_locks_fn(const char* fpath, const struct stat *sb, int tyupeflag) { + const char* dot = strrchr(fpath, '.'); + if (dot && strcmp(dot, ".lock") == 0) { + unlink(fpath); + } + return 0; +} + +static void clear_locks() { + ftw(LOG_ROOT, clear_locks_fn, 16); +} + +static void bootlog() { + int err; + + { + auto words = gen_init_data(); + auto bytes = words.asBytes(); + logger_init(&s.logger, "bootlog", bytes.begin(), bytes.size(), false); + } + + err = logger_next(&s.logger, LOG_ROOT, s.segment_path, sizeof(s.segment_path), &s.rotate_segment); + assert(err == 0); + LOGW("bootlog to %s", s.segment_path); + + { + capnp::MallocMessageBuilder msg; + auto event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto boot = event.initBoot(); + + boot.setWallTimeNanos(nanos_since_epoch()); + + std::string lastKmsg = util::read_file("/sys/fs/pstore/console-ramoops"); + boot.setLastKmsg(capnp::Data::Reader((const kj::byte*)lastKmsg.data(), lastKmsg.size())); + + std::string lastPmsg = util::read_file("/sys/fs/pstore/pmsg-ramoops-0"); + boot.setLastPmsg(capnp::Data::Reader((const kj::byte*)lastPmsg.data(), lastPmsg.size())); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + logger_log(&s.logger, bytes.begin(), bytes.size(), false); + } + + logger_close(&s.logger); +} + +int main(int argc, char** argv) { + int err; + + if (argc > 1 && strcmp(argv[1], "--bootlog") == 0) { + bootlog(); + return 0; + } + + setpriority(PRIO_PROCESS, 0, -12); + + clear_locks(); + + signal(SIGINT, (sighandler_t)set_do_exit); + signal(SIGTERM, (sighandler_t)set_do_exit); + + s.ctx = Context::create(); + Poller * poller = Poller::create(); + + std::string exe_dir = util::dir_name(util::readlink("/proc/self/exe")); + std::string service_list_path = exe_dir + "/../../cereal/service_list.yaml"; + + // subscribe to all services + + SubSocket *frame_sock = NULL; + std::vector socks; + + std::map qlog_counter; + std::map qlog_freqs; + + YAML::Node service_list = YAML::LoadFile(service_list_path); + for (const auto& it : service_list) { + auto name = it.first.as(); + bool should_log = it.second[1].as(); + int qlog_freq = it.second[3] ? it.second[3].as() : 0; + + if (should_log) { + SubSocket * sock = SubSocket::create(s.ctx, name); + assert(sock != NULL); + + poller->registerSocket(sock); + socks.push_back(sock); + + if (name == "frame") { + frame_sock = sock; + } + + qlog_counter[sock] = (qlog_freq == 0) ? -1 : 0; + qlog_freqs[sock] = qlog_freq; + } + } + + + { + auto words = gen_init_data(); + auto bytes = words.asBytes(); + logger_init(&s.logger, "rlog", bytes.begin(), bytes.size(), true); + } + + bool is_streaming = false; + bool is_logging = true; + + if (argc > 1 && strcmp(argv[1], "--stream") == 0) { + is_streaming = true; + } else if (argc > 1 && strcmp(argv[1], "--only-stream") == 0) { + is_streaming = true; + is_logging = false; + } + + if (is_logging) { + err = logger_next(&s.logger, LOG_ROOT, s.segment_path, sizeof(s.segment_path), &s.rotate_segment); + assert(err == 0); + LOGW("logging to %s", s.segment_path); + } + + double start_ts = seconds_since_boot(); + double last_rotate_ts = start_ts; + +#ifndef DISABLE_ENCODER + // rear camera + std::thread encoder_thread_handle(encoder_thread, is_streaming, false, false); + + // front camera + std::thread front_encoder_thread_handle(encoder_thread, false, false, true); +#endif + +#if ENABLE_LIDAR + std::thread lidar_thread_handle(lidar_thread); +#endif + + uint64_t msg_count = 0; + uint64_t bytes_count = 0; + + while (!do_exit) { + for (auto sock : poller->poll(100 * 1000)){ + while (true) { + Message * msg = sock->receive(true); + if (msg == NULL){ + break; + } + + uint8_t* data = (uint8_t*)msg->getData(); + size_t len = msg->getSize(); + + if (sock == frame_sock) { + // track camera frames to sync to encoder + auto amsg = kj::heapArray((len / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), data, len); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + if (event.isFrame()) { + std::unique_lock lk(s.lock); + s.last_frame_id = event.getFrame().getFrameId(); + lk.unlock(); + s.cv.notify_all(); + } + } + + logger_log(&s.logger, data, len, qlog_counter[sock] == 0); + delete msg; + + if (qlog_counter[sock] != -1) { + //printf("%p: %d/%d\n", socks[i], qlog_counter[socks[i]], qlog_freqs[socks[i]]); + qlog_counter[sock]++; + qlog_counter[sock] %= qlog_freqs[sock]; + } + + bytes_count += len; + msg_count++; + } + } + + double ts = seconds_since_boot(); + if (ts - last_rotate_ts > SEGMENT_LENGTH) { + // rotate the log + + last_rotate_ts += SEGMENT_LENGTH; + + std::lock_guard guard(s.lock); + s.rotate_last_frame_id = s.last_frame_id; + + if (is_logging) { + err = logger_next(&s.logger, LOG_ROOT, s.segment_path, sizeof(s.segment_path), &s.rotate_segment); + assert(err == 0); + LOGW("rotated to %s", s.segment_path); + } + } + + if ((msg_count%1000) == 0) { + LOGD("%lu messages, %.2f msg/sec, %.2f KB/sec", msg_count, msg_count*1.0/(ts-start_ts), bytes_count*0.001/(ts-start_ts)); + } + } + + LOGW("joining threads"); + s.cv.notify_all(); + + +#ifndef DISABLE_ENCODER + front_encoder_thread_handle.join(); + encoder_thread_handle.join(); + LOGW("encoder joined"); +#endif + +#if ENABLE_LIDAR + lidar_thread_handle.join(); + LOGW("lidar joined"); +#endif + + logger_close(&s.logger); + + for (auto s : socks){ + delete s; + } + + delete s.ctx; + return 0; +} diff --git a/selfdrive/loggerd/raw_logger.cc b/selfdrive/loggerd/raw_logger.cc new file mode 100644 index 000000000..37b3f2842 --- /dev/null +++ b/selfdrive/loggerd/raw_logger.cc @@ -0,0 +1,163 @@ +#include +#include +#include + +#include +#include + +#define __STDC_CONSTANT_MACROS + +extern "C" { +#include +#include +#include +} + +#include "common/swaglog.h" +#include "common/utilpp.h" + +#include "raw_logger.h" + +RawLogger::RawLogger(const std::string &afilename, int awidth, int aheight, int afps) + : filename(afilename), + width(awidth), + height(aheight), + fps(afps) { + + int err = 0; + + av_register_all(); + codec = avcodec_find_encoder(AV_CODEC_ID_FFVHUFF); + // codec = avcodec_find_encoder(AV_CODEC_ID_FFV1); + assert(codec); + + codec_ctx = avcodec_alloc_context3(codec); + assert(codec_ctx); + codec_ctx->width = width; + codec_ctx->height = height; + codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P; + + // codec_ctx->thread_count = 2; + + // ffv1enc doesn't respect AV_PICTURE_TYPE_I. make every frame a key frame for now. + // codec_ctx->gop_size = 0; + + codec_ctx->time_base = (AVRational){ 1, fps }; + + err = avcodec_open2(codec_ctx, codec, NULL); + assert(err >= 0); + + frame = av_frame_alloc(); + assert(frame); + frame->format = codec_ctx->pix_fmt; + frame->width = width; + frame->height = height; + frame->linesize[0] = width; + frame->linesize[1] = width/2; + frame->linesize[2] = width/2; +} + +RawLogger::~RawLogger() { + av_frame_free(&frame); + avcodec_close(codec_ctx); + av_free(codec_ctx); +} + +void RawLogger::Open(const std::string &path) { + int err = 0; + + std::lock_guard guard(lock); + + vid_path = util::string_format("%s/%s.mkv", path.c_str(), filename.c_str()); + + // create camera lock file + lock_path = util::string_format("%s/%s.lock", path.c_str(), filename.c_str()); + + LOG("open %s\n", lock_path.c_str()); + + int lock_fd = open(lock_path.c_str(), O_RDWR | O_CREAT, 0777); + assert(lock_fd >= 0); + close(lock_fd); + + format_ctx = NULL; + avformat_alloc_output_context2(&format_ctx, NULL, NULL, vid_path.c_str()); + assert(format_ctx); + + stream = avformat_new_stream(format_ctx, codec); + // AVStream *stream = avformat_new_stream(format_ctx, NULL); + assert(stream); + stream->id = 0; + stream->time_base = (AVRational){ 1, fps }; + // codec_ctx->time_base = stream->time_base; + + err = avcodec_parameters_from_context(stream->codecpar, codec_ctx); + assert(err >= 0); + + err = avio_open(&format_ctx->pb, vid_path.c_str(), AVIO_FLAG_WRITE); + assert(err >= 0); + + err = avformat_write_header(format_ctx, NULL); + assert(err >= 0); + + is_open = true; + counter = 0; +} + +void RawLogger::Close() { + int err = 0; + + std::lock_guard guard(lock); + + if (!is_open) return; + + err = av_write_trailer(format_ctx); + assert(err == 0); + + avcodec_close(stream->codec); + + err = avio_closep(&format_ctx->pb); + assert(err == 0); + + avformat_free_context(format_ctx); + format_ctx = NULL; + + unlink(lock_path.c_str()); + is_open = false; +} + +int RawLogger::ProcessFrame(uint64_t ts, const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr) { + int err = 0; + + AVPacket pkt; + av_init_packet(&pkt); + pkt.data = NULL; + pkt.size = 0; + + frame->data[0] = (uint8_t*)y_ptr; + frame->data[1] = (uint8_t*)u_ptr; + frame->data[2] = (uint8_t*)v_ptr; + frame->pts = ts; + + int ret = counter; + + int got_output = 0; + err = avcodec_encode_video2(codec_ctx, &pkt, frame, &got_output); + if (err) { + LOGE("encoding error\n"); + ret = -1; + } else if (got_output) { + + av_packet_rescale_ts(&pkt, codec_ctx->time_base, stream->time_base); + pkt.stream_index = 0; + + err = av_interleaved_write_frame(format_ctx, &pkt); + if (err < 0) { + LOGE("encoder writer error\n"); + ret = -1; + } else { + counter++; + } + } + + return ret; +} diff --git a/selfdrive/loggerd/raw_logger.h b/selfdrive/loggerd/raw_logger.h new file mode 100644 index 000000000..249be40db --- /dev/null +++ b/selfdrive/loggerd/raw_logger.h @@ -0,0 +1,43 @@ +#ifndef FFV1LOGGER_H +#define FFV1LOGGER_H + +#include +#include + +#include +#include +#include +#include + +extern "C" { +#include +#include +#include +} + +#include "frame_logger.h" + +class RawLogger : public FrameLogger { +public: + RawLogger(const std::string &filename, int awidth, int aheight, int afps); + ~RawLogger(); + + int ProcessFrame(uint64_t ts, const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr); + void Open(const std::string &path); + void Close(); + +private: + std::string filename; + int width, height, fps; + int counter = 0; + + AVCodec *codec = NULL; + AVCodecContext *codec_ctx = NULL; + + AVStream *stream = NULL; + AVFormatContext *format_ctx = NULL; + + AVFrame *frame = NULL; +}; + +#endif diff --git a/selfdrive/loggerd/tests/Makefile b/selfdrive/loggerd/tests/Makefile new file mode 100644 index 000000000..d05d54917 --- /dev/null +++ b/selfdrive/loggerd/tests/Makefile @@ -0,0 +1,35 @@ +CC = clang +CXX = clang++ + +PHONELIBS = ../../../phonelibs + +WARN_FLAGS = -Werror=implicit-function-declaration \ + -Werror=incompatible-pointer-types \ + -Werror=int-conversion \ + -Werror=return-type \ + -Werror=format-extra-args \ + -Wno-deprecated-declarations + +CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) +CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) + +FFMPEG_LIBS = -lavformat \ + -lavcodec \ + -lswscale \ + -lavutil \ + -lz + +OBJS = testraw.o \ + ../RawLogger.o \ + ../../common/visionipc.o + +testraw: $(OBJS) + $(CXX) -fPIC -o '$@' $^ -L/usr/lib $(FFMPEG_LIBS) + +%.o: %.cc + @echo "[ CXX ] $@" + $(CXX) $(CXXFLAGS) \ + -I../ \ + -I../../ \ + -I../../../ \ + -c -o '$@' '$<' diff --git a/selfdrive/loggerd/tests/__init__.py b/selfdrive/loggerd/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/loggerd/tests/fill_eon.py b/selfdrive/loggerd/tests/fill_eon.py new file mode 100755 index 000000000..f273ea89e --- /dev/null +++ b/selfdrive/loggerd/tests/fill_eon.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python3 +"""Script to fill up EON with fake data""" + +import os + +from selfdrive.loggerd.config import ROOT, get_available_percent +from selfdrive.loggerd.tests.loggerd_tests_common import create_random_file + + +if __name__ == "__main__": + segment_idx = 0 + while True: + seg_name = "1970-01-01--00-00-00--%d" % segment_idx + seg_path = os.path.join(ROOT, seg_name) + + print(seg_path) + + create_random_file(os.path.join(seg_path, 'fcamera.hevc'), 36) + create_random_file(os.path.join(seg_path, 'rlog.bz2'), 2) + + segment_idx += 1 + + # Fill up to 99 percent + available_percent = get_available_percent() + if available_percent < 1.0: + break diff --git a/selfdrive/loggerd/tests/loggerd_tests_common.py b/selfdrive/loggerd/tests/loggerd_tests_common.py new file mode 100644 index 000000000..f36ef2175 --- /dev/null +++ b/selfdrive/loggerd/tests/loggerd_tests_common.py @@ -0,0 +1,81 @@ +import os +import errno +import shutil +import random +import tempfile +import unittest + +import selfdrive.loggerd.uploader as uploader + +def create_random_file(file_path, size_mb, lock=False): + try: + os.mkdir(os.path.dirname(file_path)) + except OSError: + pass + + lock_path = file_path + ".lock" + os.close(os.open(lock_path, os.O_CREAT | os.O_EXCL)) + + chunks = 128 + chunk_bytes = int(size_mb * 1024 * 1024 / chunks) + data = os.urandom(chunk_bytes) + + with open(file_path, 'wb') as f: + for _ in range(chunks): + f.write(data) + + if not lock: + os.remove(lock_path) + +class MockResponse(): + def __init__(self, text): + self.text = text + +class MockApi(): + def __init__(self, dongle_id): + pass + + def get(self, *args, **kwargs): + return MockResponse('{"url": "http://localhost/does/not/exist", "headers": {}}') + + def get_token(self): + return "fake-token" + +class MockParams(): + def __init__(self): + self.params = { + "DongleId": b"0000000000000000", + "IsUploadRawEnabled": b"1", + } + + def get(self, k): + return self.params[k] + +class UploaderTestCase(unittest.TestCase): + f_type = "UNKNOWN" + + def setUp(self): + self.root = tempfile.mkdtemp() + uploader.ROOT = self.root # Monkey patch root dir + uploader.Api = MockApi + uploader.Params = MockParams + uploader.fake_upload = 1 + uploader.is_on_hotspot = lambda *args: False + uploader.is_on_wifi = lambda *args: True + self.seg_num = random.randint(1, 300) + self.seg_format = "2019-04-18--12-52-54--{}" + self.seg_format2 = "2019-05-18--11-22-33--{}" + self.seg_dir = self.seg_format.format(self.seg_num) + + def tearDown(self): + try: + shutil.rmtree(self.root) + except OSError as e: + if e.errno != errno.ENOENT: + raise + + def make_file_with_data(self, f_dir, fn, size_mb=.1, lock=False): + file_path = os.path.join(self.root, f_dir, fn) + create_random_file(file_path, size_mb, lock) + + return file_path diff --git a/selfdrive/loggerd/tests/test_deleter.py b/selfdrive/loggerd/tests/test_deleter.py new file mode 100644 index 000000000..f06946b2f --- /dev/null +++ b/selfdrive/loggerd/tests/test_deleter.py @@ -0,0 +1,108 @@ +import os +import time +import threading +import unittest +from collections import namedtuple + +import selfdrive.loggerd.deleter as deleter +from common.timeout import Timeout, TimeoutException + +from selfdrive.loggerd.tests.loggerd_tests_common import UploaderTestCase + +Stats = namedtuple("Stats", ['f_bavail', 'f_blocks', 'f_frsize']) + + +class TestDeleter(UploaderTestCase): + def fake_statvfs(self, d): + return self.fake_stats + + def setUp(self): + self.f_type = "fcamera.hevc" + super(TestDeleter, self).setUp() + self.fake_stats = Stats(f_bavail=0, f_blocks=10, f_frsize=4096) + deleter.os.statvfs = self.fake_statvfs + deleter.ROOT = self.root + + def tearDown(self): + super(TestDeleter, self).tearDown() + + def start_thread(self): + self.end_event = threading.Event() + self.del_thread = threading.Thread(target=deleter.deleter_thread, args=[self.end_event]) + self.del_thread.daemon = True + self.del_thread.start() + + def join_thread(self): + self.end_event.set() + self.del_thread.join() + + def test_delete(self): + f_path = self.make_file_with_data(self.seg_dir, self.f_type, 1) + + self.start_thread() + + with Timeout(5, "Timeout waiting for file to be deleted"): + while os.path.exists(f_path): + time.sleep(0.01) + self.join_thread() + + self.assertFalse(os.path.exists(f_path), "File not deleted") + + def test_delete_files_in_create_order(self): + f_path_1 = self.make_file_with_data(self.seg_dir, self.f_type) + time.sleep(1) + self.seg_num += 1 + self.seg_dir = self.seg_format.format(self.seg_num) + f_path_2 = self.make_file_with_data(self.seg_dir, self.f_type) + + self.start_thread() + + with Timeout(5, "Timeout waiting for file to be deleted"): + while os.path.exists(f_path_1) and os.path.exists(f_path_2): + time.sleep(0.01) + + self.join_thread() + + self.assertFalse(os.path.exists(f_path_1), "Older file not deleted") + + self.assertTrue(os.path.exists(f_path_2), "Newer file deleted before older file") + + def test_no_delete_when_available_space(self): + f_path = self.make_file_with_data(self.seg_dir, self.f_type) + + block_size = 4096 + available = (10 * 1024 * 1024 * 1024) / block_size # 10GB free + self.fake_stats = Stats(f_bavail=available, f_blocks=10, f_frsize=block_size) + + self.start_thread() + + try: + with Timeout(2, "Timeout waiting for file to be deleted"): + while os.path.exists(f_path): + time.sleep(0.01) + except TimeoutException: + pass + finally: + self.join_thread() + + self.assertTrue(os.path.exists(f_path), "File deleted with available space") + + def test_no_delete_with_lock_file(self): + f_path = self.make_file_with_data(self.seg_dir, self.f_type, lock=True) + + self.start_thread() + + try: + with Timeout(2, "Timeout waiting for file to be deleted"): + while os.path.exists(f_path): + time.sleep(0.01) + except TimeoutException: + pass + finally: + self.join_thread() + + self.assertTrue(os.path.exists(f_path), "File deleted when locked") + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/loggerd/tests/test_uploader.py b/selfdrive/loggerd/tests/test_uploader.py new file mode 100644 index 000000000..ed9a97222 --- /dev/null +++ b/selfdrive/loggerd/tests/test_uploader.py @@ -0,0 +1,116 @@ +import os +import time +import threading +import logging +import json + +from selfdrive.swaglog import cloudlog +import selfdrive.loggerd.uploader as uploader + +from common.timeout import Timeout + +from selfdrive.loggerd.tests.loggerd_tests_common import UploaderTestCase + +class TestLogHandler(logging.Handler): + def __init__(self): + logging.Handler.__init__(self) + self.reset() + + def reset(self): + self.upload_order = list() + + def emit(self, record): + try: + j = json.loads(record.message) + if j["event"] == "upload_success": + self.upload_order.append(j["key"]) + except BaseException: + pass + +log_handler = TestLogHandler() +cloudlog.addHandler(log_handler) + +class TestUploader(UploaderTestCase): + def setUp(self): + super(TestUploader, self).setUp() + log_handler.reset() + + def tearDown(self): + super(TestUploader, self).tearDown() + + def start_thread(self): + self.end_event = threading.Event() + self.up_thread = threading.Thread(target=uploader.uploader_fn, args=[self.end_event]) + self.up_thread.daemon = True + self.up_thread.start() + + def join_thread(self): + self.end_event.set() + self.up_thread.join() + + def gen_files(self, lock=False): + f_paths = list() + for t in ["bootlog.bz2", "qlog.bz2", "rlog.bz2", "dcamera.hevc", "fcamera.hevc"]: + f_paths.append(self.make_file_with_data(self.seg_dir, t, 1, lock=lock)) + return f_paths + + def gen_order(self, seg1, seg2): + keys = [f"{self.seg_format.format(i)}/qlog.bz2" for i in seg1] + keys += [f"{self.seg_format2.format(i)}/qlog.bz2" for i in seg2] + for i in seg1: + keys += [f"{self.seg_format.format(i)}/{f}" for f in ['rlog.bz2','fcamera.hevc','dcamera.hevc']] + for i in seg2: + keys += [f"{self.seg_format2.format(i)}/{f}" for f in ['rlog.bz2','fcamera.hevc','dcamera.hevc']] + keys += [f"{self.seg_format.format(i)}/bootlog.bz2" for i in seg1] + keys += [f"{self.seg_format2.format(i)}/bootlog.bz2" for i in seg2] + return keys + + def test_upload(self): + f_paths = self.gen_files(lock=False) + + self.start_thread() + + with Timeout(5, "Timeout waiting for file to be uploaded"): + while len(os.listdir(self.root)): + time.sleep(0.01) + self.join_thread() + + for f_path in f_paths: + self.assertFalse(os.path.exists(f_path), "All files not uploaded") + exp_order = self.gen_order([self.seg_num], []) + self.assertTrue(log_handler.upload_order == exp_order, "Files uploaded in wrong order") + + def test_upload_files_in_create_order(self): + f_paths = list() + seg1_nums = [0,1,2,10,20] + for i in seg1_nums: + self.seg_dir = self.seg_format.format(i) + f_paths += self.gen_files() + seg2_nums = [5,50,51] + for i in seg2_nums: + self.seg_dir = self.seg_format2.format(i) + f_paths += self.gen_files() + + self.start_thread() + + with Timeout(5, "Timeout waiting for file to be upload"): + while len(os.listdir(self.root)): + time.sleep(0.01) + + self.join_thread() + + for f_path in f_paths: + self.assertFalse(os.path.exists(f_path), "All files not uploaded") + exp_order = self.gen_order(seg1_nums, seg2_nums) + self.assertTrue(log_handler.upload_order == exp_order, "Files uploaded in wrong order") + + def test_no_upload_with_lock_file(self): + f_paths = self.gen_files(lock=True) + + self.start_thread() + # allow enough time that files should have been uploaded if they would be uploaded + time.sleep(5) + self.join_thread() + + for f_path in f_paths: + self.assertTrue(os.path.exists(f_path), "File upload when locked") diff --git a/selfdrive/loggerd/tests/testraw.cc b/selfdrive/loggerd/tests/testraw.cc new file mode 100644 index 000000000..80858e6f2 --- /dev/null +++ b/selfdrive/loggerd/tests/testraw.cc @@ -0,0 +1,57 @@ +#include +#include +#include + +#include + +#include "common/visionipc.h" +#include "common/timing.h" + +#include "RawLogger.h" + +int main() { + int err; + + VisionStream stream; + + VisionStreamBufs buf_info; + while (true) { + err = visionstream_init(&stream, VISION_STREAM_YUV, false, &buf_info); + if (err != 0) { + printf("visionstream fail\n"); + usleep(100000); + } + break; + } + + RawLogger vidlogger("prcamera", buf_info.width, buf_info.height, 20); + vidlogger.Open("o1"); + + for (int cnt=0; cnt<200; cnt++) { + VIPCBufExtra extra; + VIPSBuf* buf = visionstream_get(&stream, &extra); + if (buf == NULL) { + printf("visionstream get failed\n"); + break; + } + + if (cnt == 100) { + vidlogger.Rotate("o2", 2); + } + + uint8_t *y = (uint8_t*)buf->addr; + uint8_t *u = y + (buf_info.width*buf_info.height); + uint8_t *v = u + (buf_info.width/2)*(buf_info.height/2); + + double t1 = millis_since_boot(); + vidlogger.LogFrame(cnt, y, u, v, NULL); + double t2 = millis_since_boot(); + printf("%d %.2f\n", cnt, (t2-t1)); + } + + vidlogger.Close(); + + visionstream_destroy(&stream); + + return 0; +} diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py new file mode 100644 index 000000000..bc83c83a0 --- /dev/null +++ b/selfdrive/loggerd/uploader.py @@ -0,0 +1,271 @@ +#!/usr/bin/env python3 +import os +import re +import time +import json +import random +import ctypes +import inspect +import requests +import traceback +import threading +import subprocess + +from selfdrive.swaglog import cloudlog +from selfdrive.loggerd.config import ROOT + +from common import android +from common.params import Params +from common.api import Api + +fake_upload = os.getenv("FAKEUPLOAD") is not None + +def raise_on_thread(t, exctype): + for ctid, tobj in threading._active.items(): + if tobj is t: + tid = ctid + break + else: + raise Exception("Could not find thread") + + '''Raises an exception in the threads with id tid''' + if not inspect.isclass(exctype): + raise TypeError("Only types can be raised (not instances)") + + res = ctypes.pythonapi.PyThreadState_SetAsyncExc(ctypes.c_long(tid), + ctypes.py_object(exctype)) + if res == 0: + raise ValueError("invalid thread id") + elif res != 1: + # "if it returns a number greater than one, you're in trouble, + # and you should call it again with exc=NULL to revert the effect" + ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, 0) + raise SystemError("PyThreadState_SetAsyncExc failed") + +def get_directory_sort(d): + return list(map(lambda s: s.rjust(10, '0'), d.rsplit('--', 1))) + +def listdir_by_creation(d): + try: + paths = os.listdir(d) + paths = sorted(paths, key=get_directory_sort) + return paths + except OSError: + cloudlog.exception("listdir_by_creation failed") + return list() + +def clear_locks(root): + for logname in os.listdir(root): + path = os.path.join(root, logname) + try: + for fname in os.listdir(path): + if fname.endswith(".lock"): + os.unlink(os.path.join(path, fname)) + except OSError: + cloudlog.exception("clear_locks failed") + +def is_on_wifi(): + # ConnectivityManager.getActiveNetworkInfo() + try: + result = android.parse_service_call_string(android.service_call(["connectivity", "2"])) + if result is None: + return True + return 'WIFI' in result + except AttributeError: + return False + +def is_on_hotspot(): + try: + result = subprocess.check_output(["ifconfig", "wlan0"], stderr=subprocess.STDOUT, encoding='utf8') + result = re.findall(r"inet addr:((\d+\.){3}\d+)", result)[0][0] + + is_android = result.startswith('192.168.43.') + is_ios = result.startswith('172.20.10.') + is_entune = result.startswith('10.0.2.') + + return (is_android or is_ios or is_entune) + except: + return False + +class Uploader(): + def __init__(self, dongle_id, root): + self.dongle_id = dongle_id + self.api = Api(dongle_id) + self.root = root + + self.upload_thread = None + + self.last_resp = None + self.last_exc = None + + self.immediate_priority = {"qlog.bz2": 0, "qcamera.ts": 1} + self.high_priority = {"rlog.bz2": 0, "fcamera.hevc": 1, "dcamera.hevc": 2} + + def clean_dirs(self): + try: + for logname in os.listdir(self.root): + path = os.path.join(self.root, logname) + # remove empty directories + if not os.listdir(path): + os.rmdir(path) + except OSError: + cloudlog.exception("clean_dirs failed") + + def get_upload_sort(self, name): + if name in self.immediate_priority: + return self.immediate_priority[name] + if name in self.high_priority: + return self.high_priority[name] + 100 + return 1000 + + def gen_upload_files(self): + if not os.path.isdir(self.root): + return + for logname in listdir_by_creation(self.root): + path = os.path.join(self.root, logname) + try: + names = os.listdir(path) + except OSError: + continue + if any(name.endswith(".lock") for name in names): + continue + + for name in sorted(names, key=self.get_upload_sort): + key = os.path.join(logname, name) + fn = os.path.join(path, name) + + yield (name, key, fn) + + def next_file_to_upload(self, with_raw): + upload_files = list(self.gen_upload_files()) + # try to upload qlog files first + for name, key, fn in upload_files: + if name in self.immediate_priority: + return (key, fn) + + if with_raw: + # then upload the full log files, rear and front camera files + for name, key, fn in upload_files: + if name in self.high_priority: + return (key, fn) + + # then upload other files + for name, key, fn in upload_files: + if not name.endswith('.lock') and not name.endswith(".tmp"): + return (key, fn) + + return None + + def do_upload(self, key, fn): + try: + url_resp = self.api.get("v1.3/"+self.dongle_id+"/upload_url/", timeout=10, path=key, access_token=self.api.get_token()) + url_resp_json = json.loads(url_resp.text) + url = url_resp_json['url'] + headers = url_resp_json['headers'] + cloudlog.info("upload_url v1.3 %s %s", url, str(headers)) + + if fake_upload: + cloudlog.info("*** WARNING, THIS IS A FAKE UPLOAD TO %s ***" % url) + class FakeResponse(): + def __init__(self): + self.status_code = 200 + self.last_resp = FakeResponse() + else: + with open(fn, "rb") as f: + self.last_resp = requests.put(url, data=f, headers=headers, timeout=10) + except Exception as e: + self.last_exc = (e, traceback.format_exc()) + raise + + def normal_upload(self, key, fn): + self.last_resp = None + self.last_exc = None + + try: + self.do_upload(key, fn) + except Exception: + pass + + return self.last_resp + + def upload(self, key, fn): + try: + sz = os.path.getsize(fn) + except OSError: + cloudlog.exception("upload: getsize failed") + return False + + cloudlog.event("upload", key=key, fn=fn, sz=sz) + + cloudlog.info("checking %r with size %r", key, sz) + + if sz == 0: + # can't upload files of 0 size + os.unlink(fn) # delete the file + success = True + else: + cloudlog.info("uploading %r", fn) + stat = self.normal_upload(key, fn) + if stat is not None and stat.status_code in (200, 201): + cloudlog.event("upload_success", key=key, fn=fn, sz=sz) + + # delete the file + try: + os.unlink(fn) + except OSError: + cloudlog.event("delete_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz) + + success = True + else: + cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz) + success = False + + self.clean_dirs() + + return success + +def uploader_fn(exit_event): + cloudlog.info("uploader_fn") + + params = Params() + dongle_id = params.get("DongleId").decode('utf8') + + if dongle_id is None: + cloudlog.info("uploader missing dongle_id") + raise Exception("uploader can't start without dongle id") + + uploader = Uploader(dongle_id, ROOT) + + backoff = 0.1 + while True: + allow_raw_upload = (params.get("IsUploadRawEnabled") != b"0") + on_hotspot = is_on_hotspot() + on_wifi = is_on_wifi() + should_upload = on_wifi and not on_hotspot + + if exit_event.is_set(): + return + + d = uploader.next_file_to_upload(with_raw=allow_raw_upload and should_upload) + if d is None: + time.sleep(5) + continue + + key, fn = d + + cloudlog.event("uploader_netcheck", is_on_hotspot=on_hotspot, is_on_wifi=on_wifi) + cloudlog.info("to upload %r", d) + success = uploader.upload(key, fn) + if success: + backoff = 0.1 + else: + cloudlog.info("backoff %r", backoff) + time.sleep(backoff + random.uniform(0, backoff)) + backoff = min(backoff*2, 120) + cloudlog.info("upload done, success=%r", success) + +def main(gctx=None): + uploader_fn(threading.Event()) + +if __name__ == "__main__": + main() diff --git a/selfdrive/mapd/__init__.py b/selfdrive/mapd/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/mapd/default_speeds.json b/selfdrive/mapd/default_speeds.json new file mode 100644 index 000000000..7126c27fc --- /dev/null +++ b/selfdrive/mapd/default_speeds.json @@ -0,0 +1,106 @@ +{ + "_comment": "These speeds are from https://wiki.openstreetmap.org/wiki/Speed_limits Special cases have been stripped", + "AR:urban": "40", + "AR:urban:primary": "60", + "AR:urban:secondary": "60", + "AR:rural": "110", + "AT:urban": "50", + "AT:rural": "100", + "AT:trunk": "100", + "AT:motorway": "130", + "BE:urban": "50", + "BE-VLG:rural": "70", + "BE-WAL:rural": "90", + "BE:trunk": "120", + "BE:motorway": "120", + "CH:urban[1]": "50", + "CH:rural": "80", + "CH:trunk": "100", + "CH:motorway": "120", + "CZ:pedestrian_zone": "20", + "CZ:living_street": "20", + "CZ:urban": "50", + "CZ:urban_trunk": "80", + "CZ:urban_motorway": "80", + "CZ:rural": "90", + "CZ:trunk": "110", + "CZ:motorway": "130", + "DK:urban": "50", + "DK:rural": "80", + "DK:motorway": "130", + "DE:living_street": "7", + "DE:residential": "30", + "DE:urban": "50", + "DE:rural": "100", + "DE:trunk": "none", + "DE:motorway": "none", + "FI:urban": "50", + "FI:rural": "80", + "FI:trunk": "100", + "FI:motorway": "120", + "FR:urban": "50", + "FR:rural": "80", + "FR:trunk": "110", + "FR:motorway": "130", + "GR:urban": "50", + "GR:rural": "90", + "GR:trunk": "110", + "GR:motorway": "130", + "HU:urban": "50", + "HU:rural": "90", + "HU:trunk": "110", + "HU:motorway": "130", + "IT:urban": "50", + "IT:rural": "90", + "IT:trunk": "110", + "IT:motorway": "130", + "JP:national": "60", + "JP:motorway": "100", + "LT:living_street": "20", + "LT:urban": "50", + "LT:rural": "90", + "LT:trunk": "120", + "LT:motorway": "130", + "PL:living_street": "20", + "PL:urban": "50", + "PL:rural": "90", + "PL:trunk": "100", + "PL:motorway": "140", + "RO:urban": "50", + "RO:rural": "90", + "RO:trunk": "100", + "RO:motorway": "130", + "RU:living_street": "20", + "RU:urban": "60", + "RU:rural": "90", + "RU:motorway": "110", + "SK:urban": "50", + "SK:rural": "90", + "SK:trunk": "90", + "SK:motorway": "90", + "SI:urban": "50", + "SI:rural": "90", + "SI:trunk": "110", + "SI:motorway": "130", + "ES:living_street": "20", + "ES:urban": "50", + "ES:rural": "50", + "ES:trunk": "90", + "ES:motorway": "120", + "SE:urban": "50", + "SE:rural": "70", + "SE:trunk": "90", + "SE:motorway": "110", + "GB:nsl_restricted": "30 mph", + "GB:nsl_single": "60 mph", + "GB:nsl_dual": "70 mph", + "GB:motorway": "70 mph", + "UA:urban": "50", + "UA:rural": "90", + "UA:trunk": "110", + "UA:motorway": "130", + "UZ:living_street": "30", + "UZ:urban": "70", + "UZ:rural": "100", + "UZ:motorway": "110" +} diff --git a/selfdrive/mapd/default_speeds_generator.py b/selfdrive/mapd/default_speeds_generator.py new file mode 100755 index 000000000..aa2e55f68 --- /dev/null +++ b/selfdrive/mapd/default_speeds_generator.py @@ -0,0 +1,240 @@ +#!/usr/bin/env python3 +import json + +DEFAULT_OUTPUT_FILENAME = "default_speeds_by_region.json" + +def main(filename = DEFAULT_OUTPUT_FILENAME): + countries = [] + + """ + -------------------------------------------------- + US - United State of America + -------------------------------------------------- + """ + US = Country("US") # First step, create the country using the ISO 3166 two letter code + countries.append(US) # Second step, add the country to countries list + + """ Default rules """ + # Third step, add some default rules for the country + # Speed limit rules are based on OpenStreetMaps (OSM) tags. + # The dictionary {...} defines the tag_name: value + # if a road in OSM has a tag with the name tag_name and this value, the speed limit listed below will be applied. + # The text at the end is the speed limit (use no unit for km/h) + # Rules apply in the order in which they are written for each country + # Rules for specific regions (states) take priority over country rules + # If you modify existing country rules, you must update all existing states without that rule to use the old rule + US.add_rule({"highway": "motorway"}, "65 mph") # On US roads with the tag highway and value motorway, the speed limit will default to 65 mph + US.add_rule({"highway": "trunk"}, "55 mph") + US.add_rule({"highway": "primary"}, "55 mph") + US.add_rule({"highway": "secondary"}, "45 mph") + US.add_rule({"highway": "tertiary"}, "35 mph") + US.add_rule({"highway": "unclassified"}, "55 mph") + US.add_rule({"highway": "residential"}, "25 mph") + US.add_rule({"highway": "service"}, "25 mph") + US.add_rule({"highway": "motorway_link"}, "55 mph") + US.add_rule({"highway": "trunk_link"}, "55 mph") + US.add_rule({"highway": "primary_link"}, "55 mph") + US.add_rule({"highway": "secondary_link"}, "45 mph") + US.add_rule({"highway": "tertiary_link"}, "35 mph") + US.add_rule({"highway": "living_street"}, "15 mph") + + """ States """ + new_york = US.add_region("New York") # Fourth step, add a state/region to country + new_york.add_rule({"highway": "primary"}, "45 mph") # Fifth step , add rules to the state. See the text above for how to write rules + new_york.add_rule({"highway": "secondary"}, "55 mph") + new_york.add_rule({"highway": "tertiary"}, "55 mph") + new_york.add_rule({"highway": "residential"}, "30 mph") + new_york.add_rule({"highway": "primary_link"}, "45 mph") + new_york.add_rule({"highway": "secondary_link"}, "55 mph") + new_york.add_rule({"highway": "tertiary_link"}, "55 mph") + # All if not written by the state, the rules will default to the country rules + + #california = US.add_region("California") + # California uses only the default US rules + + michigan = US.add_region("Michigan") + michigan.add_rule({"highway": "motorway"}, "70 mph") + + oregon = US.add_region("Oregon") + oregon.add_rule({"highway": "motorway"}, "55 mph") + oregon.add_rule({"highway": "secondary"}, "35 mph") + oregon.add_rule({"highway": "tertiary"}, "30 mph") + oregon.add_rule({"highway": "service"}, "15 mph") + oregon.add_rule({"highway": "secondary_link"}, "35 mph") + oregon.add_rule({"highway": "tertiary_link"}, "30 mph") + + south_dakota = US.add_region("South Dakota") + south_dakota.add_rule({"highway": "motorway"}, "80 mph") + south_dakota.add_rule({"highway": "trunk"}, "70 mph") + south_dakota.add_rule({"highway": "primary"}, "65 mph") + south_dakota.add_rule({"highway": "trunk_link"}, "70 mph") + south_dakota.add_rule({"highway": "primary_link"}, "65 mph") + + wisconsin = US.add_region("Wisconsin") + wisconsin.add_rule({"highway": "trunk"}, "65 mph") + wisconsin.add_rule({"highway": "tertiary"}, "45 mph") + wisconsin.add_rule({"highway": "unclassified"}, "35 mph") + wisconsin.add_rule({"highway": "trunk_link"}, "65 mph") + wisconsin.add_rule({"highway": "tertiary_link"}, "45 mph") + + """ + -------------------------------------------------- + AU - Australia + -------------------------------------------------- + """ + AU = Country("AU") + countries.append(AU) + + """ Default rules """ + AU.add_rule({"highway": "motorway"}, "100") + AU.add_rule({"highway": "trunk"}, "80") + AU.add_rule({"highway": "primary"}, "80") + AU.add_rule({"highway": "secondary"}, "50") + AU.add_rule({"highway": "tertiary"}, "50") + AU.add_rule({"highway": "unclassified"}, "80") + AU.add_rule({"highway": "residential"}, "50") + AU.add_rule({"highway": "service"}, "40") + AU.add_rule({"highway": "motorway_link"}, "90") + AU.add_rule({"highway": "trunk_link"}, "80") + AU.add_rule({"highway": "primary_link"}, "80") + AU.add_rule({"highway": "secondary_link"}, "50") + AU.add_rule({"highway": "tertiary_link"}, "50") + AU.add_rule({"highway": "living_street"}, "30") + + """ + -------------------------------------------------- + CA - Canada + -------------------------------------------------- + """ + CA = Country("CA") + countries.append(CA) + + """ Default rules """ + CA.add_rule({"highway": "motorway"}, "100") + CA.add_rule({"highway": "trunk"}, "80") + CA.add_rule({"highway": "primary"}, "80") + CA.add_rule({"highway": "secondary"}, "50") + CA.add_rule({"highway": "tertiary"}, "50") + CA.add_rule({"highway": "unclassified"}, "80") + CA.add_rule({"highway": "residential"}, "40") + CA.add_rule({"highway": "service"}, "40") + CA.add_rule({"highway": "motorway_link"}, "90") + CA.add_rule({"highway": "trunk_link"}, "80") + CA.add_rule({"highway": "primary_link"}, "80") + CA.add_rule({"highway": "secondary_link"}, "50") + CA.add_rule({"highway": "tertiary_link"}, "50") + CA.add_rule({"highway": "living_street"}, "20") + + + """ + -------------------------------------------------- + DE - Germany + -------------------------------------------------- + """ + DE = Country("DE") + countries.append(DE) + + """ Default rules """ + DE.add_rule({"highway": "motorway"}, "none") + DE.add_rule({"highway": "living_street"}, "10") + DE.add_rule({"highway": "residential"}, "30") + DE.add_rule({"zone:traffic": "DE:rural"}, "100") + DE.add_rule({"zone:traffic": "DE:urban"}, "50") + DE.add_rule({"zone:maxspeed": "DE:30"}, "30") + DE.add_rule({"zone:maxspeed": "DE:urban"}, "50") + DE.add_rule({"zone:maxspeed": "DE:rural"}, "100") + DE.add_rule({"zone:maxspeed": "DE:motorway"}, "none") + DE.add_rule({"bicycle_road": "yes"}, "30") + + + """ + -------------------------------------------------- + EE - Estonia + -------------------------------------------------- + """ + EE = Country("EE") + countries.append(EE) + + """ Default rules """ + EE.add_rule({"highway": "motorway"}, "90") + EE.add_rule({"highway": "trunk"}, "90") + EE.add_rule({"highway": "primary"}, "90") + EE.add_rule({"highway": "secondary"}, "50") + EE.add_rule({"highway": "tertiary"}, "50") + EE.add_rule({"highway": "unclassified"}, "90") + EE.add_rule({"highway": "residential"}, "40") + EE.add_rule({"highway": "service"}, "40") + EE.add_rule({"highway": "motorway_link"}, "90") + EE.add_rule({"highway": "trunk_link"}, "70") + EE.add_rule({"highway": "primary_link"}, "70") + EE.add_rule({"highway": "secondary_link"}, "50") + EE.add_rule({"highway": "tertiary_link"}, "50") + EE.add_rule({"highway": "living_street"}, "20") + + + """ --- DO NOT MODIFY CODE BELOW THIS LINE --- """ + """ --- ADD YOUR COUNTRY OR STATE ABOVE --- """ + + # Final step + write_json(countries, filename) + +def write_json(countries, filename = DEFAULT_OUTPUT_FILENAME): + out_dict = {} + for country in countries: + out_dict.update(country.jsonify()) + json_string = json.dumps(out_dict, indent=2) + with open(filename, "wb") as f: + f.write(json_string) + + +class Region(): + ALLOWABLE_TAG_KEYS = ["highway", "zone:traffic", "bicycle_road", "zone:maxspeed"] + ALLOWABLE_HIGHWAY_TYPES = ["motorway", "trunk", "primary", "secondary", "tertiary", "unclassified", "residential", "service", "motorway_link", "trunk_link", "primary_link", "secondary_link", "tertiary_link", "living_street"] + def __init__(self, name): + self.name = name + self.rules = [] + + def add_rule(self, tag_conditions, speed): + new_rule = {} + if not isinstance(tag_conditions, dict): + raise TypeError("Rule tag conditions must be dictionary") + if not all(tag_key in self.ALLOWABLE_TAG_KEYS for tag_key in tag_conditions): + raise ValueError("Rule tag keys must be in allowable tag kesy") # If this is by mistake, please update ALLOWABLE_TAG_KEYS + if 'highway' in tag_conditions: + if not tag_conditions['highway'] in self.ALLOWABLE_HIGHWAY_TYPES: + raise ValueError("Invalid Highway type {}".format(tag_conditions["highway"])) + new_rule['tags'] = tag_conditions + try: + new_rule['speed'] = str(speed) + except ValueError: + raise ValueError("Rule speed must be string") + self.rules.append(new_rule) + + def jsonify(self): + ret_dict = {} + ret_dict[self.name] = self.rules + return ret_dict + +class Country(Region): + ALLOWABLE_COUNTRY_CODES = ["AF","AX","AL","DZ","AS","AD","AO","AI","AQ","AG","AR","AM","AW","AU","AT","AZ","BS","BH","BD","BB","BY","BE","BZ","BJ","BM","BT","BO","BQ","BA","BW","BV","BR","IO","BN","BG","BF","BI","KH","CM","CA","CV","KY","CF","TD","CL","CN","CX","CC","CO","KM","CG","CD","CK","CR","CI","HR","CU","CW","CY","CZ","DK","DJ","DM","DO","EC","EG","SV","GQ","ER","EE","ET","FK","FO","FJ","FI","FR","GF","PF","TF","GA","GM","GE","DE","GH","GI","GR","GL","GD","GP","GU","GT","GG","GN","GW","GY","HT","HM","VA","HN","HK","HU","IS","IN","ID","IR","IQ","IE","IM","IL","IT","JM","JP","JE","JO","KZ","KE","KI","KP","KR","KW","KG","LA","LV","LB","LS","LR","LY","LI","LT","LU","MO","MK","MG","MW","MY","MV","ML","MT","MH","MQ","MR","MU","YT","MX","FM","MD","MC","MN","ME","MS","MA","MZ","MM","NA","NR","NP","NL","NC","NZ","NI","NE","NG","NU","NF","MP","NO","OM","PK","PW","PS","PA","PG","PY","PE","PH","PN","PL","PT","PR","QA","RE","RO","RU","RW","BL","SH","KN","LC","MF","PM","VC","WS","SM","ST","SA","SN","RS","SC","SL","SG","SX","SK","SI","SB","SO","ZA","GS","SS","ES","LK","SD","SR","SJ","SZ","SE","CH","SY","TW","TJ","TZ","TH","TL","TG","TK","TO","TT","TN","TR","TM","TC","TV","UG","UA","AE","GB","US","UM","UY","UZ","VU","VE","VN","VG","VI","WF","EH","YE","ZM","ZW"] + def __init__(self, ISO_3166_alpha_2): + Region.__init__(self, ISO_3166_alpha_2) + if ISO_3166_alpha_2 not in self.ALLOWABLE_COUNTRY_CODES: + raise ValueError("Not valid IOS 3166 country code") + self.regions = {} + + def add_region(self, name): + self.regions[name] = Region(name) + return self.regions[name] + + def jsonify(self): + ret_dict = {} + ret_dict[self.name] = {} + for r_name, region in self.regions.items(): + ret_dict[self.name].update(region.jsonify()) + ret_dict[self.name]['Default'] = self.rules + return ret_dict + + +if __name__ == '__main__': + main() diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py new file mode 100755 index 000000000..5bd454b1a --- /dev/null +++ b/selfdrive/mapd/mapd.py @@ -0,0 +1,296 @@ +#!/usr/bin/env python3 + +# Add phonelibs openblas to LD_LIBRARY_PATH if import fails +from common.basedir import BASEDIR +try: + from scipy import spatial +except ImportError as e: + import os + import sys + + + openblas_path = os.path.join(BASEDIR, "phonelibs/openblas/") + os.environ['LD_LIBRARY_PATH'] += ':' + openblas_path + + args = [sys.executable] + args.extend(sys.argv) + os.execv(sys.executable, args) + +DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json" +from selfdrive.mapd import default_speeds_generator +default_speeds_generator.main(DEFAULT_SPEEDS_BY_REGION_JSON_FILE) + +import os +import sys +import time +import zmq +import threading +import numpy as np +import overpy +from collections import defaultdict + +from common.params import Params +from common.transformations.coordinates import geodetic2ecef +from cereal.services import service_list +import cereal.messaging as messaging +from selfdrive.mapd.mapd_helpers import MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points +import selfdrive.crash as crash +from selfdrive.version import version, dirty + + +OVERPASS_API_URL = "https://overpass.kumi.systems/api/interpreter" +OVERPASS_HEADERS = { + 'User-Agent': 'NEOS (comma.ai)', + 'Accept-Encoding': 'gzip' +} + +last_gps = None +query_lock = threading.Lock() +last_query_result = None +last_query_pos = None +cache_valid = False + +def build_way_query(lat, lon, radius=50): + """Builds a query to find all highways within a given radius around a point""" + pos = " (around:%f,%f,%f)" % (radius, lat, lon) + lat_lon = "(%f,%f)" % (lat, lon) + q = """( + way + """ + pos + """ + [highway][highway!~"^(footway|path|bridleway|steps|cycleway|construction|bus_guideway|escape)$"]; + >;);out;""" + """is_in""" + lat_lon + """;area._[admin_level~"[24]"]; + convert area ::id = id(), admin_level = t['admin_level'], + name = t['name'], "ISO3166-1:alpha2" = t['ISO3166-1:alpha2'];out; + """ + return q + + +def query_thread(): + global last_query_result, last_query_pos, cache_valid + api = overpy.Overpass(url=OVERPASS_API_URL, headers=OVERPASS_HEADERS, timeout=10.) + + while True: + time.sleep(1) + if last_gps is not None: + fix_ok = last_gps.flags & 1 + if not fix_ok: + continue + + if last_query_pos is not None: + cur_ecef = geodetic2ecef((last_gps.latitude, last_gps.longitude, last_gps.altitude)) + prev_ecef = geodetic2ecef((last_query_pos.latitude, last_query_pos.longitude, last_query_pos.altitude)) + dist = np.linalg.norm(cur_ecef - prev_ecef) + if dist < 1000: #updated when we are 1km from the edge of the downloaded circle + continue + + if dist > 3000: + cache_valid = False + + q = build_way_query(last_gps.latitude, last_gps.longitude, radius=3000) + try: + new_result = api.query(q) + + # Build kd-tree + nodes = [] + real_nodes = [] + node_to_way = defaultdict(list) + location_info = {} + + for n in new_result.nodes: + nodes.append((float(n.lat), float(n.lon), 0)) + real_nodes.append(n) + + for way in new_result.ways: + for n in way.nodes: + node_to_way[n.id].append(way) + + for area in new_result.areas: + if area.tags.get('admin_level', '') == "2": + location_info['country'] = area.tags.get('ISO3166-1:alpha2', '') + if area.tags.get('admin_level', '') == "4": + location_info['region'] = area.tags.get('name', '') + + nodes = np.asarray(nodes) + nodes = geodetic2ecef(nodes) + tree = spatial.cKDTree(nodes) + + query_lock.acquire() + last_query_result = new_result, tree, real_nodes, node_to_way, location_info + last_query_pos = last_gps + cache_valid = True + query_lock.release() + + except Exception as e: + print(e) + query_lock.acquire() + last_query_result = None + query_lock.release() + + +def mapsd_thread(): + global last_gps + + gps_sock = messaging.sub_sock('gpsLocation', conflate=True) + gps_external_sock = messaging.sub_sock('gpsLocationExternal', conflate=True) + map_data_sock = messaging.pub_sock('liveMapData') + + cur_way = None + curvature_valid = False + curvature = None + upcoming_curvature = 0. + dist_to_turn = 0. + road_points = None + + while True: + gps = messaging.recv_one(gps_sock) + gps_ext = messaging.recv_one_or_none(gps_external_sock) + + if gps_ext is not None: + gps = gps_ext.gpsLocationExternal + else: + gps = gps.gpsLocation + + last_gps = gps + + fix_ok = gps.flags & 1 + if not fix_ok or last_query_result is None or not cache_valid: + cur_way = None + curvature = None + curvature_valid = False + upcoming_curvature = 0. + dist_to_turn = 0. + road_points = None + map_valid = False + else: + map_valid = True + lat = gps.latitude + lon = gps.longitude + heading = gps.bearing + speed = gps.speed + + query_lock.acquire() + cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way) + if cur_way is not None: + pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) + + xs = pnts[:, 0] + ys = pnts[:, 1] + road_points = [float(x) for x in xs], [float(y) for y in ys] + + if speed < 10: + curvature_valid = False + if curvature_valid and pnts.shape[0] <= 3: + curvature_valid = False + + # The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found + if curvature_valid: + # Compute the curvature for each point + with np.errstate(divide='ignore'): + circles = [circle_through_points(*p) for p in zip(pnts, pnts[1:], pnts[2:])] + circles = np.asarray(circles) + radii = np.nan_to_num(circles[:, 2]) + radii[radii < 10] = np.inf + curvature = 1. / radii + + # Index of closest point + closest = np.argmin(np.linalg.norm(pnts, axis=1)) + dist_to_closest = pnts[closest, 0] # We can use x distance here since it should be close + + # Compute distance along path + dists = list() + dists.append(0) + for p, p_prev in zip(pnts, pnts[1:, :]): + dists.append(dists[-1] + np.linalg.norm(p - p_prev)) + dists = np.asarray(dists) + dists = dists - dists[closest] + dist_to_closest + dists = dists[1:-1] + + close_idx = np.logical_and(dists > 0, dists < 500) + dists = dists[close_idx] + curvature = curvature[close_idx] + + if len(curvature): + # TODO: Determine left or right turn + curvature = np.nan_to_num(curvature) + + # Outlier rejection + new_curvature = np.percentile(curvature, 90, interpolation='lower') + + k = 0.6 + upcoming_curvature = k * upcoming_curvature + (1 - k) * new_curvature + in_turn_indices = curvature > 0.8 * new_curvature + + if np.any(in_turn_indices): + dist_to_turn = np.min(dists[in_turn_indices]) + else: + dist_to_turn = 999 + else: + upcoming_curvature = 0. + dist_to_turn = 999 + + query_lock.release() + + dat = messaging.new_message() + dat.init('liveMapData') + + if last_gps is not None: + dat.liveMapData.lastGps = last_gps + + if cur_way is not None: + dat.liveMapData.wayId = cur_way.id + + # Speed limit + max_speed = cur_way.max_speed() + if max_speed is not None: + dat.liveMapData.speedLimitValid = True + dat.liveMapData.speedLimit = max_speed + + # TODO: use the function below to anticipate upcoming speed limits + #max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) + #if max_speed_ahead is not None and max_speed_ahead_dist is not None: + # dat.liveMapData.speedLimitAheadValid = True + # dat.liveMapData.speedLimitAhead = float(max_speed_ahead) + # dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist) + + + advisory_max_speed = cur_way.advisory_max_speed() + if advisory_max_speed is not None: + dat.liveMapData.speedAdvisoryValid = True + dat.liveMapData.speedAdvisory = advisory_max_speed + + # Curvature + dat.liveMapData.curvatureValid = curvature_valid + dat.liveMapData.curvature = float(upcoming_curvature) + dat.liveMapData.distToTurn = float(dist_to_turn) + if road_points is not None: + dat.liveMapData.roadX, dat.liveMapData.roadY = road_points + if curvature is not None: + dat.liveMapData.roadCurvatureX = [float(x) for x in dists] + dat.liveMapData.roadCurvature = [float(x) for x in curvature] + + dat.liveMapData.mapValid = map_valid + + map_data_sock.send(dat.to_bytes()) + + +def main(gctx=None): + params = Params() + dongle_id = params.get("DongleId") + crash.bind_user(id=dongle_id) + crash.bind_extra(version=version, dirty=dirty, is_eon=True) + crash.install() + + main_thread = threading.Thread(target=mapsd_thread) + main_thread.daemon = True + main_thread.start() + + q_thread = threading.Thread(target=query_thread) + q_thread.daemon = True + q_thread.start() + + while True: + time.sleep(0.1) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py new file mode 100644 index 000000000..58a33ee41 --- /dev/null +++ b/selfdrive/mapd/mapd_helpers.py @@ -0,0 +1,364 @@ +import math +import json +import numpy as np +from datetime import datetime +from common.basedir import BASEDIR +from selfdrive.config import Conversions as CV +from common.transformations.coordinates import LocalCoord, geodetic2ecef + +LOOKAHEAD_TIME = 10. +MAPS_LOOKAHEAD_DISTANCE = 50 * LOOKAHEAD_TIME + +DEFAULT_SPEEDS_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds.json" +DEFAULT_SPEEDS = {} +with open(DEFAULT_SPEEDS_JSON_FILE, "rb") as f: + DEFAULT_SPEEDS = json.loads(f.read()) + +DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json" +DEFAULT_SPEEDS_BY_REGION = {} +with open(DEFAULT_SPEEDS_BY_REGION_JSON_FILE, "rb") as f: + DEFAULT_SPEEDS_BY_REGION = json.loads(f.read()) + +def circle_through_points(p1, p2, p3): + """Fits a circle through three points + Formulas from: http://www.ambrsoft.com/trigocalc/circle3d.htm""" + x1, y1, _ = p1 + x2, y2, _ = p2 + x3, y3, _ = p3 + + A = x1 * (y2 - y3) - y1 * (x2 - x3) + x2 * y3 - x3 * y2 + B = (x1**2 + y1**2) * (y3 - y2) + (x2**2 + y2**2) * (y1 - y3) + (x3**2 + y3**2) * (y2 - y1) + C = (x1**2 + y1**2) * (x2 - x3) + (x2**2 + y2**2) * (x3 - x1) + (x3**2 + y3**2) * (x1 - x2) + D = (x1**2 + y1**2) * (x3 * y2 - x2 * y3) + (x2**2 + y2**2) * (x1 * y3 - x3 * y1) + (x3**2 + y3**2) * (x2 * y1 - x1 * y2) + + return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2))) + +def parse_speed_unit(max_speed): + """Converts a maxspeed string to m/s based on the unit present in the input. + OpenStreetMap defaults to kph if no unit is present. """ + + if not max_speed: + return None + + conversion = CV.KPH_TO_MS + if 'mph' in max_speed: + max_speed = max_speed.replace(' mph', '') + conversion = CV.MPH_TO_MS + try: + return float(max_speed) * conversion + except ValueError: + return None + +def parse_speed_tags(tags): + """Parses tags on a way to find the maxspeed string""" + max_speed = None + + if 'maxspeed' in tags: + max_speed = tags['maxspeed'] + + if 'maxspeed:conditional' in tags: + try: + max_speed_cond, cond = tags['maxspeed:conditional'].split(' @ ') + cond = cond[1:-1] + + start, end = cond.split('-') + now = datetime.now() # TODO: Get time and timezone from gps fix so this will work correctly on replays + start = datetime.strptime(start, "%H:%M").replace(year=now.year, month=now.month, day=now.day) + end = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day) + + if start <= now <= end: + max_speed = max_speed_cond + except ValueError: + pass + + if not max_speed and 'source:maxspeed' in tags: + max_speed = DEFAULT_SPEEDS.get(tags['source:maxspeed'], None) + if not max_speed and 'maxspeed:type' in tags: + max_speed = DEFAULT_SPEEDS.get(tags['maxspeed:type'], None) + + max_speed = parse_speed_unit(max_speed) + return max_speed + +def geocode_maxspeed(tags, location_info): + max_speed = None + try: + geocode_country = location_info.get('country', '') + geocode_region = location_info.get('region', '') + + country_rules = DEFAULT_SPEEDS_BY_REGION.get(geocode_country, {}) + country_defaults = country_rules.get('Default', []) + for rule in country_defaults: + rule_valid = all( + tag_name in tags + and tags[tag_name] == value + for tag_name, value in rule['tags'].items() + ) + if rule_valid: + max_speed = rule['speed'] + break #stop searching country + + region_rules = country_rules.get(geocode_region, []) + for rule in region_rules: + rule_valid = all( + tag_name in tags + and tags[tag_name] == value + for tag_name, value in rule['tags'].items() + ) + if rule_valid: + max_speed = rule['speed'] + break #stop searching region + except KeyError: + pass + max_speed = parse_speed_unit(max_speed) + return max_speed + +class Way: + def __init__(self, way, query_results): + self.id = way.id + self.way = way + self.query_results = query_results + + points = list() + + for node in self.way.get_nodes(resolve_missing=False): + points.append((float(node.lat), float(node.lon), 0.)) + + self.points = np.asarray(points) + + @classmethod + def closest(cls, query_results, lat, lon, heading, prev_way=None): + results, tree, real_nodes, node_to_way, location_info = query_results + + cur_pos = geodetic2ecef((lat, lon, 0)) + nodes = tree.query_ball_point(cur_pos, 500) + + # If no nodes within 500m, choose closest one + if not nodes: + nodes = [tree.query(cur_pos)[1]] + + ways = [] + for n in nodes: + real_node = real_nodes[n] + ways += node_to_way[real_node.id] + ways = set(ways) + + closest_way = None + best_score = None + for way in ways: + way = Way(way, query_results) + points = way.points_in_car_frame(lat, lon, heading) + + on_way = way.on_way(lat, lon, heading, points) + if not on_way: + continue + + # Create mask of points in front and behind + x = points[:, 0] + y = points[:, 1] + angles = np.arctan2(y, x) + front = np.logical_and((-np.pi / 2) < angles, + angles < (np.pi / 2)) + behind = np.logical_not(front) + + dists = np.linalg.norm(points, axis=1) + + # Get closest point behind the car + dists_behind = np.copy(dists) + dists_behind[front] = np.NaN + closest_behind = points[np.nanargmin(dists_behind)] + + # Get closest point in front of the car + dists_front = np.copy(dists) + dists_front[behind] = np.NaN + closest_front = points[np.nanargmin(dists_front)] + + # fit line: y = a*x + b + x1, y1, _ = closest_behind + x2, y2, _ = closest_front + a = (y2 - y1) / max((x2 - x1), 1e-5) + b = y1 - a * x1 + + # With a factor of 60 a 20m offset causes the same error as a 20 degree heading error + # (A 20 degree heading offset results in an a of about 1/3) + score = abs(a) * 60. + abs(b) + + # Prefer same type of road + if prev_way is not None: + if way.way.tags.get('highway', '') == prev_way.way.tags.get('highway', ''): + score *= 0.5 + + if closest_way is None or score < best_score: + closest_way = way + best_score = score + + # Normal score is < 5 + if best_score > 50: + return None + + return closest_way + + def __str__(self): + return "%s %s" % (self.id, self.way.tags) + + def max_speed(self): + """Extracts the (conditional) speed limit from a way""" + if not self.way: + return None + + max_speed = parse_speed_tags(self.way.tags) + if not max_speed: + location_info = self.query_results[4] + max_speed = geocode_maxspeed(self.way.tags, location_info) + + return max_speed + + def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead): + """Look ahead for a max speed""" + if not self.way: + return None + + speed_ahead = None + speed_ahead_dist = None + lookahead_ways = 5 + way = self + for i in range(lookahead_ways): + way_pts = way.points_in_car_frame(lat, lon, heading) + + # Check current lookahead distance + max_dist = np.linalg.norm(way_pts[-1, :]) + + if max_dist > 2 * lookahead: + break + + if 'maxspeed' in way.way.tags: + spd = parse_speed_tags(way.way.tags) + if not spd: + location_info = self.query_results[4] + spd = geocode_maxspeed(way.way.tags, location_info) + if spd < current_speed_limit: + speed_ahead = spd + min_dist = np.linalg.norm(way_pts[1, :]) + speed_ahead_dist = min_dist + break + # Find next way + way = way.next_way() + if not way: + break + + return speed_ahead, speed_ahead_dist + + def advisory_max_speed(self): + if not self.way: + return None + + tags = self.way.tags + adv_speed = None + + if 'maxspeed:advisory' in tags: + adv_speed = tags['maxspeed:advisory'] + adv_speed = parse_speed_unit(adv_speed) + return adv_speed + + def on_way(self, lat, lon, heading, points=None): + if points is None: + points = self.points_in_car_frame(lat, lon, heading) + x = points[:, 0] + return np.min(x) < 0. and np.max(x) > 0. + + def closest_point(self, lat, lon, heading, points=None): + if points is None: + points = self.points_in_car_frame(lat, lon, heading) + i = np.argmin(np.linalg.norm(points, axis=1)) + return points[i] + + def distance_to_closest_node(self, lat, lon, heading, points=None): + if points is None: + points = self.points_in_car_frame(lat, lon, heading) + return np.min(np.linalg.norm(points, axis=1)) + + def points_in_car_frame(self, lat, lon, heading): + lc = LocalCoord.from_geodetic([lat, lon, 0.]) + + # Build rotation matrix + heading = math.radians(-heading + 90) + c, s = np.cos(heading), np.sin(heading) + rot = np.array([[c, s, 0.], [-s, c, 0.], [0., 0., 1.]]) + + # Convert to local coordinates + points_carframe = lc.geodetic2ned(self.points).T + + # Rotate with heading of car + points_carframe = np.dot(rot, points_carframe[(1, 0, 2), :]).T + + return points_carframe + + def next_way(self, backwards=False): + results, tree, real_nodes, node_to_way, location_info = self.query_results + + if backwards: + node = self.way.nodes[0] + else: + node = self.way.nodes[-1] + + ways = node_to_way[node.id] + + way = None + try: + # Simple heuristic to find next way + ways = [w for w in ways if w.id != self.id] + ways = [w for w in ways if w.nodes[0] == node] + + # Filter on highway tag + acceptable_tags = list() + cur_tag = self.way.tags['highway'] + acceptable_tags.append(cur_tag) + if cur_tag == 'motorway_link': + acceptable_tags.append('motorway') + acceptable_tags.append('trunk') + acceptable_tags.append('primary') + ways = [w for w in ways if w.tags['highway'] in acceptable_tags] + + # Filter on number of lanes + cur_num_lanes = int(self.way.tags['lanes']) + if len(ways) > 1: + ways_same_lanes = [w for w in ways if int(w.tags['lanes']) == cur_num_lanes] + if len(ways_same_lanes) == 1: + ways = ways_same_lanes + if len(ways) > 1: + ways = [w for w in ways if int(w.tags['lanes']) > cur_num_lanes] + if len(ways) == 1: + way = Way(ways[0], self.query_results) + + except (KeyError, ValueError): + pass + + return way + + def get_lookahead(self, lat, lon, heading, lookahead): + pnts = None + way = self + valid = False + + for i in range(5): + # Get new points and append to list + new_pnts = way.points_in_car_frame(lat, lon, heading) + + if pnts is None: + pnts = new_pnts + else: + pnts = np.vstack([pnts, new_pnts]) + + # Check current lookahead distance + max_dist = np.linalg.norm(pnts[-1, :]) + if max_dist > lookahead: + valid = True + + if max_dist > 2 * lookahead: + break + + # Find next way + way = way.next_way() + if not way: + break + + return pnts, valid diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript new file mode 100644 index 000000000..aa995fb3f --- /dev/null +++ b/selfdrive/modeld/SConscript @@ -0,0 +1,36 @@ +Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc') +lenv = env.Clone() + +libs = [messaging, common, 'OpenCL', 'SNPE', 'capnp', 'zmq', 'kj', 'yuv', gpucommon, visionipc] + +common_src = [ + "models/commonmodel.c", + "runners/snpemodel.cc", + "transforms/loadyuv.c", + "transforms/transform.c"] + +if arch == "aarch64": + libs += ['gsl', 'CB', 'gnustl_shared'] +else: + libs += ['symphony-cpu', 'pthread'] + + if FindFile('libtensorflow.so', env['LIBPATH']): + # for tensorflow support + common_src += ['runners/tfmodel.cc'] + libs += ['tensorflow'] + # tell runners to use it + lenv['CFLAGS'].append("-DUSE_TF_MODEL") + lenv['CXXFLAGS'].append("-DUSE_TF_MODEL") + +common = lenv.Object(common_src) + +lenv.Program('_monitoringd', [ + "monitoringd.cc", + "models/monitoring.cc", + ]+common, LIBS=libs) + +lenv.Program('_modeld', [ + "modeld.cc", + "models/driving.cc", + ]+common, LIBS=libs) + diff --git a/selfdrive/modeld/__init__.py b/selfdrive/modeld/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py new file mode 100644 index 000000000..2bf48574f --- /dev/null +++ b/selfdrive/modeld/constants.py @@ -0,0 +1,6 @@ +MAX_DISTANCE = 140. +LANE_OFFSET = 1.8 +MAX_REL_V = 10. + +LEAD_X_SCALE = 10 +LEAD_Y_SCALE = 10 diff --git a/selfdrive/modeld/modeld b/selfdrive/modeld/modeld new file mode 100755 index 000000000..3629f7a71 --- /dev/null +++ b/selfdrive/modeld/modeld @@ -0,0 +1,4 @@ +#!/bin/sh +export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/:/home/batman/one/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH" +exec ./_modeld + diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc new file mode 100644 index 000000000..fa49d1b5d --- /dev/null +++ b/selfdrive/modeld/modeld.cc @@ -0,0 +1,247 @@ +#include +#include + +#include "common/visionbuf.h" +#include "common/visionipc.h" +#include "common/swaglog.h" + +#include "models/driving.h" + +volatile sig_atomic_t do_exit = 0; + +static void set_do_exit(int sig) { + do_exit = 1; +} + +// globals +bool run_model; +mat3 cur_transform; +pthread_mutex_t transform_lock; + +void* live_thread(void *arg) { + int err; + set_thread_name("live"); + + Context * c = Context::create(); + SubSocket * live_calibration_sock = SubSocket::create(c, "liveCalibration"); + assert(live_calibration_sock != NULL); + + Poller * poller = Poller::create({live_calibration_sock}); + + /* + import numpy as np + from common.transformations.model import medmodel_frame_from_road_frame + medmodel_frame_from_ground = medmodel_frame_from_road_frame[:, (0, 1, 3)] + ground_from_medmodel_frame = np.linalg.inv(medmodel_frame_from_ground) + */ + Eigen::Matrix ground_from_medmodel_frame; + ground_from_medmodel_frame << + 0.00000000e+00, 0.00000000e+00, 1.00000000e+00, + -1.09890110e-03, 0.00000000e+00, 2.81318681e-01, + -1.84808520e-20, 9.00738606e-04,-4.28751576e-02; + + Eigen::Matrix eon_intrinsics; + eon_intrinsics << + 910.0, 0.0, 582.0, + 0.0, 910.0, 437.0, + 0.0, 0.0, 1.0; + + while (!do_exit) { + for (auto sock : poller->poll(10)){ + Message * msg = sock->receive(); + + auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), msg->getData(), msg->getSize()); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + + if (event.isLiveCalibration()) { + pthread_mutex_lock(&transform_lock); + + auto extrinsic_matrix = event.getLiveCalibration().getExtrinsicMatrix(); + Eigen::Matrix extrinsic_matrix_eigen; + for (int i = 0; i < 4*3; i++){ + extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i]; + } + + auto camera_frame_from_road_frame = eon_intrinsics * extrinsic_matrix_eigen; + Eigen::Matrix camera_frame_from_ground; + camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0); + camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1); + camera_frame_from_ground.col(2) = camera_frame_from_road_frame.col(3); + + auto warp_matrix = camera_frame_from_ground * ground_from_medmodel_frame; + + for (int i=0; i<3*3; i++) { + cur_transform.v[i] = warp_matrix(i / 3, i % 3); + } + + run_model = true; + pthread_mutex_unlock(&transform_lock); + } + + delete msg; + } + + } + + + return NULL; +} + +int main(int argc, char **argv) { + int err; + set_realtime_priority(1); + + // start calibration thread + pthread_t live_thread_handle; + err = pthread_create(&live_thread_handle, NULL, live_thread, NULL); + assert(err == 0); + + // messaging + Context *msg_context = Context::create(); + PubSocket *model_sock = PubSocket::create(msg_context, "model"); + PubSocket *posenet_sock = PubSocket::create(msg_context, "cameraOdometry"); + SubSocket *pathplan_sock = SubSocket::create(msg_context, "pathPlan", "127.0.0.1", true); + + assert(model_sock != NULL); + assert(posenet_sock != NULL); + assert(pathplan_sock != NULL); + + // cl init + cl_device_id device_id; + cl_context context; + cl_command_queue q; + { + // TODO: refactor this + cl_platform_id platform_id[2]; + cl_uint num_devices; + cl_uint num_platforms; + + err = clGetPlatformIDs(sizeof(platform_id)/sizeof(cl_platform_id), platform_id, &num_platforms); + assert(err == 0); + + #ifdef QCOM + int clPlatform = 0; + #else + // don't use nvidia on pc, it's broken + // TODO: write this nicely + int clPlatform = num_platforms-1; + #endif + + char cBuffer[1024]; + clGetPlatformInfo(platform_id[clPlatform], CL_PLATFORM_NAME, sizeof(cBuffer), &cBuffer, NULL); + LOGD("got %d opencl platform(s), using %s", num_platforms, cBuffer); + + err = clGetDeviceIDs(platform_id[clPlatform], CL_DEVICE_TYPE_DEFAULT, 1, + &device_id, &num_devices); + assert(err == 0); + + context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err); + assert(err == 0); + + q = clCreateCommandQueue(context, device_id, 0, &err); + assert(err == 0); + } + + // init the models + ModelState model; + model_init(&model, device_id, context, true); + LOGW("models loaded, modeld starting"); + + // debayering does a 2x downscale + mat3 yuv_transform = transform_scale_buffer((mat3){{ + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + }}, 0.5); + + // loop + VisionStream stream; + while (!do_exit) { + VisionStreamBufs buf_info; + err = visionstream_init(&stream, VISION_STREAM_YUV, true, &buf_info); + if (err) { + printf("visionstream connect fail\n"); + usleep(100000); + continue; + } + LOGW("connected with buffer size: %d", buf_info.buf_len); + + // one frame in memory + cl_mem yuv_cl; + VisionBuf yuv_ion = visionbuf_allocate_cl(buf_info.buf_len, device_id, context, &yuv_cl); + + double last = 0; + int desire = -1; + while (!do_exit) { + VIPCBuf *buf; + VIPCBufExtra extra; + buf = visionstream_get(&stream, &extra); + if (buf == NULL) { + printf("visionstream get failed\n"); + break; + } + + pthread_mutex_lock(&transform_lock); + mat3 transform = cur_transform; + const bool run_model_this_iter = run_model; + pthread_mutex_unlock(&transform_lock); + + Message *msg = pathplan_sock->receive(true); + if (msg != NULL) { + // TODO: copy and pasted from camerad/main.cc + auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), msg->getData(), msg->getSize()); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + + // TODO: path planner timeout? + desire = ((int)event.getPathPlan().getDesire()) - 1; + delete msg; + } + + double mt1 = 0, mt2 = 0; + if (run_model_this_iter) { + float vec_desire[DESIRE_SIZE] = {0}; + if (desire >= 0 && desire < DESIRE_SIZE) { + vec_desire[desire] = 1.0; + } + + mat3 model_transform = matmul3(yuv_transform, transform); + + mt1 = millis_since_boot(); + + // TODO: don't make copies! + memcpy(yuv_ion.addr, buf->addr, buf_info.buf_len); + + ModelDataRaw model_buf = + model_eval_frame(&model, q, yuv_cl, buf_info.width, buf_info.height, + model_transform, NULL, vec_desire); + mt2 = millis_since_boot(); + + model_publish(model_sock, extra.frame_id, model_buf, extra.timestamp_eof); + posenet_publish(posenet_sock, extra.frame_id, model_buf, extra.timestamp_eof); + + LOGD("model process: %.2fms, from last %.2fms", mt2-mt1, mt1-last); + last = mt1; + } + + } + visionbuf_free(&yuv_ion); + } + + visionstream_destroy(&stream); + + delete model_sock; + + model_free(&model); + + LOG("joining live_thread"); + err = pthread_join(live_thread_handle, NULL); + assert(err == 0); + + return 0; +} diff --git a/selfdrive/modeld/models/commonmodel.c b/selfdrive/modeld/models/commonmodel.c new file mode 100644 index 000000000..143480fb0 --- /dev/null +++ b/selfdrive/modeld/models/commonmodel.c @@ -0,0 +1,68 @@ +#include "commonmodel.h" + +#include +#include "cereal/gen/c/log.capnp.h" +#include "common/mat.h" +#include "common/timing.h" + +void frame_init(ModelFrame* frame, int width, int height, + cl_device_id device_id, cl_context context) { + int err; + frame->device_id = device_id; + frame->context = context; + + transform_init(&frame->transform, context, device_id); + frame->transformed_width = width; + frame->transformed_height = height; + + frame->transformed_y_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE, + frame->transformed_width*frame->transformed_height, NULL, &err); + assert(err == 0); + frame->transformed_u_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE, + (frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err); + assert(err == 0); + frame->transformed_v_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE, + (frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err); + assert(err == 0); + + frame->net_input_size = ((width*height*3)/2)*sizeof(float); + frame->net_input = clCreateBuffer(frame->context, CL_MEM_READ_WRITE, + frame->net_input_size, (void*)NULL, &err); + assert(err == 0); + + loadyuv_init(&frame->loadyuv, context, device_id, frame->transformed_width, frame->transformed_height); +} + +float *frame_prepare(ModelFrame* frame, cl_command_queue q, + cl_mem yuv_cl, int width, int height, + mat3 transform) { + int err; + int i = 0; + transform_queue(&frame->transform, q, + yuv_cl, width, height, + frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl, + frame->transformed_width, frame->transformed_height, + transform); + loadyuv_queue(&frame->loadyuv, q, + frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl, + frame->net_input); + float *net_input_buf = (float *)clEnqueueMapBuffer(q, frame->net_input, CL_TRUE, + CL_MAP_READ, 0, frame->net_input_size, + 0, NULL, NULL, &err); + clFinish(q); + return net_input_buf; +} + +void frame_free(ModelFrame* frame) { + transform_destroy(&frame->transform); + loadyuv_destroy(&frame->loadyuv); +} + + +float sigmoid(float input) { + return 1 / (1 + expf(-input)); +} + +float softplus(float input) { + return log1p(expf(input)); +} diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h new file mode 100644 index 000000000..4e3ee448c --- /dev/null +++ b/selfdrive/modeld/models/commonmodel.h @@ -0,0 +1,42 @@ +#ifndef COMMONMODEL_H +#define COMMONMODEL_H + +#include + +#include "common/mat.h" +#include "transforms/transform.h" +#include "transforms/loadyuv.h" + +#ifdef __cplusplus +extern "C" { +#endif + +float softplus(float input); +float sigmoid(float input); + +typedef struct ModelFrame { + cl_device_id device_id; + cl_context context; + + // input + Transform transform; + int transformed_width, transformed_height; + cl_mem transformed_y_cl, transformed_u_cl, transformed_v_cl; + LoadYUVState loadyuv; + cl_mem net_input; + size_t net_input_size; +} ModelFrame; + +void frame_init(ModelFrame* frame, int width, int height, + cl_device_id device_id, cl_context context); +float *frame_prepare(ModelFrame* frame, cl_command_queue q, + cl_mem yuv_cl, int width, int height, + mat3 transform); +void frame_free(ModelFrame* frame); + +#ifdef __cplusplus +} +#endif + +#endif + diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc new file mode 100644 index 000000000..06c820ff7 --- /dev/null +++ b/selfdrive/modeld/models/driving.cc @@ -0,0 +1,287 @@ +#include +#include +#include +#include +#include "common/timing.h" +#include "driving.h" + + +#define PATH_IDX 0 +#define LL_IDX PATH_IDX + MODEL_PATH_DISTANCE*2 +#define RL_IDX LL_IDX + MODEL_PATH_DISTANCE*2 + 1 +#define LEAD_IDX RL_IDX + MODEL_PATH_DISTANCE*2 + 1 +#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION +#define LONG_V_IDX LONG_X_IDX + TIME_DISTANCE*2 +#define LONG_A_IDX LONG_V_IDX + TIME_DISTANCE*2 +#define META_IDX LONG_A_IDX + TIME_DISTANCE*2 +#define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE +#define OUTPUT_SIZE POSE_IDX + POSE_SIZE +#ifdef TEMPORAL + #define TEMPORAL_SIZE 512 +#else + #define TEMPORAL_SIZE 0 +#endif + +// #define DUMP_YUV + +Eigen::Matrix vander; + +void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) { + frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context); + s->input_frames = (float*)calloc(MODEL_FRAME_SIZE * 2, sizeof(float)); + + const int output_size = OUTPUT_SIZE + TEMPORAL_SIZE; + s->output = (float*)calloc(output_size, sizeof(float)); + + s->m = new DefaultRunModel("../../models/supercombo.dlc", s->output, output_size, USE_GPU_RUNTIME); + +#ifdef TEMPORAL + assert(temporal); + s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE); +#endif + +#ifdef DESIRE + s->desire = (float*)malloc(DESIRE_SIZE * sizeof(float)); + for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = 0.0; + s->m->addDesire(s->desire, DESIRE_SIZE); +#endif + + // Build Vandermonde matrix + for(int i = 0; i < MODEL_PATH_DISTANCE; i++) { + for(int j = 0; j < POLYFIT_DEGREE - 1; j++) { + vander(i, j) = pow(i, POLYFIT_DEGREE-j-1); + } + } +} + + + +ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, + cl_mem yuv_cl, int width, int height, + mat3 transform, void* sock, float *desire_in) { +#ifdef DESIRE + if (desire_in != NULL) { + for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = desire_in[i]; + } +#endif + + //for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n"); + + float *new_frame_buf = frame_prepare(&s->frame, q, yuv_cl, width, height, transform); + memmove(&s->input_frames[0], &s->input_frames[MODEL_FRAME_SIZE], sizeof(float)*MODEL_FRAME_SIZE); + memmove(&s->input_frames[MODEL_FRAME_SIZE], new_frame_buf, sizeof(float)*MODEL_FRAME_SIZE); + s->m->execute(s->input_frames); + + #ifdef DUMP_YUV + FILE *dump_yuv_file = fopen("/sdcard/dump.yuv", "wb"); + fwrite(new_frame_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file); + fclose(dump_yuv_file); + assert(1==2); + #endif + + // net outputs + ModelDataRaw net_outputs; + net_outputs.path = &s->output[PATH_IDX]; + net_outputs.left_lane = &s->output[LL_IDX]; + net_outputs.right_lane = &s->output[RL_IDX]; + net_outputs.lead = &s->output[LEAD_IDX]; + net_outputs.long_x = &s->output[LONG_X_IDX]; + net_outputs.long_v = &s->output[LONG_V_IDX]; + net_outputs.long_a = &s->output[LONG_A_IDX]; + net_outputs.meta = &s->output[META_IDX]; + net_outputs.pose = &s->output[POSE_IDX]; + return net_outputs; +} + +void model_free(ModelState* s) { + free(s->output); + free(s->input_frames); + frame_free(&s->frame); + delete s->m; +} + +void poly_fit(float *in_pts, float *in_stds, float *out) { + // References to inputs + Eigen::Map > pts(in_pts, MODEL_PATH_DISTANCE); + Eigen::Map > std(in_stds, MODEL_PATH_DISTANCE); + Eigen::Map > p(out, POLYFIT_DEGREE - 1); + + float y0 = pts[0]; + pts = pts.array() - y0; + + // Build Least Squares equations + Eigen::Matrix lhs = vander.array().colwise() / std.array(); + Eigen::Matrix rhs = pts.array() / std.array(); + + // Improve numerical stability + Eigen::Matrix scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum(); + lhs = lhs * scale.asDiagonal(); + + // Solve inplace + Eigen::ColPivHouseholderQR > qr(lhs); + p = qr.solve(rhs); + + // Apply scale to output + p = p.transpose() * scale.asDiagonal(); + out[3] = y0; +} + + +void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bool has_prob, const float offset) { + float points_arr[MODEL_PATH_DISTANCE]; + float stds_arr[MODEL_PATH_DISTANCE]; + float poly_arr[POLYFIT_DEGREE]; + float std; + float prob; + + for (int i=0; i stds(&stds_arr[0], ARRAYSIZE(stds_arr)); + path.setStds(stds); + + kj::ArrayPtr points(&points_arr[0], ARRAYSIZE(points_arr)); + path.setPoints(points); + } + + kj::ArrayPtr poly(&poly_arr[0], ARRAYSIZE(poly_arr)); + path.setPoly(poly); + path.setProb(prob); + path.setStd(std); +} + +void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, int mdn_max_idx) { + const double x_scale = 10.0; + const double y_scale = 10.0; + + lead.setProb(sigmoid(data[LEAD_MDN_N*MDN_GROUP_SIZE])); + lead.setDist(x_scale * data[mdn_max_idx*MDN_GROUP_SIZE]); + lead.setStd(x_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS])); + lead.setRelY(y_scale * data[mdn_max_idx*MDN_GROUP_SIZE + 1]); + lead.setRelYStd(y_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1])); + lead.setRelVel(data[mdn_max_idx*MDN_GROUP_SIZE + 2]); + lead.setRelVelStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2])); + lead.setRelA(data[mdn_max_idx*MDN_GROUP_SIZE + 3]); + lead.setRelAStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3])); +} + +void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) { + meta.setEngagedProb(meta_data[0]); + meta.setGasDisengageProb(meta_data[1]); + meta.setBrakeDisengageProb(meta_data[2]); + meta.setSteerOverrideProb(meta_data[3]); + kj::ArrayPtr desire_pred(&meta_data[OTHER_META_SIZE], DESIRE_PRED_SIZE); + meta.setDesirePrediction(desire_pred); +} + +void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float * long_v_data, const float * long_a_data) { + // just doing 10 vals, 1 every sec for now + float speed_arr[TIME_DISTANCE/10]; + float accel_arr[TIME_DISTANCE/10]; + for (int i=0; i speed(&speed_arr[0], ARRAYSIZE(speed_arr)); + longi.setSpeeds(speed); + kj::ArrayPtr accel(&accel_arr[0], ARRAYSIZE(accel_arr)); + longi.setAccelerations(accel); +} + +void model_publish(PubSocket *sock, uint32_t frame_id, + const ModelDataRaw net_outputs, uint64_t timestamp_eof) { + // make msg + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto framed = event.initModel(); + framed.setFrameId(frame_id); + framed.setTimestampEof(timestamp_eof); + + auto lpath = framed.initPath(); + fill_path(lpath, net_outputs.path, false, 0); + auto left_lane = framed.initLeftLane(); + fill_path(left_lane, net_outputs.left_lane, true, 1.8); + auto right_lane = framed.initRightLane(); + fill_path(right_lane, net_outputs.right_lane, true, -1.8); + auto longi = framed.initLongitudinal(); + fill_longi(longi, net_outputs.long_v, net_outputs.long_a); + + + // Find the distribution that corresponds to the current lead + int mdn_max_idx = 0; + for (int i=1; i net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) { + mdn_max_idx = i; + } + } + auto lead = framed.initLead(); + fill_lead(lead, net_outputs.lead, mdn_max_idx); + // Find the distribution that corresponds to the lead in 2s + mdn_max_idx = 0; + for (int i=1; i net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 9]) { + mdn_max_idx = i; + } + } + auto lead_future = framed.initLeadFuture(); + fill_lead(lead_future, net_outputs.lead, mdn_max_idx); + + + auto meta = framed.initMeta(); + fill_meta(meta, net_outputs.meta); + + + // send message + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + sock->send((char*)bytes.begin(), bytes.size()); + } + +void posenet_publish(PubSocket *sock, uint32_t frame_id, + const ModelDataRaw net_outputs, uint64_t timestamp_eof) { + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + float trans_arr[3]; + float trans_std_arr[3]; + float rot_arr[3]; + float rot_std_arr[3]; + + for (int i =0; i < 3; i++) { + trans_arr[i] = net_outputs.pose[i]; + trans_std_arr[i] = softplus(net_outputs.pose[6 + i]) + 1e-6; + + rot_arr[i] = M_PI * net_outputs.pose[3 + i] / 180.0; + rot_std_arr[i] = M_PI * (softplus(net_outputs.pose[9 + i]) + 1e-6) / 180.0; + } + + auto posenetd = event.initCameraOdometry(); + kj::ArrayPtr trans_vs(&trans_arr[0], 3); + posenetd.setTrans(trans_vs); + kj::ArrayPtr rot_vs(&rot_arr[0], 3); + posenetd.setRot(rot_vs); + kj::ArrayPtr trans_std_vs(&trans_std_arr[0], 3); + posenetd.setTransStd(trans_std_vs); + kj::ArrayPtr rot_std_vs(&rot_std_arr[0], 3); + posenetd.setRotStd(rot_std_vs); + + posenetd.setTimestampEof(timestamp_eof); + posenetd.setFrameId(frame_id); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + sock->send((char*)bytes.begin(), bytes.size()); + } diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h new file mode 100644 index 000000000..bf46cb9d4 --- /dev/null +++ b/selfdrive/modeld/models/driving.h @@ -0,0 +1,81 @@ +#ifndef MODEL_H +#define MODEL_H + +// gate this here +#define TEMPORAL +#define DESIRE + +#ifdef DESIRE + #define DESIRE_SIZE 8 +#endif + +#ifdef QCOM +#include +#else +#include +#endif + +#include "common/mat.h" +#include "common/util.h" + +#include "commonmodel.h" +#include "runners/run.h" + +#include "cereal/gen/cpp/log.capnp.h" +#include +#include +#include "messaging.hpp" + +#define MODEL_WIDTH 512 +#define MODEL_HEIGHT 256 +#define MODEL_FRAME_SIZE MODEL_WIDTH * MODEL_HEIGHT * 3 / 2 +#define MODEL_NAME "supercombo_dlc" + +#define MODEL_PATH_DISTANCE 192 +#define POLYFIT_DEGREE 4 +#define SPEED_PERCENTILES 10 +#define DESIRE_PRED_SIZE 32 +#define OTHER_META_SIZE 4 +#define LEAD_MDN_N 5 // probs for 5 groups +#define MDN_VALS 4 // output xyva for each lead group +#define SELECTION 3 //output 3 group (lead now, in 2s and 6s) +#define MDN_GROUP_SIZE 11 +#define TIME_DISTANCE 100 +#define POSE_SIZE 12 + +struct ModelDataRaw { + float *path; + float *left_lane; + float *right_lane; + float *lead; + float *long_x; + float *long_v; + float *long_a; + float *meta; + float *pose; + }; + + +typedef struct ModelState { + ModelFrame frame; + float *output; + float *input_frames; + RunModel *m; +#ifdef DESIRE + float *desire; +#endif +} ModelState; + +void model_init(ModelState* s, cl_device_id device_id, + cl_context context, int temporal); +ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, + cl_mem yuv_cl, int width, int height, + mat3 transform, void* sock, float *desire_in); +void model_free(ModelState* s); +void poly_fit(float *in_pts, float *in_stds, float *out); + +void model_publish(PubSocket* sock, uint32_t frame_id, + const ModelDataRaw data, uint64_t timestamp_eof); +void posenet_publish(PubSocket* sock, uint32_t frame_id, + const ModelDataRaw data, uint64_t timestamp_eof); +#endif diff --git a/selfdrive/modeld/models/monitoring.cc b/selfdrive/modeld/models/monitoring.cc new file mode 100644 index 000000000..126f74247 --- /dev/null +++ b/selfdrive/modeld/models/monitoring.cc @@ -0,0 +1,163 @@ +#include +#include "monitoring.h" +#include "common/mat.h" +#include "common/timing.h" + +// #include +#include + +#define MODEL_WIDTH 160 +#define MODEL_HEIGHT 320 +#define FULL_W 426 + +void monitoring_init(MonitoringState* s) { + s->m = new DefaultRunModel("../../models/monitoring_model_q.dlc", (float*)&s->output, OUTPUT_SIZE, USE_DSP_RUNTIME); +} + +MonitoringResult monitoring_eval_frame(MonitoringState* s, void* stream_buf, int width, int height) { + + uint8_t *raw_buf = (uint8_t*) stream_buf; + uint8_t *raw_y_buf = raw_buf; + uint8_t *raw_u_buf = raw_y_buf + (width * height); + uint8_t *raw_v_buf = raw_u_buf + ((width/2) * (height/2)); + + int cropped_width = height/2; + int cropped_height = height; + + int resized_width = MODEL_WIDTH; + int resized_height = MODEL_HEIGHT; + + uint8_t *cropped_buf = new uint8_t[cropped_width*cropped_height*3/2]; + uint8_t *cropped_y_buf = cropped_buf; + uint8_t *cropped_u_buf = cropped_y_buf + (cropped_width * cropped_height); + uint8_t *cropped_v_buf = cropped_u_buf + ((cropped_width/2) * (cropped_height/2)); + + if (true) { + for (int r = 0; r < height/2; r++) { + memcpy(cropped_y_buf + 2*r*cropped_width, raw_y_buf + 2*r*width + (width - cropped_width), cropped_width); + memcpy(cropped_y_buf + (2*r+1)*cropped_width, raw_y_buf + (2*r+1)*width + (width - cropped_width), cropped_width); + memcpy(cropped_u_buf + r*cropped_width/2, raw_u_buf + r*width/2 + ((width/2) - (cropped_width/2)), cropped_width/2); + memcpy(cropped_v_buf + r*cropped_width/2, raw_v_buf + r*width/2 + ((width/2) - (cropped_width/2)), cropped_width/2); + } + } else { + // not tested + uint8_t *premirror_cropped_buf = new uint8_t[cropped_width*cropped_height*3/2]; + uint8_t *premirror_cropped_y_buf = premirror_cropped_buf; + uint8_t *premirror_cropped_u_buf = premirror_cropped_y_buf + (cropped_width * cropped_height); + uint8_t *premirror_cropped_v_buf = premirror_cropped_u_buf + ((cropped_width/2) * (cropped_height/2)); + for (int r = 0; r < height/2; r++) { + memcpy(premirror_cropped_y_buf + 2*r*cropped_width, raw_y_buf + 2*r*width, cropped_width); + memcpy(premirror_cropped_y_buf + (2*r+1)*cropped_width, raw_y_buf + (2*r+1)*width, cropped_width); + memcpy(premirror_cropped_u_buf + r*cropped_width/2, raw_u_buf + r*width/2, cropped_width/2); + memcpy(premirror_cropped_v_buf + r*cropped_width/2, raw_v_buf + r*width/2, cropped_width/2); + } + libyuv::I420Mirror(premirror_cropped_y_buf, cropped_width, + premirror_cropped_u_buf, cropped_width/2, + premirror_cropped_v_buf, cropped_width/2, + cropped_y_buf, cropped_width, + cropped_u_buf, cropped_width/2, + cropped_v_buf, cropped_width/2, + cropped_width, cropped_height); + } + + uint8_t *resized_buf = new uint8_t[resized_width*resized_height*3/2]; + uint8_t *resized_y_buf = resized_buf; + uint8_t *resized_u_buf = resized_y_buf + (resized_width * resized_height); + uint8_t *resized_v_buf = resized_u_buf + ((resized_width/2) * (resized_height/2)); + + libyuv::FilterMode mode = libyuv::FilterModeEnum::kFilterBilinear; + libyuv::I420Scale(cropped_y_buf, cropped_width, + cropped_u_buf, cropped_width/2, + cropped_v_buf, cropped_width/2, + cropped_width, cropped_height, + resized_y_buf, resized_width, + resized_u_buf, resized_width/2, + resized_v_buf, resized_width/2, + resized_width, resized_height, + mode); + + int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v + + float *net_input_buf = new float[yuv_buf_len]; + // one shot conversion, O(n) anyway + // yuvframe2tensor, normalize + 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; + // 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; + // 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; + // 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; + // 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; + // 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; + } + } + + // 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); + + // 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); + + delete[] cropped_buf; + delete[] resized_buf; + s->m->execute(net_input_buf); + delete[] net_input_buf; + + MonitoringResult ret = {0}; + memcpy(&ret.face_orientation, &s->output[0], sizeof ret.face_orientation); + memcpy(&ret.face_orientation_meta, &s->output[6], sizeof ret.face_orientation_meta); + memcpy(&ret.face_position, &s->output[3], sizeof ret.face_position); + memcpy(&ret.face_position_meta, &s->output[9], sizeof ret.face_position_meta); + memcpy(&ret.face_prob, &s->output[12], sizeof ret.face_prob); + memcpy(&ret.left_eye_prob, &s->output[21], sizeof ret.left_eye_prob); + memcpy(&ret.right_eye_prob, &s->output[30], sizeof ret.right_eye_prob); + memcpy(&ret.left_blink_prob, &s->output[31], sizeof ret.right_eye_prob); + memcpy(&ret.right_blink_prob, &s->output[32], sizeof ret.right_eye_prob); + ret.face_orientation_meta[0] = softplus(ret.face_orientation_meta[0]); + ret.face_orientation_meta[1] = softplus(ret.face_orientation_meta[1]); + ret.face_orientation_meta[2] = softplus(ret.face_orientation_meta[2]); + ret.face_position_meta[0] = softplus(ret.face_position_meta[0]); + ret.face_position_meta[1] = softplus(ret.face_position_meta[1]); + return ret; +} + +void monitoring_publish(PubSocket* sock, uint32_t frame_id, const MonitoringResult res) { + // make msg + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto framed = event.initDriverMonitoring(); + framed.setFrameId(frame_id); + + kj::ArrayPtr face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation)); + kj::ArrayPtr face_orientation_std(&res.face_orientation_meta[0], ARRAYSIZE(res.face_orientation_meta)); + kj::ArrayPtr face_position(&res.face_position[0], ARRAYSIZE(res.face_position)); + kj::ArrayPtr face_position_std(&res.face_position_meta[0], ARRAYSIZE(res.face_position_meta)); + framed.setFaceOrientation(face_orientation); + framed.setFaceOrientationStd(face_orientation_std); + framed.setFacePosition(face_position); + framed.setFacePositionStd(face_position_std); + framed.setFaceProb(res.face_prob); + framed.setLeftEyeProb(res.left_eye_prob); + framed.setRightEyeProb(res.right_eye_prob); + framed.setLeftBlinkProb(res.left_blink_prob); + framed.setRightBlinkProb(res.right_blink_prob); + + // send message + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + sock->send((char*)bytes.begin(), bytes.size()); +} + +void monitoring_free(MonitoringState* s) { + delete s->m; +} diff --git a/selfdrive/modeld/models/monitoring.h b/selfdrive/modeld/models/monitoring.h new file mode 100644 index 000000000..009ca64f0 --- /dev/null +++ b/selfdrive/modeld/models/monitoring.h @@ -0,0 +1,44 @@ +#ifndef MONITORING_H +#define MONITORING_H + +#include "common/util.h" +#include "commonmodel.h" +#include "runners/run.h" + +#include "cereal/gen/cpp/log.capnp.h" +#include +#include "messaging.hpp" + +#ifdef __cplusplus +extern "C" { +#endif + +#define OUTPUT_SIZE 33 + +typedef struct MonitoringResult { + float face_orientation[3]; + float face_orientation_meta[3]; + float face_position[2]; + float face_position_meta[2]; + float face_prob; + float left_eye_prob; + float right_eye_prob; + float left_blink_prob; + float right_blink_prob; +} MonitoringResult; + +typedef struct MonitoringState { + RunModel *m; + float output[OUTPUT_SIZE]; +} MonitoringState; + +void monitoring_init(MonitoringState* s); +MonitoringResult monitoring_eval_frame(MonitoringState* s, void* stream_buf, int width, int height); +void monitoring_publish(PubSocket *sock, uint32_t frame_id, const MonitoringResult res); +void monitoring_free(MonitoringState* s); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/selfdrive/modeld/monitoringd b/selfdrive/modeld/monitoringd new file mode 100755 index 000000000..419d82633 --- /dev/null +++ b/selfdrive/modeld/monitoringd @@ -0,0 +1,5 @@ +#!/bin/sh +export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8:/home/batman/one/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH" +export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/" +exec ./_monitoringd + diff --git a/selfdrive/modeld/monitoringd.cc b/selfdrive/modeld/monitoringd.cc new file mode 100644 index 000000000..98863ec9f --- /dev/null +++ b/selfdrive/modeld/monitoringd.cc @@ -0,0 +1,80 @@ +#include +#include +#include +#include +#include + +#include "common/visionbuf.h" +#include "common/visionipc.h" +#include "common/swaglog.h" + +#include "models/monitoring.h" + +#ifndef PATH_MAX +#include +#endif + + +volatile sig_atomic_t do_exit = 0; + +static void set_do_exit(int sig) { + do_exit = 1; +} + +int main(int argc, char **argv) { + int err; + set_realtime_priority(1); + + // messaging + Context *msg_context = Context::create(); + PubSocket *monitoring_sock = PubSocket::create(msg_context, "driverMonitoring"); + + // init the models + MonitoringState monitoring; + monitoring_init(&monitoring); + + // loop + VisionStream stream; + while (!do_exit) { + VisionStreamBufs buf_info; + err = visionstream_init(&stream, VISION_STREAM_YUV_FRONT, true, &buf_info); + if (err) { + printf("visionstream connect fail\n"); + usleep(100000); + continue; + } + LOGW("connected with buffer size: %d", buf_info.buf_len); + + double last = 0; + while (!do_exit) { + VIPCBuf *buf; + VIPCBufExtra extra; + buf = visionstream_get(&stream, &extra); + if (buf == NULL) { + printf("visionstream get failed\n"); + break; + } + //printf("frame_id: %d %dx%d\n", extra.frame_id, buf_info.width, buf_info.height); + + double t1 = millis_since_boot(); + + MonitoringResult res = monitoring_eval_frame(&monitoring, buf->addr, buf_info.width, buf_info.height); + + double t2 = millis_since_boot(); + + // send dm packet + monitoring_publish(monitoring_sock, extra.frame_id, res); + + LOGD("monitoring process: %.2fms, from last %.2fms", t2-t1, t1-last); + last = t1; + } + + } + + visionstream_destroy(&stream); + + delete monitoring_sock; + monitoring_free(&monitoring); + + return 0; +} diff --git a/selfdrive/modeld/runners/run.h b/selfdrive/modeld/runners/run.h new file mode 100644 index 000000000..56e785397 --- /dev/null +++ b/selfdrive/modeld/runners/run.h @@ -0,0 +1,18 @@ +#ifndef RUN_H +#define RUN_H + +#include "runmodel.h" +#include "snpemodel.h" + +#ifdef QCOM + #define DefaultRunModel SNPEModel +#else + #ifdef USE_TF_MODEL + #include "tfmodel.h" + #define DefaultRunModel TFModel + #else + #define DefaultRunModel SNPEModel + #endif +#endif + +#endif diff --git a/selfdrive/modeld/runners/runmodel.h b/selfdrive/modeld/runners/runmodel.h new file mode 100644 index 000000000..d1563a61a --- /dev/null +++ b/selfdrive/modeld/runners/runmodel.h @@ -0,0 +1,12 @@ +#ifndef RUNMODEL_H +#define RUNMODEL_H + +class RunModel { +public: + virtual void addRecurrent(float *state, int state_size) {} + virtual void addDesire(float *state, int state_size) {} + virtual void execute(float *net_input_buf) {} +}; + +#endif + diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc new file mode 100644 index 000000000..257f203d7 --- /dev/null +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -0,0 +1,126 @@ +#include +#include +#include "common/util.h" +#include "snpemodel.h" + +void PrintErrorStringAndExit() { + const char* const errStr = zdl::DlSystem::getLastErrorString(); + std::cerr << zdl::DlSystem::getLastErrorString() << std::endl; + std::exit(EXIT_FAILURE); +} + +SNPEModel::SNPEModel(const char *path, float *output, size_t output_size, int runtime) { +#ifdef QCOM + zdl::DlSystem::Runtime_t Runtime; + if (runtime==USE_GPU_RUNTIME) { + Runtime = zdl::DlSystem::Runtime_t::GPU; + } else if (runtime==USE_DSP_RUNTIME) { + Runtime = zdl::DlSystem::Runtime_t::DSP; + } else { + Runtime = zdl::DlSystem::Runtime_t::CPU; + } + assert(zdl::SNPE::SNPEFactory::isRuntimeAvailable(Runtime)); +#endif + size_t model_size; + model_data = (uint8_t *)read_file(path, &model_size); + assert(model_data); + + // load model + std::unique_ptr container = zdl::DlContainer::IDlContainer::open(model_data, model_size); + if (!container) { PrintErrorStringAndExit(); } + printf("loaded model with size: %u\n", model_size); + + // create model runner + zdl::SNPE::SNPEBuilder snpeBuilder(container.get()); + while (!snpe) { +#ifdef QCOM + snpe = snpeBuilder.setOutputLayers({}) + .setRuntimeProcessor(Runtime) + .setUseUserSuppliedBuffers(true) + .setPerformanceProfile(zdl::DlSystem::PerformanceProfile_t::HIGH_PERFORMANCE) + .build(); +#else + snpe = snpeBuilder.setOutputLayers({}) + .setUseUserSuppliedBuffers(true) + .setPerformanceProfile(zdl::DlSystem::PerformanceProfile_t::HIGH_PERFORMANCE) + .build(); +#endif + if (!snpe) std::cerr << zdl::DlSystem::getLastErrorString() << std::endl; + } + + // get input and output names + const auto &strListi_opt = snpe->getInputTensorNames(); + if (!strListi_opt) throw std::runtime_error("Error obtaining Input tensor names"); + const auto &strListi = *strListi_opt; + //assert(strListi.size() == 1); + const char *input_tensor_name = strListi.at(0); + + const auto &strListo_opt = snpe->getOutputTensorNames(); + if (!strListo_opt) throw std::runtime_error("Error obtaining Output tensor names"); + const auto &strListo = *strListo_opt; + assert(strListo.size() == 1); + const char *output_tensor_name = strListo.at(0); + + printf("model: %s -> %s\n", input_tensor_name, output_tensor_name); + + zdl::DlSystem::UserBufferEncodingFloat userBufferEncodingFloat; + zdl::DlSystem::IUserBufferFactory& ubFactory = zdl::SNPE::SNPEFactory::getUserBufferFactory(); + + // create input buffer + { + const auto &inputDims_opt = snpe->getInputDimensions(input_tensor_name); + const zdl::DlSystem::TensorShape& bufferShape = *inputDims_opt; + std::vector strides(bufferShape.rank()); + strides[strides.size() - 1] = sizeof(float); + size_t product = 1; + for (size_t i = 0; i < bufferShape.rank(); i++) product *= bufferShape[i]; + size_t stride = strides[strides.size() - 1]; + for (size_t i = bufferShape.rank() - 1; i > 0; i--) { + stride *= bufferShape[i]; + strides[i-1] = stride; + } + printf("input product is %u\n", product); + inputBuffer = ubFactory.createUserBuffer(NULL, product*sizeof(float), strides, &userBufferEncodingFloat); + + inputMap.add(input_tensor_name, inputBuffer.get()); + } + + // create output buffer + { + std::vector outputStrides = {output_size * sizeof(float), sizeof(float)}; + outputBuffer = ubFactory.createUserBuffer(output, output_size * sizeof(float), outputStrides, &userBufferEncodingFloat); + outputMap.add(output_tensor_name, outputBuffer.get()); + } +} + +void SNPEModel::addRecurrent(float *state, int state_size) { + recurrentBuffer = this->addExtra(state, state_size, 2); +} + +void SNPEModel::addDesire(float *state, int state_size) { + desireBuffer = this->addExtra(state, state_size, 1); +} + +std::unique_ptr SNPEModel::addExtra(float *state, int state_size, int idx) { + // get input and output names + const auto &strListi_opt = snpe->getInputTensorNames(); + if (!strListi_opt) throw std::runtime_error("Error obtaining Input tensor names"); + const auto &strListi = *strListi_opt; + const char *input_tensor_name = strListi.at(idx); + printf("adding index %d: %s\n", idx, input_tensor_name); + + zdl::DlSystem::UserBufferEncodingFloat userBufferEncodingFloat; + zdl::DlSystem::IUserBufferFactory& ubFactory = zdl::SNPE::SNPEFactory::getUserBufferFactory(); + std::vector retStrides = {state_size * sizeof(float), sizeof(float)}; + auto ret = ubFactory.createUserBuffer(state, state_size * sizeof(float), retStrides, &userBufferEncodingFloat); + inputMap.add(input_tensor_name, ret.get()); + return ret; +} + +void SNPEModel::execute(float *net_input_buf) { + assert(inputBuffer->setBufferAddress(net_input_buf)); + if (!snpe->execute(inputMap, outputMap)) { + PrintErrorStringAndExit(); + } +} + diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h new file mode 100644 index 000000000..f750baaf1 --- /dev/null +++ b/selfdrive/modeld/runners/snpemodel.h @@ -0,0 +1,51 @@ +#ifndef SNPEMODEL_H +#define SNPEMODEL_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "runmodel.h" + +#define USE_CPU_RUNTIME 0 +#define USE_GPU_RUNTIME 1 +#define USE_DSP_RUNTIME 2 + +class SNPEModel : public RunModel { +public: + SNPEModel(const char *path, float *output, size_t output_size, int runtime); + ~SNPEModel() { + if (model_data) free(model_data); + } + void addRecurrent(float *state, int state_size); + void addDesire(float *state, int state_size); + void execute(float *net_input_buf); +private: + uint8_t *model_data = NULL; + + // snpe model stuff + std::unique_ptr snpe; + + // snpe input stuff + zdl::DlSystem::UserBufferMap inputMap; + std::unique_ptr inputBuffer; + + // snpe output stuff + zdl::DlSystem::UserBufferMap outputMap; + std::unique_ptr outputBuffer; + float *output; + + // recurrent and desire + std::unique_ptr addExtra(float *state, int state_size, int idx); + std::unique_ptr recurrentBuffer; + std::unique_ptr desireBuffer; +}; + +#endif + diff --git a/selfdrive/modeld/runners/tfmodel.cc b/selfdrive/modeld/runners/tfmodel.cc new file mode 100644 index 000000000..2bc9c464b --- /dev/null +++ b/selfdrive/modeld/runners/tfmodel.cc @@ -0,0 +1,160 @@ +#include "tfmodel.h" +#include +#include +#include +#include +#include "common/util.h" +#include "common/swaglog.h" +#include + +void TFModel::status_check() const { + if (TF_GetCode(this->status) != TF_OK) { + throw std::runtime_error(TF_Message(status)); + } +} + +TF_Tensor *TFModel::allocate_tensor_for_output(TF_Output out, float *dat) { + int num_dims = TF_GraphGetTensorNumDims(graph, out, status); + status_check(); + int64_t *dims = new int64_t[num_dims]; + TF_GraphGetTensorShape(graph, out, dims, num_dims, status); + status_check(); + dims[0] = 1; + + int total = 1; + for (int i = 0; i < num_dims; i++) total *= dims[i]; + //printf("dims %d total %d wdat %p\n", num_dims, total, dat); + + // don't deallocate the buffers + auto d = [](void* ddata, size_t, void* arg) {}; + TF_Tensor *ret = TF_NewTensor(TF_FLOAT, dims, num_dims, (void*)dat, sizeof(float)*total, d, NULL); + + //TF_Tensor *ret = TF_AllocateTensor(TF_FLOAT, dims, num_dims, sizeof(float)*total); + //memcpy(TF_TensorData(ret), dat, sizeof(float)*total); + + assert(ret); + delete[] dims; + + return ret; +} + +TFModel::TFModel(const char *path, float *_output, size_t _output_size, int runtime) { + // load model + { + TF_Buffer* buf; + size_t model_size; + char tmp[1024]; + snprintf(tmp, sizeof(tmp), "%s.pb", path); + LOGD("loading model %s", tmp); + uint8_t *model_data = (uint8_t *)read_file(tmp, &model_size); + assert(model_data); + buf = TF_NewBuffer(); + buf->data = model_data; + buf->length = model_size; + buf->data_deallocator = [](void *data, size_t) { free(data); }; + LOGD("loaded model of size %d", model_size); + + // import graph + status = TF_NewStatus(); + graph = TF_NewGraph(); + TF_ImportGraphDefOptions *opts = TF_NewImportGraphDefOptions(); + // TODO: fix the GPU, currently it hangs if you set this to /gpu:0 + //TF_ImportGraphDefOptionsSetDefaultDevice(opts, "/cpu:0"); + TF_GraphImportGraphDef(graph, buf, opts, status); + TF_DeleteImportGraphDefOptions(opts); + TF_DeleteBuffer(buf); + status_check(); + LOGD("imported graph"); + } + + // set up session + TF_SessionOptions* sess_opts = TF_NewSessionOptions(); + + // don't use all GPU memory + /*uint8_t config[15] = {0x32, 0xb, 0x9, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x20, 0x1, 0x38, 0x1}; + double gpu_memory_fraction = 0.2; + auto bytes = reinterpret_cast(&gpu_memory_fraction); + for (std::size_t i = 0; i < sizeof(gpu_memory_fraction); ++i) { + config[i + 3] = bytes[i]; + } + TF_SetConfig(sess_opts, config, sizeof(config), status); + status_check();*/ + + // make session + session = TF_NewSession(graph, sess_opts, status); + TF_DeleteSessionOptions(sess_opts); + status_check(); + + // find tensors + // TODO: make this generic + input_operation = {TF_GraphOperationByName(graph, "lambda/div"), 0}; + if (input_operation.oper == NULL) { + input_operation = {TF_GraphOperationByName(graph, "vision_lambda/div"), 0}; + } + assert(input_operation.oper != NULL); + + output_operation = {TF_GraphOperationByName(graph, "outputs/outputs/Identity"), 0}; + if (output_operation.oper == NULL) { + output_operation = {TF_GraphOperationByName(graph, "outputs/concat"), 0}; + } + assert(output_operation.oper != NULL); + + // output tensor is good to bind now + output = _output; + output_size = _output_size; +} + +TFModel::~TFModel() { + TF_DeleteSession(session, status); + status_check(); + TF_DeleteGraph(graph); + TF_DeleteStatus(status); +} + +void TFModel::addRecurrent(float *state, int state_size) { + rnn_operation.oper = TF_GraphOperationByName(graph, "rnn_state"); + rnn_operation.index = 0; + assert(rnn_operation.oper != NULL); + + rnn_input_buf = state; +} + +void TFModel::addDesire(float *state, int state_size) { + desire_operation.oper = TF_GraphOperationByName(graph, "desire"); + desire_operation.index = 0; + assert(desire_operation.oper != NULL); + + desire_input_buf = state; +} + +void TFModel::execute(float *net_input_buf) { + TF_Tensor *input_tensor = allocate_tensor_for_output(input_operation, net_input_buf); + assert(input_tensor); + TF_Tensor *output_tensor = NULL; + + if (rnn_input_buf == NULL) { + TF_SessionRun(session, NULL, + &input_operation, &input_tensor, 1, + &output_operation, &output_tensor, 1, + NULL, 0, NULL, status); + } else { + //printf("%f %f %f\n", net_input_buf[0], rnn_input_buf[0], desire_input_buf[0]); + TF_Tensor *rnn_tensor = allocate_tensor_for_output(rnn_operation, rnn_input_buf); + TF_Tensor *desire_tensor = allocate_tensor_for_output(desire_operation, desire_input_buf); + TF_Output io[] = {input_operation, rnn_operation, desire_operation}; + TF_Tensor* id[] = {input_tensor, rnn_tensor, desire_tensor}; + TF_SessionRun(session, NULL, + io, id, 3, + &output_operation, &output_tensor, 1, + NULL, 0, NULL, status); + TF_DeleteTensor(rnn_tensor); + TF_DeleteTensor(desire_tensor); + } + TF_DeleteTensor(input_tensor); + status_check(); + assert(output_tensor); + memcpy((void*)output, TF_TensorData(output_tensor), output_size*sizeof(float)); + TF_DeleteTensor(output_tensor); +} + + diff --git a/selfdrive/modeld/runners/tfmodel.h b/selfdrive/modeld/runners/tfmodel.h new file mode 100644 index 000000000..c8f46f2fe --- /dev/null +++ b/selfdrive/modeld/runners/tfmodel.h @@ -0,0 +1,39 @@ +#ifndef TFMODEL_H +#define TFMODEL_H + +#include +#include "runmodel.h" + +#include "tensorflow/c/c_api.h" + +struct TFState; + +class TFModel : public RunModel { +public: + TFModel(const char *path, float *output, size_t output_size, int runtime); + ~TFModel(); + void addRecurrent(float *state, int state_size); + void addDesire(float *state, int state_size); + void execute(float *net_input_buf); +private: + void status_check() const; + TF_Tensor *allocate_tensor_for_output(TF_Output out, float *dat); + + float *output; + size_t output_size; + + TF_Session* session; + TF_Graph* graph; + TF_Status* status; + + TF_Output input_operation; + TF_Output rnn_operation; + TF_Output desire_operation; + TF_Output output_operation; + + float *rnn_input_buf = NULL; + float *desire_input_buf = NULL; +}; + +#endif + diff --git a/selfdrive/modeld/test/polyfit/.gitignore b/selfdrive/modeld/test/polyfit/.gitignore new file mode 100644 index 000000000..ba2906d06 --- /dev/null +++ b/selfdrive/modeld/test/polyfit/.gitignore @@ -0,0 +1 @@ +main diff --git a/selfdrive/modeld/test/polyfit/Makefile b/selfdrive/modeld/test/polyfit/Makefile new file mode 100644 index 000000000..1b8ce740e --- /dev/null +++ b/selfdrive/modeld/test/polyfit/Makefile @@ -0,0 +1,15 @@ +PHONELIBS = ../../../../phonelibs + +EIGEN_FLAGS = -I$(PHONELIBS)/eigen + +CXXFLAGS += $(EIGEN_FLAGS) +LDFLAGS += -lm + +.PHONY: clean + +main: main.cc data.h + g++ -O2 $(EIGEN_FLAGS) -o main main.cc -lm + + +clean: + rm -f main diff --git a/selfdrive/modeld/test/polyfit/data.h b/selfdrive/modeld/test/polyfit/data.h new file mode 100644 index 000000000..0eaedf29e --- /dev/null +++ b/selfdrive/modeld/test/polyfit/data.h @@ -0,0 +1,8 @@ +#pragma once + +#define MODEL_PATH_DISTANCE 192 +#define POLYFIT_DEGREE 4 + +float pts[MODEL_PATH_DISTANCE] = {3.1261718, 3.1642578, 3.0548828, 3.1125, 3.190625, 3.01875, 2.9816406, 3.1222656, 2.9728515, 2.9826171, 3.034375, 3.0392578, 3.1642578, 3.0792968, 3.0011718, 3.0705078, 2.9904296, 3.0089843, 3.0597656, 3.0978515, 2.9210937, 2.9992187, 2.9474609, 2.9621093, 2.9289062, 2.89375, 2.7975585, 2.9015625, 2.8175781, 2.9132812, 2.8175781, 2.7501953, 2.8332031, 2.8166015, 2.7638671, 2.8878906, 2.7599609, 2.6999023, 2.6720703, 2.6398437, 2.7243164, 2.6120117, 2.6588867, 2.5558593, 2.5978515, 2.5485351, 2.4269531, 2.5001953, 2.4855468, 2.4367187, 2.2973144, 2.2812011, 2.2890136, 2.39375, 2.2836425, 2.3815429, 2.2138183, 2.1964843, 2.1840332, 2.1759765, 2.0421875, 2.1034667, 2.0281494, 2.0880859, 1.9706542, 1.9276855, 1.8522155, 1.8991821, 1.7780273, 1.8180053, 1.8326843, 1.8270385, 1.7182128, 1.6439941, 1.5360839, 1.68385, 1.4584472, 1.5955322, 1.6002929, 1.4157226, 1.4704101, 1.2936523, 1.2990234, 1.4281738, 1.4357421, 1.409375, 1.2511718, 1.2194335, 1.1554687, 1.043164, 1.0954101, 1.0392578, 1.0895507, 1.0880859, 0.897168, 0.83369142, 0.86494142, 0.87763673, 0.85322267, 0.72968751, 0.57832032, 0.73066407, 0.78828126, 0.69160157, 0.64375, 0.5919922, 0.5529297, 0.52070314, 0.60957032, 0.51093751, 0.3576172, 0.49921876, 0.284375, 0.21992187, 0.25214845, 0.30683595, 0.30976564, 0.2716797, 0.22089843, 0.25507814, 0.084179685, 0.071484372, 0.1828125, 0.15644531, 0.13789062, 0.054882813, 0.021679688, -0.091601565, -0.0203125, -0.13359375, -0.037890624, -0.29765624, -0.15605469, -0.30351561, -0.055468749, -0.22148438, -0.246875, -0.31718749, -0.25468749, -0.35234374, -0.16484375, -0.56523436, -0.56523436, -0.39921874, -0.58671874, -0.45585936, -0.50859374, -0.44023436, -0.42656249, -0.56328124, -0.70195311, -0.403125, -0.76445311, -0.98710936, -0.7625, -0.75273436, -0.825, -0.996875, -0.86210936, -0.99492186, -0.85625, -0.88359374, -0.97148436, -1.0320313, -1.1609375, -1.1296875, -1.0203125, -1.0691407, -1.2371094, -1.1277344, -1.2214844, -1.1921875, -1.2996094, -1.2917969, -1.3699219, -1.434375, -1.3699219, -1.3601563, -1.5730469, -1.3152344, -1.4851563, -1.48125, -1.5925782, -1.746875, -1.5847657, -1.6003907, -1.5984375, -1.7703125, -1.8328125, -1.8152344, -1.9714844, -1.9421875}; + +float stds[MODEL_PATH_DISTANCE] = {1.0945262, 1.156862, 1.0777057, 1.1501777, 1.234844, 1.0140595, 1.2004665, 1.1926303, 1.1269455, 1.0362904, 0.98873031, 0.88530254, 1.0078473, 0.93637651, 0.90959895, 0.86409503, 0.86353016, 0.74534553, 0.78025728, 0.88014913, 0.75756663, 0.77129823, 0.75581717, 0.79222, 0.84098673, 0.79402477, 0.85648865, 0.80315614, 0.77346581, 0.73097658, 0.72557795, 0.72930044, 0.666103, 0.77142948, 0.704379, 0.6806078, 0.67680347, 0.71318036, 0.72244918, 0.66123307, 0.62547487, 0.67786956, 0.68404138, 0.70508122, 0.62400025, 0.72325015, 0.73942852, 0.67811751, 0.70370805, 0.65040058, 0.6870054, 0.66093785, 0.666103, 0.70040214, 0.65300173, 0.69714534, 0.65825552, 0.64833081, 0.6464982, 0.75850725, 0.69627059, 0.71659386, 0.69307244, 0.61554217, 0.62015557, 0.61998636, 0.67650336, 0.68142927, 0.6278621, 0.612294, 0.62592906, 0.63736153, 0.74233508, 0.69297016, 0.69621509, 0.67229682, 0.64879686, 0.72361159, 0.70229048, 0.60928106, 0.62712252, 0.66923952, 0.65802008, 0.68361813, 0.61587888, 0.63348651, 0.60727841, 0.64873856, 0.68847752, 0.58432156, 0.61683363, 0.63311422, 0.64981711, 0.57369792, 0.62604266, 0.62162364, 0.62066346, 0.62808979, 0.58524042, 0.63537884, 0.65367514, 0.63900274, 0.61089778, 0.62513435, 0.6470505, 0.63952166, 0.5937764, 0.64310449, 0.64330715, 0.64322031, 0.64632386, 0.60827911, 0.58887208, 0.61959165, 0.70725286, 0.64287293, 0.62326396, 0.65896219, 0.55610275, 0.6658656, 0.65681434, 0.583188, 0.6311124, 0.559652, 0.71419227, 0.62490743, 0.66699386, 0.62032485, 0.663036, 0.61414057, 0.66179425, 0.59399503, 0.65203643, 0.67839557, 0.63698763, 0.617452, 0.61022842, 0.7398752, 0.65657932, 0.68718743, 0.67901206, 0.66126263, 0.69949967, 0.70709819, 0.713336, 0.68130863, 0.68652785, 0.67028236, 0.7626031, 0.65259206, 0.72977453, 0.66049516, 0.64261246, 0.66906089, 0.69762796, 0.73719794, 0.69081914, 0.69849437, 0.72435051, 0.62354708, 0.68812829, 0.7193296, 0.66211933, 0.69278532, 0.7518425, 0.69661695, 0.672491, 0.71539241, 0.7369433, 0.66120356, 0.79088491, 0.77491313, 0.79442614, 0.7878198, 0.78881842, 0.70690477, 0.80707121, 0.78768665, 0.7215547, 0.75226194, 0.72196257, 0.765799, 0.77267712, 0.75844234, 0.81038833, 0.81188059, 0.79864907, 0.816436, 0.845298, 0.85074174, 0.73668873, 0.83516812}; diff --git a/selfdrive/modeld/test/polyfit/main.cc b/selfdrive/modeld/test/polyfit/main.cc new file mode 100644 index 000000000..b649ee9ca --- /dev/null +++ b/selfdrive/modeld/test/polyfit/main.cc @@ -0,0 +1,52 @@ +#include +#include + +#include + +#include "data.h" + +Eigen::Matrix vander; + +void poly_init(){ + // Build Vandermonde matrix + for(int i = 0; i < MODEL_PATH_DISTANCE; i++) { + for(int j = 0; j < POLYFIT_DEGREE; j++) { + vander(i, j) = pow(i, POLYFIT_DEGREE-j-1); + } + } +} + +void poly_fit(float *in_pts, float *in_stds, float *out) { + // References to inputs + Eigen::Map > pts(in_pts, MODEL_PATH_DISTANCE); + Eigen::Map > std(in_stds, MODEL_PATH_DISTANCE); + Eigen::Map > p(out, POLYFIT_DEGREE); + + // Build Least Squares equations + Eigen::Matrix lhs = vander.array().colwise() / std.array(); + Eigen::Matrix rhs = pts.array() / std.array(); + + Eigen::Matrix scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum(); + lhs = lhs * scale.asDiagonal(); + + // Solve inplace + Eigen::ColPivHouseholderQR > qr(lhs); + p = qr.solve(rhs); + + p = p.transpose() * scale.asDiagonal(); +} + +int main(void) { + poly_init(); + + + float poly[4]; + poly_fit(pts, stds, poly); + + std::cout << "["; + std::cout << poly[0] << ","; + std::cout << poly[1] << ","; + std::cout << poly[2] << ","; + std::cout << poly[3]; + std::cout << "]" << std::endl; +} diff --git a/selfdrive/modeld/test/polyfit/test.py b/selfdrive/modeld/test/polyfit/test.py new file mode 100644 index 000000000..0caadf352 --- /dev/null +++ b/selfdrive/modeld/test/polyfit/test.py @@ -0,0 +1,24 @@ +import numpy as np + +pts = np.array([3.1261718, 3.1642578, 3.0548828, 3.1125, 3.190625, 3.01875, 2.9816406, 3.1222656, 2.9728515, 2.9826171, 3.034375, 3.0392578, 3.1642578, 3.0792968, 3.0011718, 3.0705078, 2.9904296, 3.0089843, 3.0597656, 3.0978515, 2.9210937, 2.9992187, 2.9474609, 2.9621093, 2.9289062, 2.89375, 2.7975585, 2.9015625, 2.8175781, 2.9132812, 2.8175781, 2.7501953, 2.8332031, 2.8166015, 2.7638671, 2.8878906, 2.7599609, 2.6999023, 2.6720703, 2.6398437, 2.7243164, 2.6120117, 2.6588867, 2.5558593, 2.5978515, 2.5485351, 2.4269531, 2.5001953, 2.4855468, 2.4367187, 2.2973144, 2.2812011, 2.2890136, 2.39375, 2.2836425, 2.3815429, 2.2138183, 2.1964843, 2.1840332, 2.1759765, 2.0421875, 2.1034667, 2.0281494, 2.0880859, 1.9706542, 1.9276855, 1.8522155, 1.8991821, 1.7780273, 1.8180053, 1.8326843, 1.8270385, 1.7182128, 1.6439941, 1.5360839, 1.68385, 1.4584472, 1.5955322, 1.6002929, 1.4157226, 1.4704101, 1.2936523, 1.2990234, 1.4281738, 1.4357421, 1.409375, 1.2511718, 1.2194335, 1.1554687, 1.043164, 1.0954101, 1.0392578, 1.0895507, 1.0880859, 0.897168, 0.83369142, 0.86494142, 0.87763673, 0.85322267, 0.72968751, 0.57832032, 0.73066407, 0.78828126, 0.69160157, 0.64375, 0.5919922, 0.5529297, 0.52070314, 0.60957032, 0.51093751, 0.3576172, 0.49921876, 0.284375, 0.21992187, 0.25214845, 0.30683595, 0.30976564, 0.2716797, 0.22089843, 0.25507814, 0.084179685, 0.071484372, 0.1828125, 0.15644531, 0.13789062, 0.054882813, 0.021679688, -0.091601565, -0.0203125, -0.13359375, -0.037890624, -0.29765624, -0.15605469, -0.30351561, -0.055468749, -0.22148438, -0.246875, -0.31718749, -0.25468749, -0.35234374, -0.16484375, -0.56523436, -0.56523436, -0.39921874, -0.58671874, -0.45585936, -0.50859374, -0.44023436, -0.42656249, -0.56328124, -0.70195311, -0.403125, -0.76445311, -0.98710936, -0.7625, -0.75273436, -0.825, -0.996875, -0.86210936, -0.99492186, -0.85625, -0.88359374, -0.97148436, -1.0320313, -1.1609375, -1.1296875, -1.0203125, -1.0691407, -1.2371094, -1.1277344, -1.2214844, -1.1921875, -1.2996094, -1.2917969, -1.3699219, -1.434375, -1.3699219, -1.3601563, -1.5730469, -1.3152344, -1.4851563, -1.48125, -1.5925782, -1.746875, -1.5847657, -1.6003907, -1.5984375, -1.7703125, -1.8328125, -1.8152344, -1.9714844, -1.9421875]) + +stds = np.array([1.0945262, 1.156862, 1.0777057, 1.1501777, 1.234844, 1.0140595, 1.2004665, 1.1926303, 1.1269455, 1.0362904, 0.98873031, 0.88530254, 1.0078473, 0.93637651, 0.90959895, 0.86409503, 0.86353016, 0.74534553, 0.78025728, 0.88014913, 0.75756663, 0.77129823, 0.75581717, 0.79222, 0.84098673, 0.79402477, 0.85648865, 0.80315614, 0.77346581, 0.73097658, 0.72557795, 0.72930044, 0.666103, 0.77142948, 0.704379, 0.6806078, 0.67680347, 0.71318036, 0.72244918, 0.66123307, 0.62547487, 0.67786956, 0.68404138, 0.70508122, 0.62400025, 0.72325015, 0.73942852, 0.67811751, 0.70370805, 0.65040058, 0.6870054, 0.66093785, 0.666103, 0.70040214, 0.65300173, 0.69714534, 0.65825552, 0.64833081, 0.6464982, 0.75850725, 0.69627059, 0.71659386, 0.69307244, 0.61554217, 0.62015557, 0.61998636, 0.67650336, 0.68142927, 0.6278621, 0.612294, 0.62592906, 0.63736153, 0.74233508, 0.69297016, 0.69621509, 0.67229682, 0.64879686, 0.72361159, 0.70229048, 0.60928106, 0.62712252, 0.66923952, 0.65802008, 0.68361813, 0.61587888, 0.63348651, 0.60727841, 0.64873856, 0.68847752, 0.58432156, 0.61683363, 0.63311422, 0.64981711, 0.57369792, 0.62604266, 0.62162364, 0.62066346, 0.62808979, 0.58524042, 0.63537884, 0.65367514, 0.63900274, 0.61089778, 0.62513435, 0.6470505, 0.63952166, 0.5937764, 0.64310449, 0.64330715, 0.64322031, 0.64632386, 0.60827911, 0.58887208, 0.61959165, 0.70725286, 0.64287293, 0.62326396, 0.65896219, 0.55610275, 0.6658656, 0.65681434, 0.583188, 0.6311124, 0.559652, 0.71419227, 0.62490743, 0.66699386, 0.62032485, 0.663036, 0.61414057, 0.66179425, 0.59399503, 0.65203643, 0.67839557, 0.63698763, 0.617452, 0.61022842, 0.7398752, 0.65657932, 0.68718743, 0.67901206, 0.66126263, 0.69949967, 0.70709819, 0.713336, 0.68130863, 0.68652785, 0.67028236, 0.7626031, 0.65259206, 0.72977453, 0.66049516, 0.64261246, 0.66906089, 0.69762796, 0.73719794, 0.69081914, 0.69849437, 0.72435051, 0.62354708, 0.68812829, 0.7193296, 0.66211933, 0.69278532, 0.7518425, 0.69661695, 0.672491, 0.71539241, 0.7369433, 0.66120356, 0.79088491, 0.77491313, 0.79442614, 0.7878198, 0.78881842, 0.70690477, 0.80707121, 0.78768665, 0.7215547, 0.75226194, 0.72196257, 0.765799, 0.77267712, 0.75844234, 0.81038833, 0.81188059, 0.79864907, 0.816436, 0.845298, 0.85074174, 0.73668873, 0.83516812]) + +order = 3 + +x = np.arange(0, len(pts)) +print(np.polyfit(x, pts, order, w=1/stds)) + +# Do polyfit manually +w = 1.0 / stds +lhs = np.vander(x, order+1).astype(np.float64) +rhs = pts + +lhs *= np.atleast_2d(w).T +rhs *= w + +scale = np.sqrt((lhs*lhs).sum(axis=0)) +lhs = lhs / scale +c, resids, rank, s = np.linalg.lstsq(lhs, rhs, rcond=None) +c = (c.T/scale).T +print(c) diff --git a/selfdrive/modeld/test/snpe_benchmark/.gitignore b/selfdrive/modeld/test/snpe_benchmark/.gitignore new file mode 100644 index 000000000..d83a1b2ff --- /dev/null +++ b/selfdrive/modeld/test/snpe_benchmark/.gitignore @@ -0,0 +1 @@ +benchmark diff --git a/selfdrive/modeld/test/snpe_benchmark/benchmark.cc b/selfdrive/modeld/test/snpe_benchmark/benchmark.cc new file mode 100644 index 000000000..cc61e96ff --- /dev/null +++ b/selfdrive/modeld/test/snpe_benchmark/benchmark.cc @@ -0,0 +1,190 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +int64_t timespecDiff(struct timespec *timeA_p, struct timespec *timeB_p) { + return ((timeA_p->tv_sec * 1000000000) + timeA_p->tv_nsec) - ((timeB_p->tv_sec * 1000000000) + timeB_p->tv_nsec); +} + +void PrintErrorStringAndExit() { + cout << "ERROR!" << endl; + const char* const errStr = zdl::DlSystem::getLastErrorString(); + std::cerr << errStr << std::endl; + std::exit(EXIT_FAILURE); +} + + +zdl::DlSystem::Runtime_t checkRuntime() { + static zdl::DlSystem::Version_t Version = zdl::SNPE::SNPEFactory::getLibraryVersion(); + static zdl::DlSystem::Runtime_t Runtime; + std::cout << "SNPE Version: " << Version.asString().c_str() << std::endl; //Print Version number + if (zdl::SNPE::SNPEFactory::isRuntimeAvailable(zdl::DlSystem::Runtime_t::DSP)) { + std::cout << "Using DSP runtime" << std::endl; + Runtime = zdl::DlSystem::Runtime_t::DSP; + } else if (zdl::SNPE::SNPEFactory::isRuntimeAvailable(zdl::DlSystem::Runtime_t::GPU)) { + std::cout << "Using GPU runtime" << std::endl; + Runtime = zdl::DlSystem::Runtime_t::GPU; + } else { + std::cout << "Using cpu runtime" << std::endl; + Runtime = zdl::DlSystem::Runtime_t::CPU; + } + return Runtime; +} + +void test(char *filename) { + static zdl::DlSystem::Runtime_t runtime = checkRuntime(); + std::unique_ptr container; + container = zdl::DlContainer::IDlContainer::open(filename); + + if (!container) { PrintErrorStringAndExit(); } + cout << "start build" << endl; + std::unique_ptr snpe; + { + snpe = NULL; + zdl::SNPE::SNPEBuilder snpeBuilder(container.get()); + snpe = snpeBuilder.setOutputLayers({}) + .setRuntimeProcessor(runtime) + .setUseUserSuppliedBuffers(false) + //.setDebugMode(true) + .build(); + if (!snpe) { + cout << "ERROR!" << endl; + const char* const errStr = zdl::DlSystem::getLastErrorString(); + std::cerr << errStr << std::endl; + } + cout << "ran snpeBuilder" << endl; + } + + const auto &strList_opt = snpe->getInputTensorNames(); + if (!strList_opt) throw std::runtime_error("Error obtaining input tensor names"); + + cout << "get input tensor names done" << endl; + const auto &strList = *strList_opt; + static zdl::DlSystem::TensorMap inputTensorMap; + static zdl::DlSystem::TensorMap outputTensorMap; + vector > inputs; + for (int i = 0; i < strList.size(); i++) { + cout << "input name: " << strList.at(i) << endl; + + const auto &inputDims_opt = snpe->getInputDimensions(strList.at(i)); + const auto &inputShape = *inputDims_opt; + inputs.push_back(zdl::SNPE::SNPEFactory::getTensorFactory().createTensor(inputShape)); + inputTensorMap.add(strList.at(i), inputs[i].get()); + } + + struct timespec start, end; + cout << "**** starting benchmark ****" << endl; + for (int i = 0; i < 10; i++) { + clock_gettime(CLOCK_MONOTONIC, &start); + assert(snpe->execute(inputTensorMap, outputTensorMap)); + clock_gettime(CLOCK_MONOTONIC, &end); + uint64_t timeElapsed = timespecDiff(&end, &start); + printf("time: %f ms\n", timeElapsed*1.0/1e6); + } +} + +void get_testframe(int index, std::unique_ptr &input) { + FILE * pFile; + string filepath="/data/ipt/quantize_samples/sample_input_"+std::to_string(index); + pFile = fopen(filepath.c_str(),"rb"); + int length = 1*6*160*320*4; + float * frame_buffer = new float[length/4]; // 32/8 + fread(frame_buffer, length, 1, pFile); + // std::cout << *(frame_buffer+length/4-1) << std::endl; + std::copy(frame_buffer, frame_buffer+(length/4), input->begin()); +} + +void SaveITensor(const std::string& path, const zdl::DlSystem::ITensor* tensor) +{ + std::ofstream os(path, std::ofstream::binary); + if (!os) + { + std::cerr << "Failed to open output file for writing: " << path << "\n"; + std::exit(EXIT_FAILURE); + } + for ( auto it = tensor->cbegin(); it != tensor->cend(); ++it ) + { + float f = *it; + if (!os.write(reinterpret_cast(&f), sizeof(float))) + { + std::cerr << "Failed to write data to: " << path << "\n"; + std::exit(EXIT_FAILURE); + } + } +} + +void testrun(char* modelfile) { + static zdl::DlSystem::Runtime_t runtime = checkRuntime(); + std::unique_ptr container; + container = zdl::DlContainer::IDlContainer::open(modelfile); + + if (!container) { PrintErrorStringAndExit(); } + cout << "start build" << endl; + std::unique_ptr snpe; + { + snpe = NULL; + zdl::SNPE::SNPEBuilder snpeBuilder(container.get()); + snpe = snpeBuilder.setOutputLayers({}) + .setRuntimeProcessor(runtime) + .setUseUserSuppliedBuffers(false) + //.setDebugMode(true) + .build(); + if (!snpe) { + cout << "ERROR!" << endl; + const char* const errStr = zdl::DlSystem::getLastErrorString(); + std::cerr << errStr << std::endl; + } + cout << "ran snpeBuilder" << endl; + } + + const auto &strList_opt = snpe->getInputTensorNames(); + if (!strList_opt) throw std::runtime_error("Error obtaining input tensor names"); + cout << "get input tensor names done" << endl; + + const auto &strList = *strList_opt; + static zdl::DlSystem::TensorMap inputTensorMap; + static zdl::DlSystem::TensorMap outputTensorMap; + + assert (strList.size() == 1); + const auto &inputDims_opt = snpe->getInputDimensions(strList.at(0)); + const auto &inputShape = *inputDims_opt; + std::cout << "winkwink" << std::endl; + + for (int i=0;i<10000;i++) { + std::unique_ptr input; + input = zdl::SNPE::SNPEFactory::getTensorFactory().createTensor(inputShape); + get_testframe(i,input); + snpe->execute(input.get(), outputTensorMap); + zdl::DlSystem::StringList tensorNames = outputTensorMap.getTensorNames(); + std::for_each( tensorNames.begin(), tensorNames.end(), [&](const char* name) { + std::ostringstream path; + path << "/data/opt/Result_" << std::to_string(i) << ".raw"; + auto tensorPtr = outputTensorMap.getTensor(name); + SaveITensor(path.str(), tensorPtr); + }); + } +} + +int main(int argc, char* argv[]) { + if (argc < 2) { + printf("usage: %s \n", argv[0]); + return -1; + } + + if (argc == 2) { + while(1) test(argv[1]); + } else if (argc == 3) { + testrun(argv[1]); + } + return 0; +} + diff --git a/selfdrive/modeld/test/snpe_benchmark/benchmark.sh b/selfdrive/modeld/test/snpe_benchmark/benchmark.sh new file mode 100755 index 000000000..685ae58aa --- /dev/null +++ b/selfdrive/modeld/test/snpe_benchmark/benchmark.sh @@ -0,0 +1,3 @@ +#!/bin/sh -e +clang++ -I ~/one/phonelibs/snpe/include/ -lSNPE -lsymphony-cpu -lsymphonypower benchmark.cc -o benchmark +./benchmark $1 diff --git a/selfdrive/modeld/test/tf_test/build.sh b/selfdrive/modeld/test/tf_test/build.sh new file mode 100755 index 000000000..4e92ca069 --- /dev/null +++ b/selfdrive/modeld/test/tf_test/build.sh @@ -0,0 +1,2 @@ +#!/bin/bash +clang++ -I /home/batman/one/external/tensorflow/include/ -L /home/batman/one/external/tensorflow/lib -Wl,-rpath=/home/batman/one/external/tensorflow/lib main.cc -ltensorflow diff --git a/selfdrive/modeld/test/tf_test/main.cc b/selfdrive/modeld/test/tf_test/main.cc new file mode 100644 index 000000000..bd325c81c --- /dev/null +++ b/selfdrive/modeld/test/tf_test/main.cc @@ -0,0 +1,69 @@ +#include +#include +#include +#include "tensorflow/c/c_api.h" + +void* read_file(const char* path, size_t* out_len) { + FILE* f = fopen(path, "r"); + if (!f) { + return NULL; + } + fseek(f, 0, SEEK_END); + long f_len = ftell(f); + rewind(f); + + char* buf = (char*)calloc(f_len, 1); + assert(buf); + + size_t num_read = fread(buf, f_len, 1, f); + fclose(f); + + if (num_read != 1) { + free(buf); + return NULL; + } + + if (out_len) { + *out_len = f_len; + } + + return buf; +} + +static void DeallocateBuffer(void* data, size_t) { + free(data); +} + +int main(int argc, char* argv[]) { + TF_Buffer* buf; + TF_Graph* graph; + TF_Status* status; + char *path = argv[1]; + + // load model + { + size_t model_size; + char tmp[1024]; + snprintf(tmp, sizeof(tmp), "%s.pb", path); + printf("loading model %s\n", tmp); + uint8_t *model_data = (uint8_t *)read_file(tmp, &model_size); + buf = TF_NewBuffer(); + buf->data = model_data; + buf->length = model_size; + buf->data_deallocator = DeallocateBuffer; + printf("loaded model of size %d\n", model_size); + } + + // import graph + status = TF_NewStatus(); + graph = TF_NewGraph(); + TF_ImportGraphDefOptions *opts = TF_NewImportGraphDefOptions(); + TF_GraphImportGraphDef(graph, buf, opts, status); + TF_DeleteImportGraphDefOptions(opts); + TF_DeleteBuffer(buf); + if (TF_GetCode(status) != TF_OK) { + printf("FAIL: %s\n", TF_Message(status)); + } else { + printf("SUCCESS\n"); + } +} diff --git a/selfdrive/modeld/test/tf_test/pb_loader.py b/selfdrive/modeld/test/tf_test/pb_loader.py new file mode 100755 index 000000000..d4db20eb6 --- /dev/null +++ b/selfdrive/modeld/test/tf_test/pb_loader.py @@ -0,0 +1,9 @@ +#!/usr/bin/env python3 +import sys +import tensorflow as tf + +with open(sys.argv[1], "rb") as f: + graph_def = tf.compat.v1.GraphDef() + graph_def.ParseFromString(f.read()) + #tf.io.write_graph(graph_def, '', sys.argv[1]+".try") + diff --git a/selfdrive/modeld/transforms/loadyuv.c b/selfdrive/modeld/transforms/loadyuv.c new file mode 100644 index 000000000..251814510 --- /dev/null +++ b/selfdrive/modeld/transforms/loadyuv.c @@ -0,0 +1,82 @@ +#include +#include + +#include "clutil.h" + +#include "loadyuv.h" + +void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height) { + int err = 0; + memset(s, 0, sizeof(*s)); + + s->width = width; + s->height = height; + + char args[1024]; + snprintf(args, sizeof(args), + "-cl-fast-relaxed-math -cl-denorms-are-zero " + "-DTRANSFORMED_WIDTH=%d -DTRANSFORMED_HEIGHT=%d", + width, height); + cl_program prg = CLU_LOAD_FROM_FILE(ctx, device_id, "transforms/loadyuv.cl", args); + + s->loadys_krnl = clCreateKernel(prg, "loadys", &err); + assert(err == 0); + s->loaduv_krnl = clCreateKernel(prg, "loaduv", &err); + assert(err == 0); + + // done with this + err = clReleaseProgram(prg); + assert(err == 0); +} + +void loadyuv_destroy(LoadYUVState* s) { + int err = 0; + + err = clReleaseKernel(s->loadys_krnl); + assert(err == 0); + err = clReleaseKernel(s->loaduv_krnl); + assert(err == 0); +} + +void loadyuv_queue(LoadYUVState* s, cl_command_queue q, + cl_mem y_cl, cl_mem u_cl, cl_mem v_cl, + cl_mem out_cl) { + int err = 0; + + err = clSetKernelArg(s->loadys_krnl, 0, sizeof(cl_mem), &y_cl); + assert(err == 0); + err = clSetKernelArg(s->loadys_krnl, 1, sizeof(cl_mem), &out_cl); + assert(err == 0); + + const size_t loadys_work_size = (s->width*s->height)/8; + err = clEnqueueNDRangeKernel(q, s->loadys_krnl, 1, NULL, + &loadys_work_size, NULL, 0, 0, NULL); + assert(err == 0); + + const size_t loaduv_work_size = ((s->width/2)*(s->height/2))/8; + cl_int loaduv_out_off = (s->width*s->height); + + err = clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &u_cl); + assert(err == 0); + err = clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl); + assert(err == 0); + err = clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &loaduv_out_off); + assert(err == 0); + + err = clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL, + &loaduv_work_size, NULL, 0, 0, NULL); + assert(err == 0); + + loaduv_out_off += (s->width/2)*(s->height/2); + + err = clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &v_cl); + assert(err == 0); + err = clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl); + assert(err == 0); + err = clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &loaduv_out_off); + assert(err == 0); + + err = clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL, + &loaduv_work_size, NULL, 0, 0, NULL); + assert(err == 0); +} diff --git a/selfdrive/modeld/transforms/loadyuv.cl b/selfdrive/modeld/transforms/loadyuv.cl new file mode 100644 index 000000000..e01542915 --- /dev/null +++ b/selfdrive/modeld/transforms/loadyuv.cl @@ -0,0 +1,43 @@ +#define UV_SIZE ((TRANSFORMED_WIDTH/2)*(TRANSFORMED_HEIGHT/2)) + +__kernel void loadys(__global uchar8 const * const Y, + __global float * out) +{ + const int gid = get_global_id(0); + const int ois = gid * 8; + const int oy = ois / TRANSFORMED_WIDTH; + const int ox = ois % TRANSFORMED_WIDTH; + + const uchar8 ys = Y[gid]; + + // y = (x - 128) / 128 + const float8 ysf = (convert_float8(ys) - 128.f) * 0.0078125f; + + // 02 + // 13 + + __global float* outy0; + __global float* outy1; + if ((oy & 1) == 0) { + outy0 = out; //y0 + outy1 = out + UV_SIZE*2; //y2 + } else { + outy0 = out + UV_SIZE; //y1 + outy1 = out + UV_SIZE*3; //y3 + } + + vstore4(ysf.s0246, 0, outy0 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2); + vstore4(ysf.s1357, 0, outy1 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2); +} + +__kernel void loaduv(__global uchar8 const * const in, + __global float8 * out, + int out_offset) +{ + const int gid = get_global_id(0); + const uchar8 inv = in[gid]; + + // y = (x - 128) / 128 + const float8 outv = (convert_float8(inv) - 128.f) * 0.0078125f; + out[gid + out_offset / 8] = outv; +} diff --git a/selfdrive/modeld/transforms/loadyuv.h b/selfdrive/modeld/transforms/loadyuv.h new file mode 100644 index 000000000..be7ea2128 --- /dev/null +++ b/selfdrive/modeld/transforms/loadyuv.h @@ -0,0 +1,34 @@ +#ifndef LOADYUV_H +#define LOADYUV_H + +#include +#include + +#ifdef __APPLE__ +#include +#else +#include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct { + int width, height; + cl_kernel loadys_krnl, loaduv_krnl; +} LoadYUVState; + +void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height); + +void loadyuv_destroy(LoadYUVState* s); + +void loadyuv_queue(LoadYUVState* s, cl_command_queue q, + cl_mem y_cl, cl_mem u_cl, cl_mem v_cl, + cl_mem out_cl); + +#ifdef __cplusplus +} +#endif + +#endif // LOADYUV_H diff --git a/selfdrive/modeld/transforms/transform.c b/selfdrive/modeld/transforms/transform.c new file mode 100644 index 000000000..0b4150ddb --- /dev/null +++ b/selfdrive/modeld/transforms/transform.c @@ -0,0 +1,149 @@ +#include +#include + +#include "clutil.h" + +#include "transform.h" + +void transform_init(Transform* s, cl_context ctx, cl_device_id device_id) { + int err = 0; + memset(s, 0, sizeof(*s)); + + cl_program prg = CLU_LOAD_FROM_FILE(ctx, device_id, "transforms/transform.cl", ""); + + s->krnl = clCreateKernel(prg, "warpPerspective", &err); + assert(err == 0); + + // done with this + err = clReleaseProgram(prg); + assert(err == 0); + + s->m_y_cl = clCreateBuffer(ctx, CL_MEM_READ_WRITE, 3*3*sizeof(float), NULL, &err); + assert(err == 0); + + s->m_uv_cl = clCreateBuffer(ctx, CL_MEM_READ_WRITE, 3*3*sizeof(float), NULL, &err); + assert(err == 0); +} + +void transform_destroy(Transform* s) { + int err = 0; + + err = clReleaseMemObject(s->m_y_cl); + assert(err == 0); + err = clReleaseMemObject(s->m_uv_cl); + assert(err == 0); + + err = clReleaseKernel(s->krnl); + assert(err == 0); +} + +void transform_queue(Transform* s, + cl_command_queue q, + cl_mem in_yuv, int in_width, int in_height, + cl_mem out_y, cl_mem out_u, cl_mem out_v, + int out_width, int out_height, + mat3 projection) { + int err = 0; + const int zero = 0; + + // sampled using pixel center origin + // (because thats how fastcv and opencv does it) + + mat3 projection_y = projection; + + // in and out uv is half the size of y. + mat3 projection_uv = transform_scale_buffer(projection, 0.5); + + err = clEnqueueWriteBuffer(q, s->m_y_cl, CL_TRUE, 0, 3*3*sizeof(float), (void*)projection_y.v, 0, NULL, NULL); + assert(err == 0); + err = clEnqueueWriteBuffer(q, s->m_uv_cl, CL_TRUE, 0, 3*3*sizeof(float), (void*)projection_uv.v, 0, NULL, NULL); + assert(err == 0); + + const int in_y_width = in_width; + const int in_y_height = in_height; + const int in_uv_width = in_width/2; + const int in_uv_height = in_height/2; + const int in_y_offset = 0; + const int in_u_offset = in_y_offset + in_y_width*in_y_height; + const int in_v_offset = in_u_offset + in_uv_width*in_uv_height; + + const int out_y_width = out_width; + const int out_y_height = out_height; + const int out_uv_width = out_width/2; + const int out_uv_height = out_height/2; + + err = clSetKernelArg(s->krnl, 0, sizeof(cl_mem), &in_yuv); + assert(err == 0); + + err = clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_y_width); + assert(err == 0); + err = clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_y_offset); + assert(err == 0); + err = clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_y_height); + assert(err == 0); + err = clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_y_width); + assert(err == 0); + + err = clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_y); + assert(err == 0); + + err = clSetKernelArg(s->krnl, 6, sizeof(cl_int), &out_y_width); + assert(err == 0); + err = clSetKernelArg(s->krnl, 7, sizeof(cl_int), &zero); + assert(err == 0); + err = clSetKernelArg(s->krnl, 8, sizeof(cl_int), &out_y_height); + assert(err == 0); + err = clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_y_width); + assert(err == 0); + + err = clSetKernelArg(s->krnl, 10, sizeof(cl_mem), &s->m_y_cl); + assert(err == 0); + + const size_t work_size_y[2] = {out_y_width, out_y_height}; + + err = clEnqueueNDRangeKernel(q, s->krnl, 2, NULL, + (const size_t*)&work_size_y, NULL, 0, 0, NULL); + assert(err == 0); + + + const size_t work_size_uv[2] = {out_uv_width, out_uv_height}; + + err = clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_uv_width); + assert(err == 0); + err = clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_u_offset); + assert(err == 0); + err = clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_uv_height); + assert(err == 0); + err = clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_uv_width); + assert(err == 0); + + err = clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_u); + assert(err == 0); + + err = clSetKernelArg(s->krnl, 6, sizeof(cl_int), &out_uv_width); + assert(err == 0); + err = clSetKernelArg(s->krnl, 7, sizeof(cl_int), &zero); + assert(err == 0); + err = clSetKernelArg(s->krnl, 8, sizeof(cl_int), &out_uv_height); + assert(err == 0); + err = clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_uv_width); + assert(err == 0); + + err = clSetKernelArg(s->krnl, 10, sizeof(cl_mem), &s->m_uv_cl); + assert(err == 0); + + err = clEnqueueNDRangeKernel(q, s->krnl, 2, NULL, + (const size_t*)&work_size_uv, NULL, 0, 0, NULL); + assert(err == 0); + + + err = clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_v_offset); + assert(err == 0); + err = clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_v); + assert(err == 0); + + + err = clEnqueueNDRangeKernel(q, s->krnl, 2, NULL, + (const size_t*)&work_size_uv, NULL, 0, 0, NULL); + assert(err == 0); +} diff --git a/selfdrive/modeld/transforms/transform.cl b/selfdrive/modeld/transforms/transform.cl new file mode 100644 index 000000000..8ad186935 --- /dev/null +++ b/selfdrive/modeld/transforms/transform.cl @@ -0,0 +1,54 @@ +#define INTER_BITS 5 +#define INTER_TAB_SIZE (1 << INTER_BITS) +#define INTER_SCALE 1.f / INTER_TAB_SIZE + +#define INTER_REMAP_COEF_BITS 15 +#define INTER_REMAP_COEF_SCALE (1 << INTER_REMAP_COEF_BITS) + +__kernel void warpPerspective(__global const uchar * src, + int src_step, int src_offset, int src_rows, int src_cols, + __global uchar * dst, + int dst_step, int dst_offset, int dst_rows, int dst_cols, + __constant float * M) +{ + int dx = get_global_id(0); + int dy = get_global_id(1); + + if (dx < dst_cols && dy < dst_rows) + { + float X0 = M[0] * dx + M[1] * dy + M[2]; + float Y0 = M[3] * dx + M[4] * dy + M[5]; + float W = M[6] * dx + M[7] * dy + M[8]; + W = W != 0.0f ? INTER_TAB_SIZE / W : 0.0f; + int X = rint(X0 * W), Y = rint(Y0 * W); + + short sx = convert_short_sat(X >> INTER_BITS); + short sy = convert_short_sat(Y >> INTER_BITS); + short ay = (short)(Y & (INTER_TAB_SIZE - 1)); + short ax = (short)(X & (INTER_TAB_SIZE - 1)); + + int v0 = (sx >= 0 && sx < src_cols && sy >= 0 && sy < src_rows) ? + convert_int(src[mad24(sy, src_step, src_offset + sx)]) : 0; + int v1 = (sx+1 >= 0 && sx+1 < src_cols && sy >= 0 && sy < src_rows) ? + convert_int(src[mad24(sy, src_step, src_offset + (sx+1))]) : 0; + int v2 = (sx >= 0 && sx < src_cols && sy+1 >= 0 && sy+1 < src_rows) ? + convert_int(src[mad24(sy+1, src_step, src_offset + sx)]) : 0; + int v3 = (sx+1 >= 0 && sx+1 < src_cols && sy+1 >= 0 && sy+1 < src_rows) ? + convert_int(src[mad24(sy+1, src_step, src_offset + (sx+1))]) : 0; + + float taby = 1.f/INTER_TAB_SIZE*ay; + float tabx = 1.f/INTER_TAB_SIZE*ax; + + int dst_index = mad24(dy, dst_step, dst_offset + dx); + + int itab0 = convert_short_sat_rte( (1.0f-taby)*(1.0f-tabx) * INTER_REMAP_COEF_SCALE ); + int itab1 = convert_short_sat_rte( (1.0f-taby)*tabx * INTER_REMAP_COEF_SCALE ); + int itab2 = convert_short_sat_rte( taby*(1.0f-tabx) * INTER_REMAP_COEF_SCALE ); + int itab3 = convert_short_sat_rte( taby*tabx * INTER_REMAP_COEF_SCALE ); + + int val = v0 * itab0 + v1 * itab1 + v2 * itab2 + v3 * itab3; + + uchar pix = convert_uchar_sat((val + (1 << (INTER_REMAP_COEF_BITS-1))) >> INTER_REMAP_COEF_BITS); + dst[dst_index] = pix; + } +} diff --git a/selfdrive/modeld/transforms/transform.h b/selfdrive/modeld/transforms/transform.h new file mode 100644 index 000000000..3854be9da --- /dev/null +++ b/selfdrive/modeld/transforms/transform.h @@ -0,0 +1,38 @@ +#ifndef TRANSFORM_H +#define TRANSFORM_H + +#include +#include + +#ifdef __APPLE__ +#include +#else +#include +#endif + +#include "common/mat.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct { + cl_kernel krnl; + cl_mem m_y_cl, m_uv_cl; +} Transform; + +void transform_init(Transform* s, cl_context ctx, cl_device_id device_id); + +void transform_destroy(Transform* transform); + +void transform_queue(Transform* s, cl_command_queue q, + cl_mem yuv, int in_width, int in_height, + cl_mem out_y, cl_mem out_u, cl_mem out_v, + int out_width, int out_height, + mat3 projection); + +#ifdef __cplusplus +} +#endif + +#endif // TRANSFORM_H diff --git a/selfdrive/modeld/visiontest.c b/selfdrive/modeld/visiontest.c new file mode 100644 index 000000000..325e257e8 --- /dev/null +++ b/selfdrive/modeld/visiontest.c @@ -0,0 +1,160 @@ +#include +#include +#include + +#define CL_USE_DEPRECATED_OPENCL_1_2_APIS +#ifdef __APPLE__ +#include +#else +#include +#endif + +#include "clutil.h" +#include "transforms/transform.h" + +typedef struct { + int disable_model; + Transform transform; + + int in_width; + int in_height; + int out_width; + int out_height; + + cl_context context; + cl_command_queue command_queue; + cl_device_id device_id; + + size_t in_yuv_size; + cl_mem in_yuv_cl; + + cl_mem out_y_cl, out_u_cl, out_v_cl; +} VisionTest; + +void initialize_opencl(VisionTest* visiontest) { + // init cl + /* Get Platform and Device Info */ + cl_platform_id platform_ids[16] = {0}; + cl_uint num_platforms; + int err = clGetPlatformIDs(16, platform_ids, &num_platforms); + if (err != 0) { + fprintf(stderr, "cl error: %d\n", err); + } + assert(err == 0); + + // try to find a CPU device + cl_device_id device_id = NULL; + for (int i=0; icontext = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err); + assert(err == 0); + + visiontest->device_id = device_id; +} + +VisionTest* visiontest_create(int temporal_model, int disable_model, + int input_width, int input_height, + int model_input_width, int model_input_height) { + int err = 0; + + VisionTest* const vt = calloc(1, sizeof(*vt)); + assert(vt); + + vt->disable_model = disable_model; + vt->in_width = input_width; + vt->in_height = input_height; + vt->out_width = model_input_width; + vt->out_height = model_input_height; + + initialize_opencl(vt); + + transform_init(&vt->transform, vt->context, vt->device_id); + + + assert((vt->in_width%2) == 0 && (vt->in_height%2) == 0); + vt->in_yuv_size = vt->in_width*vt->in_height*3/2; + vt->in_yuv_cl = clCreateBuffer(vt->context, CL_MEM_READ_WRITE, + vt->in_yuv_size, NULL, &err); + assert(err == 0); + + vt->out_y_cl = clCreateBuffer(vt->context, CL_MEM_READ_WRITE, + vt->out_width*vt->out_width, NULL, &err); + assert(err == 0); + vt->out_u_cl = clCreateBuffer(vt->context, CL_MEM_READ_WRITE, + vt->out_width*vt->out_width/4, NULL, &err); + assert(err == 0); + vt->out_v_cl = clCreateBuffer(vt->context, CL_MEM_READ_WRITE, + vt->out_width*vt->out_width/4, NULL, &err); + assert(err == 0); + + vt->command_queue = clCreateCommandQueue(vt->context, vt->device_id, 0, &err); + assert(err == 0); + + return vt; +} + +void visiontest_destroy(VisionTest* vt) { + transform_destroy(&vt->transform); + + int err = 0; + + err = clReleaseMemObject(vt->in_yuv_cl); + assert(err == 0); + err = clReleaseMemObject(vt->out_y_cl); + assert(err == 0); + err = clReleaseMemObject(vt->out_u_cl); + assert(err == 0); + err = clReleaseMemObject(vt->out_v_cl); + assert(err == 0); + + err = clReleaseCommandQueue(vt->command_queue); + assert(err == 0); + + err = clReleaseContext(vt->context); + assert(err == 0); + + free(vt); +} + +void visiontest_transform(VisionTest* vt, const uint8_t* yuv_data, + uint8_t* out_y, uint8_t* out_u, uint8_t* out_v, + const float* transform) { + int err = 0; + + err = clEnqueueWriteBuffer(vt->command_queue, vt->in_yuv_cl, CL_FALSE, + 0, vt->in_yuv_size, yuv_data, 0, NULL, NULL); + assert(err == 0); + + mat3 transform_m = *(const mat3*)transform; + + transform_queue(&vt->transform, vt->command_queue, + vt->in_yuv_cl, vt->in_width, vt->in_height, + vt->out_y_cl, vt->out_u_cl, vt->out_v_cl, + vt->out_width, vt->out_height, + transform_m); + + err = clEnqueueReadBuffer(vt->command_queue, vt->out_y_cl, CL_FALSE, + 0, vt->out_width*vt->out_height, out_y, + 0, NULL, NULL); + assert(err == 0); + err = clEnqueueReadBuffer(vt->command_queue, vt->out_u_cl, CL_FALSE, + 0, vt->out_width*vt->out_height/4, out_u, + 0, NULL, NULL); + assert(err == 0); + err = clEnqueueReadBuffer(vt->command_queue, vt->out_v_cl, CL_FALSE, + 0, vt->out_width*vt->out_height/4, out_v, + 0, NULL, NULL); + assert(err == 0); + + clFinish(vt->command_queue); +} + diff --git a/selfdrive/modeld/visiontest.mk b/selfdrive/modeld/visiontest.mk new file mode 100644 index 000000000..f1aa7afdb --- /dev/null +++ b/selfdrive/modeld/visiontest.mk @@ -0,0 +1,105 @@ +CC:=clang +CXX:=clang++ +OPT_FLAGS:=-O2 -g -ggdb3 + +UNAME_S := $(shell uname -s) +ifeq ($(UNAME_S),Linux) + SHARED_FLAGS=-Wl,--whole-archive $^ -Wl,--no-whole-archive +endif +ifeq ($(UNAME_S),Darwin) + SHARED_FLAGS=-Wl,-force_load $^ +endif + +PHONELIBS := ../../phonelibs +BASEDIR := ../.. + +WARN_FLAGS = -Werror=implicit-function-declaration \ + -Werror=incompatible-pointer-types \ + -Werror=int-conversion \ + -Werror=return-type \ + -Werror=format-extra-args + +CFLAGS = -std=gnu11 -g -fPIC $(OPT_FLAGS) $(WARN_FLAGS) +CXXFLAGS = -std=c++14 -fPIC $(OPT_FLAGS) $(WARN_FLAGS) + +EIGEN_FLAGS = -I$(PHONELIBS)/eigen + +CEREAL_LIBS = $(BASEDIR)/cereal/libmessaging.a + +OPENCV_LIBS = -lopencv_video -lopencv_core -lopencv_imgproc + +ifeq ($(UNAME_S),Darwin) + VT_LDFLAGS += $(PHONELIBS)/capnp-c/mac/lib/libcapnp_c.a \ + $(PHONELIBS)/zmq/mac/lib/libczmq.a \ + $(PHONELIBS)/zmq/mac/lib/libzmq.a \ + -framework OpenCL + + OPENCV_LIBS += -L/usr/local/opt/opencv@2/lib + OPENCV_FLAGS += -I/usr/local/opt/opencv@2/include + +else + VT_LDFLAGS += $(CEREAL_LIBS) \ + -L/system/vendor/lib64 \ + -L$(BASEDIR)/external/zmq/lib/ \ + -l:libczmq.a -l:libzmq.a \ + -lOpenCL +endif + +.PHONY: all visiontest clean test +all: visiontest + +libvisiontest_inputs := visiontest.c \ + transforms/transform.c \ + transforms/loadyuv.c \ + ../common/clutil.c \ + $(BASEDIR)/selfdrive/common/util.c \ + $(CEREAL_OBJS) + +visiontest: libvisiontest.so +all-tests := $(addsuffix .test, $(basename $(wildcard test_*))) + +%.o: %.cc + @echo "[ CXX ] $@" + $(CXX) $(CXXFLAGS) -MMD \ + -I. -I.. -I../.. \ + -Wall \ + -I$(BASEDIR)/ -I$(BASEDIR)/selfdrive -I$(BASEDIR)/selfdrive/common \ + $(EIGEN_FLAGS) \ + $(OPENCV_FLAGS) \ + $(CEREAL_CXXFLAGS) \ + -c -o '$@' '$<' + +%.o: %.c + @echo "[ CXX ] $@" + $(CC) $(CFLAGS) -MMD \ + -I. -I.. -I../.. \ + -Wall \ + -I$(BASEDIR)/ -I$(BASEDIR)/selfdrive -I$(BASEDIR)/selfdrive/common \ + $(CEREAL_CFLAGS) \ + -c -o '$@' '$<' + +libvisiontest.so: $(libvisiontest_inputs) + $(eval $@_TMP := $(shell mktemp)) + $(CC) -std=gnu11 -shared -fPIC -O2 -g \ + -Werror=implicit-function-declaration -Werror=incompatible-pointer-types \ + -Werror=int-conversion -Wno-pointer-to-int-cast \ + -I. -DCLU_NO_CACHE \ + $^ -o $($@_TMP) \ + -I$(PHONELIBS)/opencl/include \ + -I$(BASEDIR)/selfdrive/common \ + $(CEREAL_CXXFLAGS) \ + $(CEREAL_CFLAGS) \ + -I$(BASEDIR)/external/zmq/include \ + -I$(BASEDIR)/ -I$(BASEDIR)/selfdrive \ + -lstdc++ \ + $(VT_LDFLAGS) \ + -lm -lpthread + mv $($@_TMP) $@ + +test : $(all-tests) + +test_%.test : test_% + @./'$<' || echo FAIL + +clean: + rm -rf *.o *.so *.a diff --git a/selfdrive/modeld/visiontest.py b/selfdrive/modeld/visiontest.py new file mode 100644 index 000000000..f9661bf46 --- /dev/null +++ b/selfdrive/modeld/visiontest.py @@ -0,0 +1,135 @@ +import os +import subprocess +from cffi import FFI +from common.basedir import BASEDIR + +# Initialize visiontest. Ignore output. +_visiond_dir = os.path.dirname(os.path.abspath(__file__)) +_libvisiontest = "libvisiontest.so" +try: # bacause this crashes somtimes when running pipeline + subprocess.check_output(["make", "-C", _visiond_dir, "-f", + os.path.join(_visiond_dir, "visiontest.mk"), + _libvisiontest]) +except: + pass + +class VisionTest(): + """A version of the vision model that can be run on a desktop. + + WARNING: This class is not thread safe. VisionTest objects cannot be + created or used on multiple threads simultaneously. + """ + + ffi = FFI() + ffi.cdef(""" + typedef unsigned char uint8_t; + + struct VisionTest; + typedef struct VisionTest VisionTest; + + VisionTest* visiontest_create(int temporal_model, int disable_model, + int input_width, int input_height, + int model_input_width, int model_input_height); + void visiontest_destroy(VisionTest* visiontest); + + void visiontest_transform(VisionTest* vt, const uint8_t* yuv_data, + uint8_t* out_y, uint8_t* out_u, uint8_t* out_v, + const float* transform); + """) + + clib = ffi.dlopen(os.path.join(_visiond_dir, _libvisiontest)) + + def __init__(self, input_size, model_input_size, model): + """Create a wrapper around visiond for off-device python code. + + Inputs: + input_size: The size of YUV images passed to transform. + model_input_size: The size of YUV images passed to the model. + model: The name of the model to use. "temporal", "yuv", or None to disable the + model (used to disable OpenCL). + """ + self._input_size = input_size + self._model_input_size = model_input_size + + if model is None: + disable_model = 1 + temporal_model = 0 + elif model == "yuv": + disable_model = 0 + temporal_model = 0 + elif model == "temporal": + disable_model = 0 + temporal_model = 1 + else: + raise ValueError("Bad model name: {}".format(model)) + + prevdir = os.getcwd() + os.chdir(_visiond_dir) # tmp hack to find kernels + os.environ['BASEDIR'] = BASEDIR + self._visiontest_c = self.clib.visiontest_create( + temporal_model, disable_model, self._input_size[0], self._input_size[1], + self._model_input_size[0], self._model_input_size[1]) + os.chdir(prevdir) + + @property + def input_size(self): + return self._input_size + + @property + def model_input_size(self): + return self._model_input_size + + def transform(self, yuv_data, transform): + y_len = self.model_input_size[0] * self.model_input_size[1] + t_y_ptr = bytearray(y_len) + t_u_ptr = bytearray(y_len // 4) + t_v_ptr = bytearray(y_len // 4) + + self.transform_output_buffer(yuv_data, t_y_ptr, t_u_ptr, t_v_ptr, + transform) + + return t_y_ptr, t_u_ptr, t_v_ptr + + def transform_contiguous(self, yuv_data, transform): + y_ol = self.model_input_size[0] * self.model_input_size[1] + uv_ol = y_ol // 4 + result = bytearray(y_ol * 3 // 2) + result_view = memoryview(result) + t_y_ptr = result_view[:y_ol] + t_u_ptr = result_view[y_ol:y_ol + uv_ol] + t_v_ptr = result_view[y_ol + uv_ol:] + + self.transform_output_buffer(yuv_data, t_y_ptr, t_u_ptr, t_v_ptr, + transform) + return result + + + def transform_output_buffer(self, yuv_data, y_out, u_out, v_out, + transform): + assert len(yuv_data) == self.input_size[0] * self.input_size[1] * 3/2 + + cast = self.ffi.cast + from_buffer = self.ffi.from_buffer + yuv_ptr = cast("unsigned char*", from_buffer(yuv_data)) + transform_ptr = self.ffi.new("float[]", transform) + + y_out_ptr = cast("unsigned char*", from_buffer(y_out)) + u_out_ptr = cast("unsigned char*", from_buffer(u_out)) + v_out_ptr = cast("unsigned char*", from_buffer(v_out)) + + self.clib.visiontest_transform(self._visiontest_c, yuv_ptr, y_out_ptr, + u_out_ptr, v_out_ptr, transform_ptr) + + def close(self): + self.clib.visiontest_destroy(self._visiontest_c) + self._visiontest_c = None + + def __enter__(self): + return self + + def __exit__(self, type, value, traceback): + self.close() + + +if __name__ == "__main__": + VisionTest((560, 304), (320, 160), "temporal") diff --git a/selfdrive/proclogd/SConscript b/selfdrive/proclogd/SConscript new file mode 100644 index 000000000..2b87f8d5e --- /dev/null +++ b/selfdrive/proclogd/SConscript @@ -0,0 +1,2 @@ +Import('env', 'messaging') +env.Program('proclogd.cc', LIBS=[messaging, 'pthread', 'zmq', 'czmq', 'capnp', 'kj']) diff --git a/selfdrive/proclogd/proclogd.cc b/selfdrive/proclogd/proclogd.cc new file mode 100644 index 000000000..ec8a9ad28 --- /dev/null +++ b/selfdrive/proclogd/proclogd.cc @@ -0,0 +1,247 @@ +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "messaging.hpp" +#include +#include "cereal/gen/cpp/log.capnp.h" + +#include "common/timing.h" +#include "common/utilpp.h" + +namespace { + +struct ProcCache { + std::string name; + std::vector cmdline; + std::string exe; +}; + +} + +int main() { + int err; + + Context * c = Context::create(); + PubSocket * publisher = PubSocket::create(c, "procLog"); + assert(publisher != NULL); + + double jiffy = sysconf(_SC_CLK_TCK); + size_t page_size = sysconf(_SC_PAGE_SIZE); + + std::unordered_map proc_cache; + + while (1) { + + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto procLog = event.initProcLog(); + + auto orphanage = msg.getOrphanage(); + + // stat + { + std::vector> otimes; + + std::ifstream sstat("/proc/stat"); + std::string stat_line; + while (std::getline(sstat, stat_line)) { + if (util::starts_with(stat_line, "cpu ")) { + // cpu total + } else if (util::starts_with(stat_line, "cpu")) { + // specific cpu + int id; + unsigned long utime, ntime, stime, itime; + unsigned long iowtime, irqtime, sirqtime; + + int count = sscanf(stat_line.data(), "cpu%d %lu %lu %lu %lu %lu %lu %lu", + &id, &utime, &ntime, &stime, &itime, &iowtime, &irqtime, &sirqtime); + + auto ltimeo = orphanage.newOrphan(); + auto ltime = ltimeo.get(); + ltime.setCpuNum(id); + ltime.setUser(utime / jiffy); + ltime.setNice(ntime / jiffy); + ltime.setSystem(stime / jiffy); + ltime.setIdle(itime / jiffy); + ltime.setIowait(iowtime / jiffy); + ltime.setIrq(irqtime / jiffy); + ltime.setSoftirq(irqtime / jiffy); + + otimes.push_back(std::move(ltimeo)); + + } else { + break; + } + } + + auto ltimes = procLog.initCpuTimes(otimes.size()); + for (size_t i = 0; i < otimes.size(); i++) { + ltimes.adoptWithCaveats(i, std::move(otimes[i])); + } + } + + // meminfo + { + auto mem = procLog.initMem(); + + std::ifstream smem("/proc/meminfo"); + std::string mem_line; + + uint64_t mem_total = 0, mem_free = 0, mem_available = 0, mem_buffers = 0; + uint64_t mem_cached = 0, mem_active = 0, mem_inactive = 0, mem_shared = 0; + + while (std::getline(smem, mem_line)) { + if (util::starts_with(mem_line, "MemTotal:")) sscanf(mem_line.data(), "MemTotal: %lu kB", &mem_total); + else if (util::starts_with(mem_line, "MemFree:")) sscanf(mem_line.data(), "MemFree: %lu kB", &mem_free); + else if (util::starts_with(mem_line, "MemAvailable:")) sscanf(mem_line.data(), "MemAvailable: %lu kB", &mem_available); + else if (util::starts_with(mem_line, "Buffers:")) sscanf(mem_line.data(), "Buffers: %lu kB", &mem_buffers); + else if (util::starts_with(mem_line, "Cached:")) sscanf(mem_line.data(), "Cached: %lu kB", &mem_cached); + else if (util::starts_with(mem_line, "Active:")) sscanf(mem_line.data(), "Active: %lu kB", &mem_active); + else if (util::starts_with(mem_line, "Inactive:")) sscanf(mem_line.data(), "Inactive: %lu kB", &mem_inactive); + else if (util::starts_with(mem_line, "Shmem:")) sscanf(mem_line.data(), "Shmem: %lu kB", &mem_shared); + } + + mem.setTotal(mem_total * 1024); + mem.setFree(mem_free * 1024); + mem.setAvailable(mem_available * 1024); + mem.setBuffers(mem_buffers * 1024); + mem.setCached(mem_cached * 1024); + mem.setActive(mem_active * 1024); + mem.setInactive(mem_inactive * 1024); + mem.setShared(mem_shared * 1024); + } + + // processes + { + std::vector> oprocs; + struct dirent *de = NULL; + DIR *d = opendir("/proc"); + assert(d); + while ((de = readdir(d))) { + if (!isdigit(de->d_name[0])) continue; + pid_t pid = atoi(de->d_name); + + + auto lproco = orphanage.newOrphan(); + auto lproc = lproco.get(); + + lproc.setPid(pid); + + char tcomm[PATH_MAX] = {0}; + + { + std::string stat = util::read_file(util::string_format("/proc/%d/stat", pid)); + + char state; + + int ppid; + unsigned long utime, stime; + long cutime, cstime, priority, nice, num_threads; + unsigned long long starttime; + unsigned long vms, rss; + int processor; + + int count = sscanf(stat.data(), + "%*d (%1024[^)]) %c %d %*d %*d %*d %*d %*d %*d %*d %*d %*d " + "%lu %lu %ld %ld %ld %ld %ld %*d %lld " + "%lu %lu %*d %*d %*d %*d %*d %*d %*d " + "%*d %*d %*d %*d %*d %*d %*d %d", + tcomm, &state, &ppid, + &utime, &stime, &cutime, &cstime, &priority, &nice, &num_threads, &starttime, + &vms, &rss, &processor); + + if (count != 14) continue; + + lproc.setState(state); + lproc.setPpid(ppid); + lproc.setCpuUser(utime / jiffy); + lproc.setCpuSystem(stime / jiffy); + lproc.setCpuChildrenUser(cutime / jiffy); + lproc.setCpuChildrenSystem(cstime / jiffy); + lproc.setPriority(priority); + lproc.setNice(nice); + lproc.setNumThreads(num_threads); + lproc.setStartTime(starttime / jiffy); + lproc.setMemVms(vms); + lproc.setMemRss((uint64_t)rss * page_size); + lproc.setProcessor(processor); + } + + std::string name(tcomm); + lproc.setName(name); + + // populate other things from cache + auto cache_it = proc_cache.find(pid); + ProcCache cache; + if (cache_it != proc_cache.end()) { + cache = cache_it->second; + } + if (cache_it == proc_cache.end() || cache.name != name) { + cache = (ProcCache){ + .name = name, + .exe = util::readlink(util::string_format("/proc/%d/exe", pid)), + }; + + // null-delimited cmdline arguments to vector + std::string cmdline_s = util::read_file(util::string_format("/proc/%d/cmdline", pid)); + const char* cmdline_p = cmdline_s.c_str(); + const char* cmdline_ep = cmdline_p + cmdline_s.size(); + + // strip trailing null bytes + while ((cmdline_ep-1) > cmdline_p && *(cmdline_ep-1) == 0) { + cmdline_ep--; + } + + while (cmdline_p < cmdline_ep) { + std::string arg(cmdline_p); + cache.cmdline.push_back(arg); + cmdline_p += arg.size() + 1; + } + + proc_cache[pid] = cache; + } + + auto lcmdline = lproc.initCmdline(cache.cmdline.size()); + for (size_t i = 0; i < lcmdline.size(); i++) { + lcmdline.set(i, cache.cmdline[i]); + } + lproc.setExe(cache.exe); + + oprocs.push_back(std::move(lproco)); + } + closedir(d); + + auto lprocs = procLog.initProcs(oprocs.size()); + for (size_t i = 0; i < oprocs.size(); i++) { + lprocs.adoptWithCaveats(i, std::move(oprocs[i])); + } + } + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + publisher->send((char*)bytes.begin(), bytes.size()); + + usleep(2000000); // 2 secs + } + + delete c; + delete publisher; + + return 0; +}