#!/usr/bin/env python3 """ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ import os os.environ['GMMU'] = '0' from openpilot.common.hardware import COMMA_HARDWARE os.environ['DEV'] = 'QCOM' if COMMA_HARDWARE else 'CPU' USBGPU = "USBGPU" in os.environ if USBGPU: os.environ['DEV'] = 'AMD' os.environ['AMD_IFACE'] = 'USB' import pickle import time import numpy as np import openpilot.cereal.messaging as messaging from openpilot.cereal import log from opendbc.car.structs import car from setproctitle import setproctitle from openpilot.cereal.messaging import PubMaster, SubMaster from openpilot.cereal.visionipc import VisionStreamType from msgq.visionipc import VisionIpcClient, VisionBuf from opendbc.car.car_helpers import get_demo_car_params from tinygrad.tensor import Tensor from tinygrad.device import Device from openpilot.common.file_chunker import open_file_chunked 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.common.transformations.model import get_warp_matrix from openpilot.system import sentry from openpilot.system.camerad.cameras.nv12_info import get_nv12_info from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output from openpilot.sunnypilot.modeld_v2.constants import Plan from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, make_split_input_queues from openpilot.sunnypilot.livedelay.helpers import get_lat_delay from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase from openpilot.sunnypilot.models.helpers import get_active_bundle from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController PROCESS_NAME = "openpilot.selfdrive.modeld.modeld_tinygrad" def _pkl_exists(path): from openpilot.common.file_chunker import get_manifest_path return os.path.exists(path) or os.path.exists(get_manifest_path(path)) def _find_driving_pkl(bundle): if (override := os.environ.get('COMBINED_MODEL_PKL')) and _pkl_exists(override): return override if bundle is None or not bundle.models: return None from openpilot.common.hardware.hw import Paths model_root = Paths.model_root() pkl_name = bundle.models[0].artifact.fileName pkl_path = os.path.join(model_root, pkl_name) if _pkl_exists(pkl_path): return pkl_path 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 class ModelState(ModelStateBase): inputs: dict[str, np.ndarray] prev_desire: np.ndarray def __init__(self, cam_w: int, cam_h: int): ModelStateBase.__init__(self) env_pkl = os.environ.get('COMBINED_MODEL_PKL') if env_pkl and os.path.exists(env_pkl): model_bundle = None else: model_bundle = get_active_bundle() self.generation = model_bundle.generation if model_bundle is not None else None overrides = {override.key: override.value for override in model_bundle.overrides} if model_bundle else {} self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0")) self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0")) self.MIN_LAT_CONTROL_SPEED = 0.3 self.PLANPLUS_CONTROL: float = 1.0 pkl_path = _find_driving_pkl(model_bundle) assert pkl_path is not None, "No driving pkl found — all models must be compiled with compile_modeld.py" self._init_combined(pkl_path, cam_w, cam_h, model_bundle) def _init_combined(self, pkl_path, cam_w, cam_h, bundle): cloudlog.warning(f"loading combined pkl: {pkl_path}") # TODO-SP: switch to load_oob from openpilot/selfdrive/helpers on next full recompile of all models jits = pickle.load(open_file_chunked(pkl_path)) self.DEV = Device.DEFAULT self.WARP_DEV = 'CPU' if USBGPU else self.DEV self.QUEUE_DEV = self.DEV metadata = jits['metadata'] self._run_policy = jits[(cam_w, cam_h)]['run_policy'] self._warp_enqueue = jits[(cam_w, cam_h)]['warp_enqueue'] # TODO-SP: Remove legacy use_packed detection block after all models are recompiled captured = getattr(self._run_policy, 'captured', None) if captured is not None: use_packed = 'packed_npy_inputs' in getattr(captured, 'expected_names', []) else: use_packed = True if 'model' in metadata: model_metadata = metadata['model'] self.vision_output_slices = model_metadata['output_slices'] self.policy_output_slices = {} self._policy_slices_list = [] self._combined_model_type = 'supercombo' self._vision_input_names = [key for key in model_metadata['input_shapes'] if 'img' in key] from openpilot.sunnypilot.modeld_v2.compile_modeld import make_supercombo_input_queues frame_skip = derive_frame_skip({}, model_metadata['input_shapes']) self.input_queues, self.numpy_inputs = make_supercombo_input_queues(model_metadata['input_shapes'], frame_skip, device=self.QUEUE_DEV, use_packed=use_packed) else: vision_metadata = metadata['vision'] policy_keys = [k for k in metadata if k != 'vision'] if policy_keys == ['policy']: self._combined_model_type = 'split' else: self._combined_model_type = 'multi_policy' self.vision_output_slices = vision_metadata['output_slices'] self._policy_keys = policy_keys self._policy_slices_list = [metadata[k]['output_slices'] for k in policy_keys] self.policy_output_slices = self._policy_slices_list[0] self._has_on_policy = any('on' in k.lower() for k in policy_keys) first_policy_metadata = metadata[policy_keys[0]] vision_input_shapes = vision_metadata['input_shapes'] policy_input_shapes = first_policy_metadata['input_shapes'] self._vision_input_names = [k for k in vision_input_shapes if 'img' in k] frame_skip = derive_frame_skip(vision_input_shapes, policy_input_shapes) self.input_queues, self.numpy_inputs = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device=self.QUEUE_DEV, use_packed=use_packed) self._desire_key = next(key for key in self.numpy_inputs if key.startswith('desire')) self._road_key = next(key for key in self._vision_input_names if 'big' not in key) self._wide_key = next(key for key in self._vision_input_names if 'big' in key) is_20hz = bundle.is20hz if bundle else self._combined_model_type in ('split', 'multi_policy') if is_20hz: from openpilot.sunnypilot.models.split_model_constants import SplitModelConstants self.constants = SplitModelConstants() else: from openpilot.sunnypilot.modeld_v2.constants import ModelConstants self.constants = ModelConstants() if self._combined_model_type != 'supercombo': from openpilot.sunnypilot.modeld_v2.parse_model_outputs_split import Parser as SplitParser self.parser = SplitParser() else: from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser as CombinedParser self.parser = CombinedParser() self.prev_desire = np.zeros(self.constants.DESIRE_LEN, dtype=np.float32) self.full_frames: dict = {} self._blob_cache: dict = {} nv12_info = get_nv12_info(cam_w, cam_h) self.frame_buf_params = dict.fromkeys(self._vision_input_names, nv12_info) yuv_size = self.frame_buf_params[self._road_key][3] self._warp_enqueue( **self.input_queues, frame=Tensor(np.zeros(yuv_size, dtype=np.uint8), device=self.WARP_DEV).contiguous().realize(), big_frame=Tensor(np.zeros(yuv_size, dtype=np.uint8), device=self.WARP_DEV).contiguous().realize()) @property def mlsim(self) -> bool: return bool(self.generation is not None and self.generation >= 11) @property def vision_input_names(self) -> list[str]: return self._vision_input_names @property def desire_key(self) -> str: return self._desire_key def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray], inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None: for key in bufs.keys(): ptr = np.frombuffer(bufs[key].data, dtype=np.uint8).ctypes.data yuv_size = self.frame_buf_params[key][3] cache_key = (key, ptr) if cache_key not in self._blob_cache: self._blob_cache[cache_key] = Tensor.from_blob(ptr, (yuv_size,), dtype='uint8', device=self.WARP_DEV) self.full_frames[key] = self._blob_cache[cache_key] desire_key = self.desire_key inputs[desire_key][0] = 0 self.numpy_inputs[desire_key][:] = np.where(inputs[desire_key] - self.prev_desire > .99, inputs[desire_key], 0) self.prev_desire[:] = inputs[desire_key] for key in ('traffic_convention', 'lateral_control_params', 'action_t'): if key in self.numpy_inputs and key in inputs: self.numpy_inputs[key][:] = inputs[key] road_key = self._road_key wide_key = self._wide_key self.numpy_inputs['tfm'][:, :] = transforms[road_key].reshape(3, 3) self.numpy_inputs['big_tfm'][:, :] = transforms[wide_key].reshape(3, 3) if prepare_only: self._warp_enqueue(**self.input_queues, frame=self.full_frames[road_key], big_frame=self.full_frames[wide_key]) return None raw_outputs = self._run_policy(**self.input_queues, frame=self.full_frames[road_key], big_frame=self.full_frames[wide_key]) if self._combined_model_type == 'supercombo': model_output = raw_outputs.numpy().flatten() sliced = {k: model_output[np.newaxis, v] for k, v in self.vision_output_slices.items()} outputs = self.parser.parse_outputs(sliced) if 'prev_feat' in self.numpy_inputs: self.numpy_inputs['prev_feat'][:] = model_output[self.vision_output_slices['hidden_state']] else: vision_output = raw_outputs[0].numpy().flatten() vision_sliced = {k: vision_output[np.newaxis, v] for k, v in self.vision_output_slices.items()} outputs = self.parser.parse_vision_outputs(vision_sliced) if 'prev_feat' in self.numpy_inputs and 'hidden_state' in self.vision_output_slices: self.numpy_inputs['prev_feat'][:] = vision_output[self.vision_output_slices['hidden_state']] for i, policy_slices in enumerate(self._policy_slices_list): policy_output = raw_outputs[i + 1].numpy().flatten() policy_sliced = {k: policy_output[np.newaxis, v] for k, v in policy_slices.items()} parsed = self.parser.parse_policy_outputs(policy_sliced) if ('off' in self._policy_keys[i] and self._has_on_policy and any('plan' in self._policy_slices_list[j] for j, k in enumerate(self._policy_keys) if 'on' in k.lower())): parsed.pop('plan', None) outputs.update(parsed) if 'planplus' in outputs and 'plan' in outputs: outputs['plan'] = outputs['plan'] + outputs['planplus'] if 'desired_curvature' in outputs and 'prev_desired_curv' in self.numpy_inputs: buf = self.numpy_inputs['prev_desired_curv'] buf[0, :-1] = buf[0, 1:] buf[0, -1, :] = outputs['desired_curvature'][0, :] if not self.mlsim else 0 # TODO-SP: This is a hack to prevent GPU corruption by calculating in CPU space, it can be removed on next recompile if 'prev_feat' not in self.numpy_inputs and 'feat_q' in self.input_queues: feat_val = self.input_queues['feat_q'].numpy() self.input_queues['feat_q'].assign(feat_val).realize() return outputs def get_action_from_model(self, 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], self.constants.T_IDXS, action_t=long_action_t) curvature_plan = (plan + (self.PLANPLUS_CONTROL - 1.0) * model_output['planplus'][0] if 'planplus' in model_output and self.PLANPLUS_CONTROL != 1.0 else plan) desired_curvature = get_curvature_from_output(model_output, curvature_plan, v_ego, lat_action_t, self.mlsim) 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, self.LONG_SMOOTH_SECONDS) if self.generation is not None and self.generation >= 10: # smooth curvature for post FOF models if v_ego > self.MIN_LAT_CONTROL_SPEED: desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, self.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)) def main(demo=False): cloudlog.warning("modeld init") sentry.set_tag("daemon", PROCESS_NAME) cloudlog.bind(daemon=PROCESS_NAME) setproctitle(PROCESS_NAME) config_realtime_process(7, 54) # 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_NARROW_ROAD in available_streams main_wide_camera = VisionStreamType.VISION_STREAM_NARROW_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_NARROW_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})") cloudlog.warning("loading model") model = ModelState(cam_w=vipc_client_main.width, cam_h=vipc_client_main.height) cloudlog.warning("models loaded, modeld starting") # messaging pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"]) sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"]) publish_state = PublishState() params = Params() # setup filter to track dropped frames frame_dropped_filter = FirstOrderFilter(0., 10., 1. / model.constants.MODEL_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() camera_offset_helper = CameraOffsetHelper() if demo: CP = get_demo_car_params() else: CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) cloudlog.info("modeld got CarParams: %s", CP.brand) # TODO Move smooth seconds to action function long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS prev_action = log.ModelDataV2.Action() DH = DesireHelper() meta_constants = load_meta_constants() RELC = RoadEdgeLaneChangeController() 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["narrowRoadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) if sm.frame % 60 == 0: model.lat_delay = get_lat_delay(params, sm["lateralDelay"].lateralDelay) model.PLANPLUS_CONTROL = params.get("PlanplusControl", return_default=True) camera_offset_helper.set_offset(params.get("CameraOffset", return_default=True)) lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS if sm.updated["extrinsicsCalibration"] and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["extrinsicsCalibration"].rpyCalib, dtype=np.float32) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))] main_intrinsics = dc.wide_road.intrinsics if main_wide_camera else dc.narrow_road.intrinsics model_transform_main = get_warp_matrix(device_from_calib_euler, main_intrinsics, False).astype(np.float32) model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.wide_road.intrinsics, True).astype(np.float32) model_transform_main, model_transform_extra = camera_offset_helper.update(model_transform_main, model_transform_extra, sm, main_wide_camera) live_calib_seen = True traffic_convention = np.zeros(2) traffic_convention[int(is_rhd)] = 1 vec_desire = np.zeros(model.constants.DESIRE_LEN, dtype=np.float32) if desire >= 0 and desire < model.constants.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) prepare_only = vipc_dropped_frames > 0 if prepare_only: cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames") 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] = { model.desire_key: vec_desire, 'traffic_convention': traffic_convention, } if 'lateral_control_params' in model.numpy_inputs: inputs['lateral_control_params'] = np.array([v_ego, lat_delay], dtype=np.float32) if 'action_t' in model.numpy_inputs: inputs['action_t'] = np.array([lat_action_t, long_action_t], dtype=np.float32) mt1 = time.perf_counter() model_output = model.run(bufs, transforms, inputs, prepare_only) 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 = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, 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, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen, meta_constants) 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 left_edge, right_edge = RELC.update_and_fill(modelv2_send.modelV2, mdv2sp_send.modelDataV2SP, v_ego) DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, left_edge, right_edge) 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 drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction 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.') args = parser.parse_args() main(demo=args.demo) except KeyboardInterrupt: cloudlog.warning(f"child {PROCESS_NAME} got SIGINT") except Exception: sentry.capture_exception() raise