#!/usr/bin/env python3 import os os.environ['GMMU'] = '0' # for usbgpu fast loading, noop for qcom from tinygrad.tensor import Tensor import time import numpy as np import openpilot.cereal.messaging as messaging from openpilot.cereal import log from opendbc.car.structs import car from openpilot.cereal.messaging import PubMaster, SubMaster from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf from opendbc.car.car_helpers import get_demo_car_params from openpilot.common.swaglog import cloudlog from openpilot.common.params import Params from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.system.camerad.cameras.nv12_info import get_nv12_info from openpilot.common.transformations.model import get_warp_matrix from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value, get_curvature_from_plan from openpilot.selfdrive.modeld.parse_model_outputs import Parser from openpilot.selfdrive.modeld.compile_modeld import make_input_queues, WARP_INPUTS, POLICY_INPUTS from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_driving_model_data, fill_pose_msg, PublishState from openpilot.common.file_chunker import open_file_chunked, get_manifest_path from openpilot.selfdrive.modeld.constants import ModelConstants, Plan from openpilot.selfdrive.modeld.helpers import usbgpu_present, modeld_pkl_path, get_tg_input_devices, load_oob from openpilot.selfdrive.modeld.usbgpu_link import wait_usbgpu_link from openpilot.tools.wgpu.zmq import WGPU_CAR_PARAMS, ZmqPubMaster, ZmqSubMaster, ZmqSubSocket from openpilot.sunnypilot.livedelay.helpers import get_lat_delay from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase PROCESS_NAME = "openpilot.selfdrive.modeld.modeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') LAT_SMOOTH_SECONDS = 0.0 LONG_SMOOTH_SECONDS = 0.3 MIN_LAT_CONTROL_SPEED = 0.3 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action: if 'action' not in model_output: plan = model_output['plan'][0] desired_accel, should_stop = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0], plan[:,Plan.ACCELERATION][:,0], ModelConstants.T_IDXS, action_t=long_action_t) desired_curvature = get_curvature_from_plan(plan[:,Plan.T_FROM_CURRENT_EULER][:,2], plan[:,Plan.ORIENTATION_RATE][:,2], ModelConstants.T_IDXS, v_ego, lat_action_t) else: desired_accel = model_output['action'][0,1] desired_curvature = model_output['action'][0,0] / (max(1.0, v_ego))**2 should_stop = (v_ego < 0.3 and desired_accel < 0.1) desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) if v_ego > MIN_LAT_CONTROL_SPEED: desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS) else: desired_curvature = prev_action.desiredCurvature return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature), desiredAcceleration=float(desired_accel), shouldStop=bool(should_stop)) class FrameMeta: frame_id: int = 0 timestamp_sof: int = 0 timestamp_eof: int = 0 def __init__(self, vipc=None): if vipc is not None: self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof def copy_nv12_to_venus(buf: VisionBuf, dst: np.ndarray, nv12: tuple[int, int, int, int]) -> None: stride, y_height, uv_height, _ = nv12 if buf.stride < buf.width: raise ValueError(f"invalid VisionIPC stride {buf.stride} for width {buf.width}") src = np.frombuffer(buf.data, dtype=np.uint8) src_size = buf.uv_offset + buf.stride * (buf.height // 2) if src.size < src_size: raise ValueError(f"VisionIPC buffer has {src.size} bytes, expected at least {src_size}") dst[:stride * y_height].reshape(y_height, stride)[:buf.height, :buf.width] = \ src[:buf.stride * buf.height].reshape(buf.height, buf.stride)[:, :buf.width] dst[stride * y_height:stride * (y_height + uv_height)].reshape(uv_height, stride)[:buf.height // 2, :buf.width] = \ src[buf.uv_offset:src_size].reshape(buf.height // 2, buf.stride)[:, :buf.width] class ModelState(ModelStateBase): prev_desire: np.ndarray # for tracking the rising edge of the pulse def __init__(self, cam_w: int, cam_h: int, usbgpu: bool, big_model: bool = False): ModelStateBase.__init__(self) self.LAT_SMOOTH_SECONDS = LAT_SMOOTH_SECONDS input_devices = get_tg_input_devices(PROCESS_NAME, usbgpu) self.WARP_DEV, self.QUEUE_DEV = input_devices['WARP_DEV'], input_devices['QUEUE_DEV'] jits = load_oob(open_file_chunked(modeld_pkl_path(usbgpu or big_model))) metadata = jits['metadata'] self.input_shapes = metadata['input_shapes'] self.vision_input_names = [k for k in self.input_shapes if 'img' in k] self.output_slices = metadata['output_slices'] self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) self.copy_vision_buffers = self.WARP_DEV.split(":")[0] == "METAL" self.frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ self.input_queues, self.npy = make_input_queues(self.input_shapes, self.frame_skip, device=self.QUEUE_DEV) self.full_frames: dict[str, Tensor] = {} self._blob_cache: dict[tuple[str, int], Tensor] = {} self._vision_staging: dict[str, np.ndarray] = {} self.parser = Parser() self.frame_buf_params = {k: get_nv12_info(cam_w, cam_h) for k in ('img', 'big_img')} self.run_policy = jits['run_policy'] self.warp = jits[(cam_w,cam_h)] def slice_outputs(self, model_outputs: np.ndarray, output_slices: dict[str, slice]) -> dict[str, np.ndarray]: parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in output_slices.items()} return parsed_model_outputs def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray], inputs: dict[str, np.ndarray]) -> dict[str, np.ndarray] | None: for key in bufs.keys(): nv12 = self.frame_buf_params[key] yuv_size = nv12[3] if self.copy_vision_buffers: # VisionIPC supplies a CPU pointer. Metal's external_ptr expects an MTLBuffer object, # so wrap its NV12 pixels in the Venus layout expected by the compiled warp, then copy. if key not in self._vision_staging: self._vision_staging[key] = np.zeros(yuv_size, dtype=np.uint8) copy_nv12_to_venus(bufs[key], self._vision_staging[key], nv12) self.full_frames[key] = Tensor(self._vision_staging[key], device=self.WARP_DEV).realize() else: # There is a ringbuffer of imgs, just cache tensors pointing to all of them. frame = np.frombuffer(bufs[key].data, dtype=np.uint8, count=yuv_size) cache_key = (key, frame.ctypes.data) if cache_key not in self._blob_cache: self._blob_cache[cache_key] = Tensor.from_blob(frame.ctypes.data, (yuv_size,), dtype='uint8', device=self.WARP_DEV) self.full_frames[key] = self._blob_cache[cache_key] # Model decides when action is completed, so desire input is just a pulse triggered on rising edge inputs['desire_pulse'][0] = 0 self.npy['desire'][:] = np.where(inputs['desire_pulse'] - self.prev_desire > .99, inputs['desire_pulse'], 0) self.prev_desire[:] = inputs['desire_pulse'] self.npy['traffic_convention'][:] = inputs['traffic_convention'] self.npy['action_t'][:] = inputs['action_t'] self.npy['tfm'][:,:] = transforms['img'][:,:] self.npy['big_tfm'][:,:] = transforms['big_img'][:,:] warped = self.warp(**{k: self.input_queues[k] for k in WARP_INPUTS}, frame=self.full_frames['img'], big_frame=self.full_frames['big_img']) outs, = self.run_policy( **{k: self.input_queues[k] for k in POLICY_INPUTS if k in self.input_queues}, warped=warped ) model_output = outs.numpy()[0] outputs_dict = self.parser.parse_outputs(self.slice_outputs(model_output, self.output_slices)) self.npy['prev_feat'][:] = model_output[self.output_slices['hidden_state']] if SEND_RAW_PRED: outputs_dict['raw_pred'] = model_output.copy() return outputs_dict def main(demo=False, remote_addr: str | None = None, big_model: bool = False): cloudlog.warning("modeld init") _present = usbgpu_present() _compiled = os.path.isfile(get_manifest_path(modeld_pkl_path(usbgpu=True))) USBGPU = _present and _compiled if big_model and not _compiled: raise FileNotFoundError(f"big model is not compiled: {modeld_pkl_path(usbgpu=True)}") params = Params() params.put_bool("UsbGpuPresent", _present) params.put_bool("UsbGpuCompiled", _compiled) config_realtime_process(7, 54) remote_CP = None if remote_addr is not None: # Do not attach to VisionIPC until all startup prerequisites are available. # Otherwise its notification queue grows while waiting for the infrequent # bridged carParams message and reconnect starts tens of seconds behind. cloudlog.warning("waiting for remote carParams") car_params_socket = ZmqSubSocket(WGPU_CAR_PARAMS, remote_addr, conflate=True) raw_car_params = car_params_socket.receive() assert raw_car_params is not None remote_CP = messaging.log_from_bytes(raw_car_params, car.CarParams) cloudlog.info("modeld got remote CarParams: %s", remote_CP.brand) # visionipc clients while True: available_streams = VisionIpcClient.available_streams("camerad", block=False) if available_streams: use_extra_client = VisionStreamType.VISION_STREAM_WIDE_ROAD in available_streams and VisionStreamType.VISION_STREAM_ROAD in available_streams main_wide_camera = VisionStreamType.VISION_STREAM_ROAD not in available_streams break time.sleep(.1) vipc_client_main_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if main_wide_camera else VisionStreamType.VISION_STREAM_ROAD vipc_client_main = VisionIpcClient("camerad", vipc_client_main_stream, True) vipc_client_extra = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False) cloudlog.warning(f"vision stream set up, main_wide_camera: {main_wide_camera}, use_extra_client: {use_extra_client}") while not vipc_client_main.connect(False): time.sleep(0.1) while use_extra_client and not vipc_client_extra.connect(False): time.sleep(0.1) cloudlog.warning(f"connected main cam with buffer size: {vipc_client_main.buffer_len} ({vipc_client_main.width} x {vipc_client_main.height})") if use_extra_client: cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})") if USBGPU: wait_usbgpu_link() st = time.monotonic() cloudlog.warning("loading model") model = ModelState(vipc_client_main.width, vipc_client_main.height, USBGPU, big_model=big_model) cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting") # messaging output_services = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] pm = ZmqPubMaster(output_services) if remote_addr is not None else PubMaster(output_services) services = ["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"] sm = ZmqSubMaster(services, remote_addr) if remote_addr is not None else SubMaster(services) publish_state = PublishState() params = Params() # setup filter to track dropped frames frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_RUN_FREQ) frame_id = 0 last_vipc_frame_id = 0 run_count = 0 model_transform_main = np.zeros((3, 3), dtype=np.float32) model_transform_extra = np.zeros((3, 3), dtype=np.float32) live_calib_seen = False buf_main, buf_extra = None, None meta_main = FrameMeta() meta_extra = FrameMeta() if demo: CP = get_demo_car_params() elif remote_addr is not None: assert remote_CP is not None CP = remote_CP else: CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) cloudlog.info("modeld got CarParams: %s", CP.brand) # TODO this needs more thought, use .2s extra for now to estimate other delays # TODO Move smooth seconds to action function long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS prev_action = log.ModelDataV2.Action() DH = DesireHelper() while True: # Keep receiving frames until we are at least 1 frame ahead of previous extra frame while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: buf_main = vipc_client_main.recv() meta_main = FrameMeta(vipc_client_main) if buf_main is None: break if buf_main is None: cloudlog.debug("vipc_client_main no frame") continue if use_extra_client: # Keep receiving extra frames until frame id matches main camera while True: buf_extra = vipc_client_extra.recv() meta_extra = FrameMeta(vipc_client_extra) if buf_extra is None or meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000: break if buf_extra is None: cloudlog.debug("vipc_client_extra no frame") continue if abs(meta_main.timestamp_sof - meta_extra.timestamp_sof) > 10000000: cloudlog.error(f"frames out of sync! main: {meta_main.frame_id} ({meta_main.timestamp_sof / 1e9:.5f}),\ extra: {meta_extra.frame_id} ({meta_extra.timestamp_sof / 1e9:.5f})") else: # Use single camera buf_extra = buf_main meta_extra = meta_main sm.update(0) desire = DH.desire is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) if sm.frame % 60 == 0: model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay) lat_delay = model.lat_delay + LAT_SMOOTH_SECONDS if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics, False).astype(np.float32) has_wide_camera = use_extra_client or main_wide_camera model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if has_wide_camera else dc.fcam.intrinsics, True).astype(np.float32) live_calib_seen = True traffic_convention = np.zeros(2) traffic_convention[int(is_rhd)] = 1 vec_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) if desire >= 0 and desire < ModelConstants.DESIRE_LEN: vec_desire[desire] = 1 # tracked dropped frames vipc_dropped_frames = max(0, meta_main.frame_id - last_vipc_frame_id - 1) frames_dropped = frame_dropped_filter.update(min(vipc_dropped_frames, 10)) if run_count < 10: # let frame drops warm up frame_dropped_filter.x = 0. frames_dropped = 0. run_count = run_count + 1 frame_drop_ratio = frames_dropped / (1 + frames_dropped) bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names} transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names} 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) lat_action_t = lat_delay + frame_delay + action_delay long_action_t = long_delay + frame_delay + action_delay inputs: dict[str, np.ndarray] = { 'desire_pulse': vec_desire, 'traffic_convention': traffic_convention, 'action_t': np.array([lat_action_t, long_action_t], dtype=np.float32), } mt1 = time.perf_counter() model_output = model.run(bufs, transforms, inputs) mt2 = time.perf_counter() model_execution_time = mt2 - mt1 if model_output is not None: modelv2_send = messaging.new_message('modelV2') drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') mdv2sp_send = messaging.new_message('modelDataV2SP') action = get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego) prev_action = action fill_model_msg(modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen) desire_state = modelv2_send.modelV2.meta.desireState l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] r_lane_change_prob = desire_state[log.Desire.laneChangeRight] lane_change_prob = l_lane_change_prob + r_lane_change_prob DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction fill_driving_model_data(drivingdata_send, modelv2_send) fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen) pm.send('modelV2', modelv2_send) pm.send('drivingModelData', drivingdata_send) pm.send('cameraOdometry', posenet_send) pm.send('modelDataV2SP', mdv2sp_send) last_vipc_frame_id = meta_main.frame_id if __name__ == "__main__": try: import argparse parser = argparse.ArgumentParser() parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.') parser.add_argument('--remote', metavar='ADDRESS', help='Run against a remote device over the cereal ZMQ bridge.') parser.add_argument('--big-model', action='store_true', help='Use the locally compiled big driving model.') args = parser.parse_args() main(demo=args.demo, remote_addr=args.remote, big_model=args.big_model) except KeyboardInterrupt: cloudlog.warning("got SIGINT")