From 1e702de434f10e80c22f7424d6c41edb8c2edf30 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Mon, 2 Jun 2025 03:39:41 +0800 Subject: [PATCH] system/ui: add face detection box and driver state icon to DriverCameraView (#35410) add face detection box and driver state icon to DriverCameraView --- system/ui/widgets/driver_camera_view.py | 56 +++++++++++++++++++++++-- 1 file changed, 52 insertions(+), 4 deletions(-) diff --git a/system/ui/widgets/driver_camera_view.py b/system/ui/widgets/driver_camera_view.py index 174fac378d..56dcbf2b9e 100644 --- a/system/ui/widgets/driver_camera_view.py +++ b/system/ui/widgets/driver_camera_view.py @@ -1,18 +1,63 @@ import numpy as np import pyray as rl +from cereal import messaging from openpilot.system.ui.widgets.cameraview import CameraView from msgq.visionipc import VisionStreamType -from openpilot.system.ui.lib.application import gui_app +from openpilot.system.ui.lib.application import gui_app, FontWeight +from openpilot.system.ui.lib.label import gui_label +from openpilot.system.ui.onroad.driver_state import DriverStateRenderer class DriverCameraView(CameraView): def __init__(self, stream_type: VisionStreamType): super().__init__("camerad", stream_type) + self.driver_state_renderer = DriverStateRenderer() - def render(self, rect): + def render(self, rect, sm): super().render(rect) - # TODO: Add additional rendering logic + if not self.frame: + gui_label( + rect, + "camera starting", + font_size=100, + font_weight=FontWeight.BOLD, + alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER, + ) + return + + self._draw_face_detection(rect, sm) + self.driver_state_renderer.draw(rect, sm) + + def _draw_face_detection(self, rect: rl.Rectangle, sm) -> None: + driver_state = sm["driverStateV2"] + is_rhd = driver_state.wheelOnRightProb > 0.5 + driver_data = driver_state.rightDriverData if is_rhd else driver_state.leftDriverData + face_detect = driver_data.faceProb > 0.7 + if not face_detect: + return + + # Get face position and orientation + face_x, face_y = driver_data.facePosition + face_std = max(driver_data.faceOrientationStd[0], driver_data.faceOrientationStd[1]) + alpha = 0.7 + if face_std > 0.15: + alpha = max(0.7 - (face_std - 0.15) * 3.5, 0.0) + + # use approx instead of distort_points + # TODO: replace with distort_points + fbox_x = int(1080.0 - 1714.0 * face_x) + fbox_y = int(-135.0 + (504.0 + abs(face_x) * 112.0) + (1205.0 - abs(face_x) * 724.0) * face_y) + box_size = 220 + + line_color = rl.Color(255, 255, 255, int(alpha * 255)) + rl.draw_rectangle_rounded_lines_ex( + rl.Rectangle(fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size), + 35.0 / box_size / 2, + 10, + 10, + line_color, + ) def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: driver_view_ratio = 2.0 @@ -38,9 +83,12 @@ class DriverCameraView(CameraView): if __name__ == "__main__": gui_app.init_window("Driver Camera View") + sm = messaging.SubMaster(["selfdriveState", "driverStateV2", "driverMonitoringState"]) + driver_camera_view = DriverCameraView(VisionStreamType.VISION_STREAM_DRIVER) try: for _ in gui_app.render(): - driver_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) + sm.update() + driver_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height), sm) finally: driver_camera_view.close()