try the gm method

# Conflicts:
#	opendbc_repo

.vscode/launch.json

@@ -52,6 +52,9 @@
       "type": "lldb",
       "request": "attach",
       "pid": "${command:pickMyProcess}",
+      "sourceMap": {
+        ".": "${workspaceFolder}/opendbc/safety"
+      },
       "initCommands": [
         "script import time; time.sleep(3)"
       ]

cereal/log.capnp

@@ -613,6 +613,11 @@ struct PandaState @0xa7649e2575e4591e {
   voltage @0 :UInt32;
   current @1 :UInt32;
 
+  # these fields are not used by openpilot, but they're
+  # reserved for forks building alternate experiences.
+  controlsAllowedRESERVED1 @38 :Bool;
+  controlsAllowedRESERVED2 @39 :Bool;
+
   enum FaultStatus {
     none @0;
     faultTemp @1;

selfdrive/modeld/modeld.py

@@ -405,7 +405,9 @@ def main(demo=False):
       drivingdata_send = messaging.new_message('drivingModelData')
       posenet_send = messaging.new_message('cameraOdometry')
 
-      action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego)
+      frame_delay = DT_MDL # compensate for time passed since the frame was captured: current_time - timestamp_eof is 50ms on average
+      action_delay = DT_MDL / 2 # middle of the interval between model output (current state) and next frame (expected state)
+      action = get_action_from_model(model_output, prev_action, lat_delay + frame_delay + action_delay, long_delay + frame_delay + action_delay, v_ego)
       prev_action = action
       fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
                      publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,

tools/replay/lib/ui_helpers.py

@@ -5,6 +5,7 @@ import numpy as np
 import pyray as rl
 
 from matplotlib.backends.backend_agg import FigureCanvasAgg
+from matplotlib.offsetbox import AnchoredOffsetbox, HPacker, TextArea
 
 from openpilot.common.transformations.camera import get_view_frame_from_calib_frame
 from openpilot.selfdrive.controls.radard import RADAR_TO_CAMERA
@@ -94,6 +95,7 @@ def draw_path(path, color, img, calibration, top_down, lid_color=None, z_off=0):
 
 def init_plots(arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_colors, plot_styles):
   color_palette = {"r": (1, 0, 0), "g": (0, 1, 0), "b": (0, 0, 1), "k": (0, 0, 0), "y": (1, 1, 0), "p": (0, 1, 1), "m": (1, 0, 1)}
+  label_palette = {**color_palette, "b": (43/255, 114/255, 1.0)}
 
   dpi = 90
   fig = plt.figure(figsize=(575 / dpi, 600 / dpi), dpi=dpi)
@@ -116,10 +118,18 @@ def init_plots(arr, name_to_arr_idx, plot_xlims, plot_ylims, plot_names, plot_co
       plots.append(plot)
       idxs.append(name_to_arr_idx[item])
       plot_select.append(i)
-    axs[i].set_title(", ".join(f"{nm} ({cl})" for (nm, cl) in zip(pl_list, plot_colors[i], strict=False)), fontsize=10)
+    # Build colored title: each label colored to match its plot line
+    title_texts = []
+    for j2, (nm, cl) in enumerate(zip(pl_list, plot_colors[i], strict=False)):
+      if j2 > 0:
+        title_texts.append(TextArea(", ", textprops=dict(color="white", fontsize=10)))
+      title_texts.append(TextArea(nm, textprops=dict(color=label_palette[cl], fontsize=10)))
+    packed = HPacker(children=title_texts, pad=0, sep=0)
+    ab = AnchoredOffsetbox(loc='lower center', child=packed, bbox_to_anchor=(0.5, 1.0),
+                           bbox_transform=axs[i].transAxes, frameon=False, pad=0)
+    axs[i].add_artist(ab)
     axs[i].tick_params(axis="x", colors="white")
     axs[i].tick_params(axis="y", colors="white")
-    axs[i].title.set_color("white")
 
     if i < len(plot_ylims) - 1:
       axs[i].set_xticks([])

tools/replay/ui.py

@@ -3,7 +3,6 @@ import argparse
 import os
 import sys
 
-import cv2
 import numpy as np
 import pyray as rl
 
@@ -22,7 +21,8 @@ from openpilot.tools.replay.lib.ui_helpers import (
   plot_lead,
   plot_model,
 )
-from msgq.visionipc import VisionIpcClient, VisionStreamType
+from msgq.visionipc import VisionStreamType
+from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
 
 os.environ['BASEDIR'] = BASEDIR
 
@@ -30,8 +30,6 @@ ANGLE_SCALE = 5.0
 
 
 def ui_thread(addr):
-  cv2.setNumThreads(1)
-
   # Get monitor info before creating window
   rl.set_config_flags(rl.ConfigFlags.FLAG_MSAA_4X_HINT)
   rl.init_window(1, 1, "")
@@ -59,14 +57,15 @@ def ui_thread(addr):
   font_path = os.path.join(BASEDIR, "selfdrive/assets/fonts/JetBrainsMono-Medium.ttf")
   font = rl.load_font_ex(font_path, 32, None, 0)
 
-  # Create textures for camera and top-down view
-  camera_image = rl.gen_image_color(640, 480, rl.BLACK)
-  camera_texture = rl.load_texture_from_image(camera_image)
-  rl.unload_image(camera_image)
+  camera_view = CameraView("camerad", VisionStreamType.VISION_STREAM_ROAD)
+
+  # Overlay texture for model/lane line drawing
+  overlay_img = np.zeros((480, 640, 4), dtype='uint8')
+  overlay_image = rl.gen_image_color(640, 480, rl.BLANK)
+  overlay_texture = rl.load_texture_from_image(overlay_image)
+  rl.unload_image(overlay_image)
 
   # lid_overlay array is (lidar_x, lidar_y) = (384, 960)
-  # pygame treats first axis as width, so texture is 384 wide x 960 tall
-  # For raylib, we need to transpose to get (height, width) = (960, 384) for the RGBA array
   top_down_image = rl.gen_image_color(UP.lidar_x, UP.lidar_y, rl.BLACK)
   top_down_texture = rl.load_texture_from_image(top_down_image)
   rl.unload_image(top_down_image)
@@ -89,7 +88,6 @@ def ui_thread(addr):
   )
 
   img = np.zeros((480, 640, 3), dtype='uint8')
-  imgff = None
   num_px = 0
   calibration = None
 
@@ -116,7 +114,7 @@ def ui_thread(addr):
   plot_arr = np.zeros((100, len(name_to_arr_idx.values())))
 
   plot_xlims = [(0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0]), (0, plot_arr.shape[0])]
