|
|
|
@ -26,6 +26,7 @@ from openpilot.common.transformations.model import dmonitoringmodel_intrinsics, |
|
|
|
|
from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye |
|
|
|
|
from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame |
|
|
|
|
from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid |
|
|
|
|
from openpilot.system import sentry |
|
|
|
|
|
|
|
|
|
MODEL_WIDTH, MODEL_HEIGHT = DM_INPUT_SIZE |
|
|
|
|
CALIB_LEN = 3 |
|
|
|
@ -37,6 +38,7 @@ SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') |
|
|
|
|
MODEL_PATH = Path(__file__).parent / 'models/dmonitoring_model.onnx' |
|
|
|
|
MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl' |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class DriverStateResult(ctypes.Structure): |
|
|
|
|
_fields_ = [ |
|
|
|
|
("face_orientation", ctypes.c_float*3), |
|
|
|
@ -55,6 +57,7 @@ class DriverStateResult(ctypes.Structure): |
|
|
|
|
("ready_prob", ctypes.c_float*4), |
|
|
|
|
("not_ready_prob", ctypes.c_float*2)] |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class DMonitoringModelResult(ctypes.Structure): |
|
|
|
|
_fields_ = [ |
|
|
|
|
("driver_state_lhd", DriverStateResult), |
|
|
|
@ -63,6 +66,7 @@ class DMonitoringModelResult(ctypes.Structure): |
|
|
|
|
("wheel_on_right_prob", ctypes.c_float), |
|
|
|
|
("features", ctypes.c_float*FEATURE_LEN)] |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class ModelState: |
|
|
|
|
inputs: dict[str, np.ndarray] |
|
|
|
|
output: np.ndarray |
|
|
|
@ -119,6 +123,7 @@ def fill_driver_state(msg, ds_result: DriverStateResult): |
|
|
|
|
msg.readyProb = [float(sigmoid(x)) for x in ds_result.ready_prob] |
|
|
|
|
msg.notReadyProb = [float(sigmoid(x)) for x in ds_result.not_ready_prob] |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: int, execution_time: float, gpu_execution_time: float): |
|
|
|
|
model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(DMonitoringModelResult)).contents |
|
|
|
|
msg = messaging.new_message('driverStateV2', valid=True) |
|
|
|
@ -139,6 +144,9 @@ def main(): |
|
|
|
|
setproctitle(PROCESS_NAME) |
|
|
|
|
set_realtime_priority(1) |
|
|
|
|
|
|
|
|
|
sentry.set_tag("daemon", PROCESS_NAME) |
|
|
|
|
cloudlog.bind(daemon=PROCESS_NAME) |
|
|
|
|
|
|
|
|
|
cl_context = CLContext() |
|
|
|
|
model = ModelState(cl_context) |
|
|
|
|
cloudlog.warning("models loaded, dmonitoringmodeld starting") |
|
|
|
@ -177,4 +185,10 @@ def main(): |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if __name__ == "__main__": |
|
|
|
|
try: |
|
|
|
|
main() |
|
|
|
|
except KeyboardInterrupt: |
|
|
|
|
cloudlog.warning(f"child {PROCESS_NAME} got SIGINT") |
|
|
|
|
except Exception: |
|
|
|
|
sentry.capture_exception() |
|
|
|
|
raise |
|
|
|
|