From 39da6912ea0b8db16d3f96f289f723452e540ab7 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 12 Jun 2022 18:00:00 -0700 Subject: [PATCH 01/10] misc jenkins fixups (#24840) * bump cereal * remove that * pull cl image * lil docker cleanup --- Dockerfile.openpilot | 7 +------ cereal | 2 +- selfdrive/manager/test/test_manager.py | 8 ++------ tools/sim/Dockerfile.sim | 10 +++++----- tools/sim/build_container.sh | 2 +- 5 files changed, 10 insertions(+), 19 deletions(-) diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index 66b9f18a11041a..102da78d7d01ee 100644 --- a/Dockerfile.openpilot +++ b/Dockerfile.openpilot @@ -8,11 +8,6 @@ ENV PYTHONPATH ${OPENPILOT_PATH}:${PYTHONPATH} RUN mkdir -p ${OPENPILOT_PATH} WORKDIR ${OPENPILOT_PATH} -COPY Pipfile Pipfile.lock $OPENPILOT_PATH -RUN pip install --no-cache-dir pipenv==2021.5.29 pip==21.3.1 && \ - pipenv install --system --deploy --dev --clear && \ - pip uninstall -y pipenv - COPY SConstruct ${OPENPILOT_PATH} COPY ./pyextra ${OPENPILOT_PATH}/pyextra @@ -30,4 +25,4 @@ COPY ./panda ${OPENPILOT_PATH}/panda COPY ./selfdrive ${OPENPILOT_PATH}/selfdrive COPY ./system ${OPENPILOT_PATH}/system -RUN scons -j$(nproc) +RUN scons --cache-readonly -j$(nproc) diff --git a/cereal b/cereal index 78870ba0561743..98f795fd733561 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 78870ba056174352218988610e5010fde4eca956 +Subproject commit 98f795fd73356198f1fc8d5ea5a8f25e6f7c57e0 diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py index e4c3ef7c4c214b..a84ff264d26af3 100755 --- a/selfdrive/manager/test/test_manager.py +++ b/selfdrive/manager/test/test_manager.py @@ -5,14 +5,13 @@ import unittest import selfdrive.manager.manager as manager -from system.hardware import AGNOS, HARDWARE from selfdrive.manager.process import DaemonProcess from selfdrive.manager.process_config import managed_processes +from system.hardware import AGNOS, HARDWARE os.environ['FAKEUPLOAD'] = "1" -# TODO: make eon fast -MAX_STARTUP_TIME = 15 +MAX_STARTUP_TIME = 3 ALL_PROCESSES = [p.name for p in managed_processes.values() if (type(p) is not DaemonProcess) and p.enabled and (p.name not in ['updated', 'pandad'])] @@ -54,9 +53,6 @@ def test_clean_exit(self): # TODO: make Qt UI exit gracefully continue - # Make sure the process is actually dead - managed_processes[p].stop() - # TODO: interrupted blocking read exits with 1 in cereal. use a more unique return code exit_codes = [0, 1] if managed_processes[p].sigkill: diff --git a/tools/sim/Dockerfile.sim b/tools/sim/Dockerfile.sim index d838efed133e95..0d6e8e584c120b 100644 --- a/tools/sim/Dockerfile.sim +++ b/tools/sim/Dockerfile.sim @@ -1,9 +1,9 @@ FROM ghcr.io/commaai/openpilot-base-cl:latest -RUN apt-get update && apt-get install -y --no-install-recommends\ - tmux \ - vim \ - && rm -rf /var/lib/apt/lists/* +RUN apt-get update && apt-get install -y --no-install-recommends \ + tmux \ + vim \ + && rm -rf /var/lib/apt/lists/* # get same tmux config used on NEOS for debugging RUN cd $HOME && \ @@ -27,6 +27,6 @@ COPY ./system $HOME/openpilot/system COPY ./tools $HOME/openpilot/tools WORKDIR $HOME/openpilot -RUN scons -j12 +RUN scons --cache-readonly -j12 RUN python -c "from selfdrive.test.helpers import set_params_enabled; set_params_enabled()" diff --git a/tools/sim/build_container.sh b/tools/sim/build_container.sh index 451277d590b1a5..81afb82d839437 100755 --- a/tools/sim/build_container.sh +++ b/tools/sim/build_container.sh @@ -3,7 +3,7 @@ DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" cd $DIR/../../ -docker pull ghcr.io/commaai/openpilot-base:latest +docker pull ghcr.io/commaai/openpilot-base-cl:latest docker build \ --cache-from ghcr.io/commaai/openpilot-sim:latest \ -t ghcr.io/commaai/openpilot-sim:latest \ From e7cad559a659d47466645290e00ee6205bb938c6 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Mon, 13 Jun 2022 05:39:36 +0100 Subject: [PATCH 02/10] Ford: remove Fusion DBC from release (#24841) --- release/files_common | 1 - 1 file changed, 1 deletion(-) diff --git a/release/files_common b/release/files_common index 76d440c402e4fd..6731ae4a120451 100644 --- a/release/files_common +++ b/release/files_common @@ -473,7 +473,6 @@ opendbc/gm_global_a_powertrain_generated.dbc opendbc/gm_global_a_object.dbc opendbc/gm_global_a_chassis.dbc -opendbc/ford_fusion_2018_adas.dbc opendbc/ford_lincoln_base_pt.dbc opendbc/honda_accord_2018_can_generated.dbc From 3db36a1958b24023a8af2a10419c0c7e4112bef7 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 12 Jun 2022 21:59:58 -0700 Subject: [PATCH 03/10] jenkins: lock simulator --- Jenkinsfile | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Jenkinsfile b/Jenkinsfile index 9d1ab337611207..ebc26a5920c3cf 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -89,8 +89,10 @@ pipeline { sh "git config --global --add safe.directory ${WORKSPACE}" sh "git lfs pull" sh "${WORKSPACE}/tools/sim/build_container.sh" - sh "DETACH=1 ${WORKSPACE}/tools/sim/start_carla.sh" - sh "${WORKSPACE}/tools/sim/start_openpilot_docker.sh" + lock(resource: "", label: "simulator", inversePrecedence: true, quantity: 1) { + sh "DETACH=1 ${WORKSPACE}/tools/sim/start_carla.sh" + sh "${WORKSPACE}/tools/sim/start_openpilot_docker.sh" + } } post { From 84c741ecf6ac0727a29aeebb5f2a6539c2d76dd2 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 13 Jun 2022 09:37:12 +0200 Subject: [PATCH 04/10] SConstruct: set AGNOS from /AGNOS --- SConstruct | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SConstruct b/SConstruct index 0aeb382c23400e..edf14b4725d8b9 100644 --- a/SConstruct +++ b/SConstruct @@ -6,7 +6,7 @@ import platform import numpy as np TICI = os.path.isfile('/TICI') -AGNOS = TICI +AGNOS = os.path.isfile('/AGNOS') Decider('MD5-timestamp') From d71295e0451260acdcfdfb14477c0e21bc4ed311 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 13 Jun 2022 09:50:40 +0200 Subject: [PATCH 05/10] Revert "SConstruct: set AGNOS from /AGNOS" until Jenkins devices are updated This reverts commit 84c741ecf6ac0727a29aeebb5f2a6539c2d76dd2. --- SConstruct | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SConstruct b/SConstruct index edf14b4725d8b9..0aeb382c23400e 100644 --- a/SConstruct +++ b/SConstruct @@ -6,7 +6,7 @@ import platform import numpy as np TICI = os.path.isfile('/TICI') -AGNOS = os.path.isfile('/AGNOS') +AGNOS = TICI Decider('MD5-timestamp') From 724b322909152d58f626b6e76fd34d7d03d67250 Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Mon, 13 Jun 2022 11:45:35 +0200 Subject: [PATCH 06/10] Laikad: Use filter for correcting measurements (#24824) * Update laikad. * Update log error --- SConstruct | 2 +- selfdrive/locationd/laikad.py | 65 ++++++++++++++++++++++------------- 2 files changed, 42 insertions(+), 25 deletions(-) diff --git a/SConstruct b/SConstruct index 0aeb382c23400e..b5c4edc99bff0d 100644 --- a/SConstruct +++ b/SConstruct @@ -359,6 +359,7 @@ Export('cereal', 'messaging', 'visionipc') rednose_config = { 'generated_folder': '#selfdrive/locationd/models/generated', 'to_build': { + 'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []), 'live': ('#selfdrive/locationd/models/live_kf.py', True, ['live_kf_constants.h']), 'car': ('#selfdrive/locationd/models/car_kf.py', True, []), }, @@ -366,7 +367,6 @@ rednose_config = { if arch != "larch64": rednose_config['to_build'].update({ - 'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []), 'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, []), 'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, []), 'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, []), diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 01457f46818c51..42c4156795bad8 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -6,6 +6,8 @@ import numpy as np from collections import defaultdict +from numpy.linalg import linalg + from cereal import log, messaging from laika import AstroDog from laika.constants import SECS_IN_MIN @@ -37,24 +39,33 @@ def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): latest_msg_t = GPSTime(report.gpsWeek, report.rcvTow) self.fetch_orbits(latest_msg_t + SECS_IN_MIN, block) new_meas = read_raw_ublox(report) + processed_measurements = process_measurements(new_meas, self.astro_dog) + pos_fix = calc_pos_fix(processed_measurements, min_measurements=4) - measurements = process_measurements(new_meas, self.astro_dog) - pos_fix = calc_pos_fix(measurements, min_measurements=4) - # To get a position fix a minimum of 5 measurements are needed. - # Each report can contain less and some measurements can't be processed. + t = ublox_mono_time * 1e-9 + kf_pos_std = None + if all(self.kf_valid(t)): + self.gnss_kf.predict(t) + kf_pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal())) + # If localizer is valid use its position to correct measurements + if kf_pos_std is not None and linalg.norm(kf_pos_std) < 100: + est_pos = self.gnss_kf.x[GStates.ECEF_POS] + elif len(pos_fix) > 0 and abs(np.array(pos_fix[1])).mean() < 1000: + est_pos = pos_fix[0][:3] + else: + est_pos = None corrected_measurements = [] - if len(pos_fix) > 0 and abs(np.array(pos_fix[1])).mean() < 1000: - corrected_measurements = correct_measurements(measurements, pos_fix[0][:3], self.astro_dog) + if est_pos is not None: + corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog) - t = ublox_mono_time * 1e-9 self.update_localizer(pos_fix, t, corrected_measurements) - localizer_valid = self.localizer_valid(t) + kf_valid = all(self.kf_valid(t)) ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist() ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist() - pos_std = self.gnss_kf.P[GStates.ECEF_POS].flatten().tolist() - vel_std = self.gnss_kf.P[GStates.ECEF_VELOCITY].flatten().tolist() + pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal())).tolist() + vel_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_VELOCITY].diagonal())).tolist() bearing_deg, bearing_std = get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std) @@ -62,9 +73,9 @@ def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): dat = messaging.new_message("gnssMeasurements") measurement_msg = log.LiveLocationKalman.Measurement.new_message dat.gnssMeasurements = { - "positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=localizer_valid), - "velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=localizer_valid), - "bearingDeg": measurement_msg(value=[bearing_deg], std=[bearing_std], valid=localizer_valid), + "positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=kf_valid), + "velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=kf_valid), + "bearingDeg": measurement_msg(value=[bearing_deg], std=[bearing_std], valid=kf_valid), "ubloxMonoTime": ublox_mono_time, "correctedMeasurements": meas_msgs } @@ -77,18 +88,21 @@ def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): def update_localizer(self, pos_fix, t: float, measurements: List[GNSSMeasurement]): # Check time and outputs are valid - if not self.localizer_valid(t): - # A position fix is needed when resetting the kalman filter. - if len(pos_fix) == 0: - return - post_est = pos_fix[0][:3].tolist() - filter_time = self.gnss_kf.filter.filter_time - if filter_time is None: + valid = self.kf_valid(t) + if not all(valid): + if not valid[0]: cloudlog.info("Init gnss kalman filter") - elif abs(t - filter_time) > MAX_TIME_GAP: + elif not valid[1]: cloudlog.error("Time gap of over 10s detected, gnss kalman reset") - else: + elif not valid[2]: cloudlog.error("Gnss kalman filter state is nan") + else: + cloudlog.error("Gnss kalman std too far") + + if len(pos_fix) == 0: + cloudlog.error("Position fix not available when resetting kalman filter") + return + post_est = pos_fix[0][:3].tolist() self.init_gnss_localizer(post_est) if len(measurements) > 0: kf_add_observations(self.gnss_kf, t, measurements) @@ -96,9 +110,12 @@ def update_localizer(self, pos_fix, t: float, measurements: List[GNSSMeasurement # Ensure gnss filter is updated even with no new measurements self.gnss_kf.predict(t) - def localizer_valid(self, t: float): + def kf_valid(self, t: float): filter_time = self.gnss_kf.filter.filter_time - return filter_time is not None and (t - filter_time) < MAX_TIME_GAP and all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS])) + return [filter_time is not None, + filter_time is not None and abs(t - filter_time) < MAX_TIME_GAP, + all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS])), + linalg.norm(self.gnss_kf.P[GStates.ECEF_POS]) < 1e5] def init_gnss_localizer(self, est_pos): x_initial, p_initial_diag = np.copy(GNSSKalman.x_initial), np.copy(np.diagonal(GNSSKalman.P_initial)) From 1221aef233180a7d290ff813d2e2e53dbf87d586 Mon Sep 17 00:00:00 2001 From: Joost Wooning Date: Mon, 13 Jun 2022 12:27:47 +0200 Subject: [PATCH 07/10] ui: skip texture frame copy (#24700) * ui_blit working * simpler and working * more believable that it's real * working on device * build on pc * use hardware pc * reduce cpu usage * yuv conversion to EGL * move everything to cameraview * some cleanup * more cleanup * init array * init images with std::map * dont destroy images * do destroy images Co-authored-by: Comma Device --- selfdrive/test/test_onroad.py | 2 +- selfdrive/ui/SConscript | 3 ++ selfdrive/ui/qt/widgets/cameraview.cc | 72 ++++++++++++++++++++++++--- selfdrive/ui/qt/widgets/cameraview.h | 17 ++++++- 4 files changed, 84 insertions(+), 10 deletions(-) diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 79c3a2ebc0429f..3586f86f12e749 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -27,7 +27,7 @@ "./camerad": 16.5, "./locationd": 9.1, "selfdrive.controls.plannerd": 11.7, - "./_ui": 26.4, + "./_ui": 19.2, "selfdrive.locationd.paramsd": 9.0, "./_sensord": 6.17, "selfdrive.controls.radard": 4.5, diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 47c4ac2a51273f..28fcc5f56f95e5 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -5,6 +5,9 @@ Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq', 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread'] + qt_env["LIBS"] +if arch == 'larch64': + base_libs.append('EGL') + maps = arch in ['larch64', 'x86_64'] if maps and arch == 'x86_64': diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index f16e8e4e0def08..000ac4847566a8 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -26,6 +26,18 @@ const char frame_vertex_shader[] = " vTexCoord = aTexCoord;\n" "}\n"; +#ifdef QCOM2 +const char frame_fragment_shader[] = + "#version 300 es\n" + "#extension GL_OES_EGL_image_external_essl3 : enable\n" + "precision mediump float;\n" + "uniform samplerExternalOES uTexture;\n" + "in vec2 vTexCoord;\n" + "out vec4 colorOut;\n" + "void main() {\n" + " colorOut = texture(uTexture, vTexCoord);\n" + "}\n"; +#else const char frame_fragment_shader[] = #ifdef __APPLE__ "#version 330 core\n" @@ -45,6 +57,7 @@ const char frame_fragment_shader[] = " float b = y + 1.772 * uv.x;\n" " colorOut = vec4(r, g, b, 1.0);\n" "}\n"; +#endif const mat4 device_transform = {{ 1.0, 0.0, 0.0, 0.0, @@ -99,7 +112,7 @@ CameraViewWidget::~CameraViewWidget() { glDeleteVertexArrays(1, &frame_vao); glDeleteBuffers(1, &frame_vbo); glDeleteBuffers(1, &frame_ibo); - glDeleteBuffers(3, textures); + glDeleteBuffers(2, textures); } doneCurrent(); } @@ -143,10 +156,15 @@ void CameraViewWidget::initializeGL() { glBindBuffer(GL_ARRAY_BUFFER, 0); glBindVertexArray(0); - glGenTextures(3, textures); glUseProgram(program->programId()); + +#ifdef QCOM2 + glUniform1i(program->uniformLocation("uTexture"), 0); +#else + glGenTextures(2, textures); glUniform1i(program->uniformLocation("uTextureY"), 0); glUniform1i(program->uniformLocation("uTextureUV"), 1); +#endif } void CameraViewWidget::showEvent(QShowEvent *event) { @@ -207,29 +225,33 @@ void CameraViewWidget::paintGL() { for (frame_idx = 0; frame_idx < frames.size() - 1; frame_idx++) { if (frames[frame_idx].first == draw_frame_id) break; } - VisionBuf *frame = frames[frame_idx].second; - glPixelStorei(GL_UNPACK_ALIGNMENT, 1); - glPixelStorei(GL_UNPACK_ROW_LENGTH, stream_stride); glViewport(0, 0, width(), height()); glBindVertexArray(frame_vao); - glUseProgram(program->programId()); + glPixelStorei(GL_UNPACK_ALIGNMENT, 1); + VisionBuf *frame = frames[frame_idx].second; + +#ifdef QCOM2 + glActiveTexture(GL_TEXTURE0); + glEGLImageTargetTexture2DOES(GL_TEXTURE_EXTERNAL_OES, egl_images[frame->idx]); + assert(glGetError() == GL_NO_ERROR); +#else + glPixelStorei(GL_UNPACK_ROW_LENGTH, stream_stride); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, textures[0]); glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, stream_width, stream_height, GL_RED, GL_UNSIGNED_BYTE, frame->y); assert(glGetError() == GL_NO_ERROR); glPixelStorei(GL_UNPACK_ROW_LENGTH, stream_stride/2); - glActiveTexture(GL_TEXTURE0 + 1); glBindTexture(GL_TEXTURE_2D, textures[1]); glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, stream_width/2, stream_height/2, GL_RG, GL_UNSIGNED_BYTE, frame->uv); assert(glGetError() == GL_NO_ERROR); +#endif glUniformMatrix4fv(program->uniformLocation("uTransform"), 1, GL_TRUE, frame_mat.v); - assert(glGetError() == GL_NO_ERROR); glEnableVertexAttribArray(0); glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void *)0); glDisableVertexAttribArray(0); @@ -247,6 +269,32 @@ void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { stream_height = vipc_client->buffers[0].height; stream_stride = vipc_client->buffers[0].stride; +#ifdef QCOM2 + egl_display = eglGetCurrentDisplay(); + + for (auto &pair : egl_images) { + eglDestroyImageKHR(egl_display, pair.second); + } + egl_images.clear(); + + for (int i = 0; i < vipc_client->num_buffers; i++) { // import buffers into OpenGL + int fd = dup(vipc_client->buffers[i].fd); // eglDestroyImageKHR will close, so duplicate + EGLint img_attrs[] = { + EGL_WIDTH, (int)vipc_client->buffers[i].width, + EGL_HEIGHT, (int)vipc_client->buffers[i].height, + EGL_LINUX_DRM_FOURCC_EXT, DRM_FORMAT_NV12, + EGL_DMA_BUF_PLANE0_FD_EXT, fd, + EGL_DMA_BUF_PLANE0_OFFSET_EXT, 0, + EGL_DMA_BUF_PLANE0_PITCH_EXT, (int)vipc_client->buffers[i].stride, + EGL_DMA_BUF_PLANE1_FD_EXT, fd, + EGL_DMA_BUF_PLANE1_OFFSET_EXT, (int)vipc_client->buffers[i].uv_offset, + EGL_DMA_BUF_PLANE1_PITCH_EXT, (int)vipc_client->buffers[i].stride, + EGL_NONE + }; + egl_images[i] = eglCreateImageKHR(egl_display, EGL_NO_CONTEXT, EGL_LINUX_DMA_BUF_EXT, 0, img_attrs); + assert(eglGetError() == EGL_SUCCESS); + } +#else glBindTexture(GL_TEXTURE_2D, textures[0]); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); @@ -262,6 +310,7 @@ void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); glTexImage2D(GL_TEXTURE_2D, 0, GL_RG8, stream_width/2, stream_height/2, 0, GL_RG, GL_UNSIGNED_BYTE, nullptr); assert(glGetError() == GL_NO_ERROR); +#endif updateFrameMat(width(), height()); } @@ -297,4 +346,11 @@ void CameraViewWidget::vipcThread() { emit vipcThreadFrameReceived(buf, meta_main.frame_id); } } + +#ifdef QCOM2 + for (auto &pair : egl_images) { + eglDestroyImageKHR(egl_display, pair.second); + } + egl_images.clear(); +#endif } diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 953cbed00b5cfc..ddc3fc253b3a41 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -6,6 +6,16 @@ #include #include #include + +#ifdef QCOM2 +#define EGL_EGLEXT_PROTOTYPES +#define EGL_NO_X11 +#define GL_TEXTURE_EXTERNAL_OES 0x8D65 +#include +#include +#include +#endif + #include "cereal/visionipc/visionipc_client.h" #include "selfdrive/camerad/cameras/camera_common.h" #include "selfdrive/ui/ui.h" @@ -41,11 +51,16 @@ class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions { bool zoomed_view; GLuint frame_vao, frame_vbo, frame_ibo; - GLuint textures[3]; + GLuint textures[2]; mat4 frame_mat; std::unique_ptr program; QColor bg = QColor("#000000"); +#ifdef QCOM2 + EGLDisplay egl_display; + std::map egl_images; +#endif + std::string stream_name; int stream_width = 0; int stream_height = 0; From a2d2378ee147f2db59aecd3401677cb2427d1ced Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Mon, 13 Jun 2022 14:02:31 +0200 Subject: [PATCH 08/10] Laikad: process executor to fetch orbits (#24843) * Use ProcessPoolExecutor to fetch orbits * update laika repo * Minor --- laika_repo | 2 +- selfdrive/locationd/laikad.py | 64 ++++++++++++------------- selfdrive/locationd/test/test_laikad.py | 33 +++++++++---- 3 files changed, 57 insertions(+), 42 deletions(-) diff --git a/laika_repo b/laika_repo index d87194613455b4..36f2621fc53484 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit d87194613455b42af19ff2b5a3f7d1cae5852885 +Subproject commit 36f2621fc5348487bb2cd606c37c8c15de0e32cd diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 42c4156795bad8..33e41398a0b440 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 import time -from multiprocessing import Process, Queue +from concurrent.futures import Future, ProcessPoolExecutor from typing import List, Optional import numpy as np @@ -10,7 +10,7 @@ from cereal import log, messaging from laika import AstroDog -from laika.constants import SECS_IN_MIN +from laika.constants import SECS_IN_HR, SECS_IN_MIN from laika.ephemeris import EphemerisType, convert_ublox_ephem from laika.gps_time import GPSTime from laika.helpers import ConstellationId @@ -29,8 +29,9 @@ class Laikad: def __init__(self, valid_const=("GPS", "GLONASS"), auto_update=False, valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV)): self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types) self.gnss_kf = GNSSKalman(GENERATED_DIR) - self.orbit_p: Optional[Process] = None - self.orbit_q = Queue() + self.orbit_fetch_executor = ProcessPoolExecutor() + self.orbit_fetch_future: Optional[Future] = None + self.last_fetch_orbits_t = None def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): if ublox_msg.which == 'measurementReport': @@ -82,7 +83,7 @@ def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): return dat elif ublox_msg.which == 'ephemeris': ephem = convert_ublox_ephem(ublox_msg.ephemeris) - self.astro_dog.add_ephems([ephem], self.astro_dog.nav) + self.astro_dog.add_navs([ephem]) # elif ublox_msg.which == 'ionoData': # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. @@ -100,7 +101,7 @@ def update_localizer(self, pos_fix, t: float, measurements: List[GNSSMeasurement cloudlog.error("Gnss kalman std too far") if len(pos_fix) == 0: - cloudlog.error("Position fix not available when resetting kalman filter") + cloudlog.warning("Position fix not available when resetting kalman filter") return post_est = pos_fix[0][:3].tolist() self.init_gnss_localizer(post_est) @@ -124,36 +125,33 @@ def init_gnss_localizer(self, est_pos): self.gnss_kf.init_state(x_initial, covs_diag=p_initial_diag) - def get_orbit_data(self, t: GPSTime, queue): - cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}") - start_time = time.monotonic() - try: - self.astro_dog.get_orbit_data(t, only_predictions=True) - except RuntimeError as e: - cloudlog.info(f"No orbit data found. {e}") - return - cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.2f}s") - if queue is not None: - queue.put((self.astro_dog.orbits, self.astro_dog.orbit_fetched_times)) - def fetch_orbits(self, t: GPSTime, block): - if t not in self.astro_dog.orbit_fetched_times: - if block: - self.get_orbit_data(t, None) - return - if self.orbit_p is None: - self.orbit_p = Process(target=self.get_orbit_data, args=(t, self.orbit_q)) - self.orbit_p.start() - if not self.orbit_q.empty(): - ret = self.orbit_q.get() + if t not in self.astro_dog.orbit_fetched_times and (self.last_fetch_orbits_t is None or t - self.last_fetch_orbits_t > SECS_IN_HR): + astro_dog_vars = self.astro_dog.valid_const, self.astro_dog.auto_update, self.astro_dog.valid_ephem_types + if self.orbit_fetch_future is None: + self.orbit_fetch_future = self.orbit_fetch_executor.submit(get_orbit_data, t, *astro_dog_vars) + if block: + self.orbit_fetch_future.result() + if self.orbit_fetch_future.done(): + ret = self.orbit_fetch_future.result() if ret: self.astro_dog.orbits, self.astro_dog.orbit_fetched_times = ret - self.orbit_p.join() - self.orbit_p = None - - def __del__(self): - if self.orbit_p is not None: - self.orbit_p.kill() + self.orbit_fetch_future = None + self.last_fetch_orbits_t = t + + +def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types): + astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types) + cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}") + start_time = time.monotonic() + data = None + try: + astro_dog.get_orbit_data(t, only_predictions=True) + data = (astro_dog.orbits, astro_dog.orbit_fetched_times) + except RuntimeError as e: + cloudlog.info(f"No orbit data found. {e}") + cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.1f}s") + return data def create_measurement_msg(meas: GNSSMeasurement): diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 7a8c1ecb13564d..7dc803d79919e1 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -69,29 +69,46 @@ def test_laika_offline(self, downloader_mock): self.assertEqual(256, len(correct_msgs)) self.assertEqual(256, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) - def test_laika_get_orbits(self): - laikad = Laikad(auto_update=False) - first_gps_time = None + def get_first_gps_time(self): for m in self.logs: if m.ubloxGnss.which == 'measurementReport': new_meas = read_raw_ublox(m.ubloxGnss.measurementReport) if len(new_meas) != 0: - first_gps_time = new_meas[0].recv_time - break + return new_meas[0].recv_time + + def test_laika_get_orbits(self): + laikad = Laikad(auto_update=False) + first_gps_time = self.get_first_gps_time() # Pretend process has loaded the orbits on startup by using the time of the first gps message. laikad.fetch_orbits(first_gps_time, block=True) - self.assertEqual(29, len(laikad.astro_dog.orbits.keys())) + self.assertEqual(29, len(laikad.astro_dog.orbits.values())) + self.assertGreater(min([len(v) for v in laikad.astro_dog.orbits.values()]), 0) @unittest.skip("Use to debug live data") def test_laika_get_orbits_now(self): laikad = Laikad(auto_update=False) laikad.fetch_orbits(GPSTime.from_datetime(datetime.utcnow()), block=True) prn = "G01" - self.assertLess(0, len(laikad.astro_dog.orbits[prn])) + self.assertGreater(len(laikad.astro_dog.orbits[prn]), 0) prn = "R01" - self.assertLess(0, len(laikad.astro_dog.orbits[prn])) + self.assertGreater(len(laikad.astro_dog.orbits[prn]), 0) print(min(laikad.astro_dog.orbits[prn], key=lambda e: e.epoch).epoch.as_datetime()) + def test_get_orbits_in_process(self): + laikad = Laikad(auto_update=False) + has_orbits = False + for m in self.logs: + laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime, block=False) + if laikad.orbit_fetch_future is not None: + laikad.orbit_fetch_future.result() + vals = laikad.astro_dog.orbits.values() + has_orbits = len(vals) > 0 and max([len(v) for v in vals]) > 0 + if has_orbits: + break + self.assertTrue(has_orbits) + self.assertGreater(len(laikad.astro_dog.orbit_fetched_times._ranges), 0) + self.assertEqual(None, laikad.orbit_fetch_future) + if __name__ == "__main__": unittest.main() From fb068f04f50fcde6906072b4963114146852c51d Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 13 Jun 2022 16:43:50 +0200 Subject: [PATCH 09/10] camerad: remove unused SubMaster (#24844) --- selfdrive/camerad/cameras/camera_qcom2.cc | 7 ++----- selfdrive/camerad/cameras/camera_qcom2.h | 1 - selfdrive/camerad/test/camera_test.h | 1 - 3 files changed, 2 insertions(+), 7 deletions(-) diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index d43beb0921e0f0..c1ffc1275a178d 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -837,7 +837,6 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i s->road_cam.camera_init(s, v, CAMERA_ID_AR0231, 1, 20, device_id, ctx, VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD, !env_disable_road); s->wide_road_cam.camera_init(s, v, CAMERA_ID_AR0231, 0, 20, device_id, ctx, VISION_STREAM_RGB_WIDE_ROAD, VISION_STREAM_WIDE_ROAD, !env_disable_wide_road); - s->sm = new SubMaster({"driverState"}); s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"}); } @@ -948,7 +947,6 @@ void cameras_close(MultiCameraState *s) { s->road_cam.camera_close(); s->wide_road_cam.camera_close(); - delete s->sm; delete s->pm; } @@ -1221,7 +1219,7 @@ static void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal framed.setTemperaturesC({temp_0, temp_1}); } -static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) { +static void driver_cam_auto_exposure(CameraState *c) { struct ExpRect {int x1, x2, x_skip, y1, y2, y_skip;}; const CameraBuf *b = &c->buf; static ExpRect rect = {96, 1832, 2, 242, 1148, 4}; @@ -1229,8 +1227,7 @@ static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) { } static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) { - s->sm->update(0); - driver_cam_auto_exposure(c, *(s->sm)); + driver_cam_auto_exposure(c); MessageBuilder msg; auto framed = msg.initEvent().initDriverCameraState(); diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h index d869620e9ae7d4..88766a68e99e6a 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.h +++ b/selfdrive/camerad/cameras/camera_qcom2.h @@ -106,6 +106,5 @@ typedef struct MultiCameraState { CameraState wide_road_cam; CameraState driver_cam; - SubMaster *sm; PubMaster *pm; } MultiCameraState; diff --git a/selfdrive/camerad/test/camera_test.h b/selfdrive/camerad/test/camera_test.h index cc521db39769b2..a9f213c923a40c 100644 --- a/selfdrive/camerad/test/camera_test.h +++ b/selfdrive/camerad/test/camera_test.h @@ -19,7 +19,6 @@ typedef struct MultiCameraState { CameraState road_cam; CameraState driver_cam; - SubMaster *sm = nullptr; PubMaster *pm = nullptr; } MultiCameraState; From 7bca95dbb89dc6b969871cc7f6bf4306d91f60cc Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 13 Jun 2022 16:44:38 +0200 Subject: [PATCH 10/10] navd: speed limits only when localizer is valid (#24845) --- selfdrive/navd/navd.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index fcd53d8b60cd02..509653a46daaf6 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -32,6 +32,7 @@ def __init__(self, sm, pm): self.last_bearing = None self.gps_ok = False + self.localizer_valid = False self.nav_destination = None self.step_idx = None @@ -73,9 +74,9 @@ def update_location(self): location = self.sm['liveLocationKalman'] self.gps_ok = location.gpsOK - localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid + self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid - if localizer_valid: + if self.localizer_valid: self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2]) self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1]) @@ -202,7 +203,7 @@ def send_instruction(self): if along_geometry < distance_along_geometry(geometry, geometry[closest_idx]): closest = geometry[closest_idx - 1] - if 'maxspeed' in closest.annotations: + if ('maxspeed' in closest.annotations) and self.localizer_valid: msg.navInstruction.speedLimit = closest.annotations['maxspeed'] # Speed limit sign type