-  plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0.0, 75.0), (-3.0, 2.0)]
+  plot_ylims = [(-0.1, 1.1), (-ANGLE_SCALE, ANGLE_SCALE), (0.0, 75.0), (-3.5, 2.0)]
   plot_names = [
     ["gas", "computer_gas", "user_brake", "computer_brake"],
     ["angle_steers", "angle_steers_des", "angle_steers_k", "steer_torque"],
@@ -138,20 +136,16 @@ def ui_thread(addr):
   palette[110] = [110, 110, 110, 255]  # car_color (gray)
   palette[255] = [255, 255, 255, 255]  # WHITE
 
-  vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
   while not rl.window_should_close():
-    # ***** frame *****
-    if not vipc_client.is_connected():
-      vipc_client.connect(False)
-
     rl.begin_drawing()
     rl.clear_background(rl.Color(64, 64, 64, 255))
 
-    yuv_img_raw = vipc_client.recv()
-    if yuv_img_raw is None or not yuv_img_raw.data.any():
-      rl.draw_text_ex(font, "waiting for frames", rl.Vector2(200, 200), 30, 0, rl.WHITE)
-      rl.end_drawing()
-      continue
+    # Render camera (NV12->RGB on GPU via shader)
+    if camera_view.frame:
+      cam_h = 640.0 * camera_view.frame.height / camera_view.frame.width
+    else:
+      cam_h = 480.0
+    camera_view.render(rl.Rectangle(0, 0, 640, cam_h))
 
     lid_overlay = lid_overlay_blank.copy()
     top_down = top_down_texture, lid_overlay
@@ -159,19 +153,10 @@ def ui_thread(addr):
     sm.update(0)
 
     camera = DEVICE_CAMERAS[("tici", str(sm['roadCameraState'].sensor))]
-
-    # Use received buffer dimensions (full HEVC can have stride != buffer_len/rows due to VENUS padding)
-    h, w, stride = yuv_img_raw.height, yuv_img_raw.width, yuv_img_raw.stride
-    nv12_size = h * 3 // 2 * stride
-    imgff = np.frombuffer(yuv_img_raw.data, dtype=np.uint8, count=nv12_size).reshape((h * 3 // 2, stride))
-    num_px = w * h
-    rgb = cv2.cvtColor(imgff[: h * 3 // 2, : w], cv2.COLOR_YUV2RGB_NV12)
-
-    qcam = "QCAM" in os.environ
-    bb_scale = (528 if qcam else camera.fcam.width) / 640.0
     calib_scale = camera.fcam.width / 640.0
-    zoom_matrix = np.asarray([[bb_scale, 0.0, 0.0], [0.0, bb_scale, 0.0], [0.0, 0.0, 1.0]])
-    cv2.warpAffine(rgb, zoom_matrix[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
+
+    if camera_view.frame:
+      num_px = camera_view.frame.width * camera_view.frame.height
 
     intrinsic_matrix = camera.fcam.intrinsics
 
@@ -183,7 +168,8 @@ def ui_thread(addr):
     else:
       angle_steers_k = np.inf
 
-    plot_arr[:-1] = plot_arr[1:]
+    if sm.updated['carState']:
+      plot_arr[:-1] = plot_arr[1:]
     plot_arr[-1, name_to_arr_idx['angle_steers']] = sm['carState'].steeringAngleDeg
     plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steeringAngleDeg
     plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k
@@ -198,9 +184,10 @@ def ui_thread(addr):
     plot_arr[-1, name_to_arr_idx['v_cruise']] = sm['carState'].cruiseState.speed
     plot_arr[-1, name_to_arr_idx['a_ego']] = sm['carState'].aEgo
 
-    if len(sm['longitudinalPlan'].accels):
-      plot_arr[-1, name_to_arr_idx['a_target']] = sm['longitudinalPlan'].accels[0]
+    plot_arr[-1, name_to_arr_idx['a_target']] = sm['longitudinalPlan'].aTarget
 
+    # Draw model overlays onto img, then blit as transparent overlay
+    img[:] = 0
     if sm.recv_frame['modelV2']:
       plot_model(sm['modelV2'], img, calibration, top_down)
 
@@ -214,11 +201,12 @@ def ui_thread(addr):
       rpyCalib = np.asarray(sm['liveCalibration'].rpyCalib)
       calibration = Calibration(num_px, rpyCalib, intrinsic_matrix, calib_scale)
 
-    # *** blits ***
-    # Update camera texture from numpy array
-    img_rgba = cv2.cvtColor(img, cv2.COLOR_RGB2RGBA)
-    rl.update_texture(camera_texture, rl.ffi.cast("void *", img_rgba.ctypes.data))
-    rl.draw_texture(camera_texture, 0, 0, rl.WHITE)  # noqa: TID251
+    # Update overlay texture (RGB img -> RGBA with non-black pixels visible)
+    mask = np.any(img > 0, axis=2)
+    overlay_img[:, :, :3] = img
+    overlay_img[:, :, 3] = mask * 255
+    rl.update_texture(overlay_texture, rl.ffi.cast("void *", overlay_img.ctypes.data))
+    rl.draw_texture(overlay_texture, 0, 0, rl.WHITE)  # noqa: TID251
 
     # display alerts
     rl.draw_text_ex(font, sm['selfdriveState'].alertText1, rl.Vector2(180, 150), 30, 0, rl.RED)
@@ -257,9 +245,10 @@ def ui_thread(addr):
 
     rl.end_drawing()
 
-  rl.unload_texture(camera_texture)
+  rl.unload_texture(overlay_texture)
   rl.unload_texture(top_down_texture)
   rl.unload_font(font)
+  camera_view.close()
   rl.close_window()