From 578102a984511a09506ebc2d549c92b9705759e6 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 18 Sep 2025 00:11:45 -0500 Subject: [PATCH] Tinygrad Dmonitoring --- common/transformations/model.py | 9 ++++++++- .../tinygrad_modeld/dmonitoringmodeld.py | 19 +++++++++++++++++-- selfdrive/monitoring/dmonitoringd.py | 7 ++----- selfdrive/monitoring/helpers.py | 12 ++++++------ system/manager/process_config.py | 2 +- 5 files changed, 34 insertions(+), 15 deletions(-) diff --git a/common/transformations/model.py b/common/transformations/model.py index aaa12d776..ea1dff30e 100644 --- a/common/transformations/model.py +++ b/common/transformations/model.py @@ -1,7 +1,7 @@ import numpy as np from openpilot.common.transformations.orientation import rot_from_euler -from openpilot.common.transformations.camera import get_view_frame_from_calib_frame, view_frame_from_device_frame +from openpilot.common.transformations.camera import get_view_frame_from_calib_frame, view_frame_from_device_frame, _ar_ox_fisheye # segnet SEGNET_SIZE = (512, 384) @@ -39,6 +39,13 @@ sbigmodel_intrinsics = np.array([ [0.0, sbigmodel_fl, 0.5 * (256 + MEDMODEL_CY)], [0.0, 0.0, 1.0]]) +DM_INPUT_SIZE = (1440, 960) +dmonitoringmodel_fl = _ar_ox_fisheye.focal_length +dmonitoringmodel_intrinsics = np.array([ + [dmonitoringmodel_fl, 0.0, DM_INPUT_SIZE[0]/2], + [0.0, dmonitoringmodel_fl, DM_INPUT_SIZE[1]/2 - (_ar_ox_fisheye.height - DM_INPUT_SIZE[1])/2], + [0.0, 0.0, 1.0]]) + bigmodel_frame_from_calib_frame = np.dot(bigmodel_intrinsics, get_view_frame_from_calib_frame(0, 0, 0, 0)) diff --git a/frogpilot/tinygrad_modeld/dmonitoringmodeld.py b/frogpilot/tinygrad_modeld/dmonitoringmodeld.py index 51d14c89c..643c3cd49 100644 --- a/frogpilot/tinygrad_modeld/dmonitoringmodeld.py +++ b/frogpilot/tinygrad_modeld/dmonitoringmodeld.py @@ -10,6 +10,7 @@ import pickle import ctypes import numpy as np from pathlib import Path +from setproctitle import setproctitle from cereal import messaging from cereal.messaging import PubMaster, SubMaster @@ -20,6 +21,7 @@ from openpilot.common.transformations.model import dmonitoringmodel_intrinsics, from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye from openpilot.frogpilot.tinygrad_modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame from openpilot.frogpilot.tinygrad_modeld.parse_model_outputs import sigmoid +from openpilot.system import sentry from openpilot.frogpilot.tinygrad_modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address MODEL_WIDTH, MODEL_HEIGHT = DM_INPUT_SIZE @@ -31,6 +33,9 @@ PROCESS_NAME = "frogpilot.tinygrad_modeld.dmonitoringmodeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl' +# Fixed small stagger to reduce overlap with driving model GPU work +DM_STAGGER_SEC = 0.010 + class DriverStateResult(ctypes.Structure): _fields_ = [ @@ -118,7 +123,7 @@ def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: ds = msg.driverStateV2 ds.frameId = frame_id ds.modelExecutionTime = execution_time - ds.gpuExecutionTime = gpu_execution_time + ds.dspExecutionTime = gpu_execution_time ds.poorVisionProb = float(sigmoid(model_result.poor_vision_prob)) ds.wheelOnRightProb = float(sigmoid(model_result.wheel_on_right_prob)) ds.rawPredictions = model_output.tobytes() if SEND_RAW_PRED else b'' @@ -128,8 +133,12 @@ def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: def main(): + setproctitle(PROCESS_NAME) config_realtime_process(7, 5) + sentry.set_tag("daemon", PROCESS_NAME) + cloudlog.bind(daemon=PROCESS_NAME) + cl_context = CLContext() model = ModelState(cl_context) cloudlog.warning("models loaded, dmonitoringmodeld starting") @@ -160,6 +169,9 @@ def main(): if sm.updated["liveCalibration"]: calib[:] = np.array(sm["liveCalibration"].rpyCalib) + # Small fixed delay to reduce GPU overlap with driving model + time.sleep(DM_STAGGER_SEC) + t1 = time.perf_counter() model_output, gpu_execution_time = model.run(buf, calib, model_transform) t2 = time.perf_counter() @@ -171,4 +183,7 @@ if __name__ == "__main__": try: main() except KeyboardInterrupt: - cloudlog.warning("got SIGINT") + cloudlog.warning(f"child {PROCESS_NAME} got SIGINT") + except Exception: + sentry.capture_exception() + raise diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index da09e2bc0..822f22aa4 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -1,15 +1,12 @@ #!/usr/bin/env python3 -import gc - import cereal.messaging as messaging from openpilot.common.params import Params -from openpilot.common.realtime import set_realtime_priority +from openpilot.common.realtime import config_realtime_process from openpilot.selfdrive.monitoring.helpers import DriverMonitoring def dmonitoringd_thread(): - gc.disable() - set_realtime_priority(2) + config_realtime_process([0, 1, 2, 3], 5) params = Params() pm = messaging.PubMaster(['driverMonitoringState']) diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py index aaee4fa1e..9974a3888 100644 --- a/selfdrive/monitoring/helpers.py +++ b/selfdrive/monitoring/helpers.py @@ -1,9 +1,9 @@ from math import atan2 +import numpy as np from cereal import car import cereal.messaging as messaging from openpilot.selfdrive.controls.lib.events import Events -from openpilot.common.numpy_fast import interp from openpilot.common.realtime import DT_DMON from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.stat_live import RunningStatFilter @@ -33,8 +33,8 @@ class DRIVER_MONITOR_SETTINGS: self._SG_THRESHOLD = 0.9 self._BLINK_THRESHOLD = 0.865 - self._EE_THRESH11 = 0.25 - self._EE_THRESH12 = 7.5 + self._EE_THRESH11 = 0.4 + self._EE_THRESH12 = 15.0 self._EE_MAX_OFFSET1 = 0.06 self._EE_MIN_OFFSET1 = 0.025 self._EE_THRESH21 = 0.01 @@ -57,7 +57,7 @@ class DRIVER_MONITOR_SETTINGS: self._POSESTD_THRESHOLD = 0.3 self._HI_STD_FALLBACK_TIME = int(10 / self._DT_DMON) # fall back to wheel touch if model is uncertain for 10s self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz - self._ALWAYS_ON_ALERT_MIN_SPEED = 7 + self._ALWAYS_ON_ALERT_MIN_SPEED = 11 self._POSE_CALIB_MIN_SPEED = 13 # 30 mph self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON) # valid data counts before calibration completes, 1min cumulative @@ -205,10 +205,10 @@ class DriverMonitoring: bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2) bp_normal = max(min(bp / k1, 0.5),0) - self.pose.cfactor_pitch = interp(bp_normal, [0, 0.5], + self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5], [self.settings._POSE_PITCH_THRESHOLD_SLACK, self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD - self.pose.cfactor_yaw = interp(bp_normal, [0, 0.5], + self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5], [self.settings._POSE_YAW_THRESHOLD_SLACK, self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD diff --git a/system/manager/process_config.py b/system/manager/process_config.py index c9348ab39..b3ee542f8 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -70,7 +70,7 @@ procs = [ PythonProcess("micd", "system.micd", iscar), PythonProcess("timed", "system.timed", always_run, enabled=not PC), - PythonProcess("dmonitoringmodeld", "selfdrive.modeld.dmonitoringmodeld", driverview, enabled=(not PC or WEBCAM)), + PythonProcess("dmonitoringmodeld", "frogpilot.tinygrad_modeld.dmonitoringmodeld", driverview, enabled=(not PC or WEBCAM)), NativeProcess("encoderd", "system/loggerd", ["./encoderd"], allow_logging), NativeProcess("stream_encoderd", "system/loggerd", ["./encoderd", "--stream"], notcar), NativeProcess("loggerd", "system/loggerd", ["./loggerd"], allow_logging),