firestar5683
610 lines
30 KiB
Python
Executable File
610 lines
30 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
import os
|
|
from openpilot.system.hardware import TICI
|
|
os.environ['DEV'] = 'QCOM' if TICI else 'LLVM'
|
|
from tinygrad.tensor import Tensor
|
|
from tinygrad.dtype import dtypes
|
|
import time
|
|
import pickle
|
|
import numpy as np
|
|
import cereal.messaging as messaging
|
|
from cereal import car, log
|
|
from pathlib import Path
|
|
from setproctitle import setproctitle
|
|
from cereal.messaging import PubMaster, SubMaster
|
|
from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
|
|
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 opendbc.car.car_helpers import get_demo_car_params
|
|
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
|
|
from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan_tomb_raider, smooth_value
|
|
from openpilot.frogpilot.tinygrad_modeld.parse_model_outputs import Parser
|
|
from openpilot.frogpilot.tinygrad_modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output
|
|
from openpilot.frogpilot.tinygrad_modeld.constants import ModelConstants, Plan
|
|
try:
|
|
from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
|
|
except ImportError:
|
|
from openpilot.frogpilot.tinygrad_modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
|
|
from openpilot.frogpilot.tinygrad_modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
|
|
from openpilot.frogpilot.common.frogpilot_variables import get_frogpilot_toggles, MODELS_PATH, params_memory
|
|
|
|
|
|
PROCESS_NAME = "frogpilot.tinygrad_modeld.tinygrad_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_param_str(params: Params, key: str, default: str = "") -> str:
|
|
try:
|
|
val = params.get(key)
|
|
except Exception:
|
|
return default
|
|
if val is None:
|
|
return default
|
|
if isinstance(val, bytes):
|
|
try:
|
|
return val.decode("utf-8")
|
|
except Exception:
|
|
return default
|
|
if isinstance(val, (dict, list)):
|
|
return default
|
|
return str(val)
|
|
|
|
|
|
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, mlsim: bool, is_v9: bool, frogpilot_toggles) -> log.ModelDataV2.Action:
|
|
plan = model_output['plan'][0]
|
|
if 'planplus' in model_output:
|
|
recovery_power = getattr(frogpilot_toggles, "recovery_power", 1.0)
|
|
plan = plan + recovery_power * model_output['planplus'][0]
|
|
cloudlog.error(f"planplus applied: shape {model_output['planplus'].shape}, RECOVERY_POWER {recovery_power}")
|
|
desired_accel, should_stop = get_accel_from_plan_tomb_raider(plan[:,Plan.VELOCITY][:,0],
|
|
plan[:,Plan.ACCELERATION][:,0],
|
|
ModelConstants.T_IDXS,
|
|
action_t=long_action_t)
|
|
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
|
|
|
|
if is_v9:
|
|
# V9: use desired_curvature if present; otherwise do NOT fall back to plan
|
|
if 'desired_curvature' in model_output:
|
|
desired_curvature = float(model_output['desired_curvature'][0, 0])
|
|
else:
|
|
desired_curvature = prev_action.desiredCurvature
|
|
else:
|
|
desired_curvature = get_curvature_from_output(model_output, plan, v_ego, lat_action_t, mlsim=mlsim)
|
|
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
|
|
|
|
class ModelState:
|
|
frames: dict[str, DrivingModelFrame]
|
|
inputs: dict[str, np.ndarray]
|
|
output: np.ndarray
|
|
prev_desire: np.ndarray # for tracking the rising edge of the pulse
|
|
|
|
def _build_policy_inputs(self, input_shapes: dict[str, tuple[int, ...]]) -> tuple[dict[str, np.ndarray], str | None]:
|
|
numpy_inputs: dict[str, np.ndarray] = {}
|
|
|
|
# Always-supported inputs (if model expects them)
|
|
desire_key_init = next((k for k in input_shapes if k.startswith('desire')), None)
|
|
if desire_key_init:
|
|
numpy_inputs[desire_key_init] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32)
|
|
if 'traffic_convention' in input_shapes:
|
|
numpy_inputs['traffic_convention'] = np.zeros((1, ModelConstants.TRAFFIC_CONVENTION_LEN), dtype=np.float32)
|
|
if 'features_buffer' in input_shapes:
|
|
numpy_inputs['features_buffer'] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32)
|
|
|
|
# Optional inputs for non-v11 (and some v10/v9 variants)
|
|
# Lateral control params
|
|
if 'lateral_control_params' in input_shapes:
|
|
numpy_inputs['lateral_control_params'] = np.zeros((1, ModelConstants.LATERAL_CONTROL_PARAMS_LEN), dtype=np.float32)
|
|
|
|
# Previous desired curvature: handle both singular and plural key names across model versions
|
|
prev_desired_curv_key = None
|
|
if 'prev_desired_curv' in input_shapes:
|
|
prev_desired_curv_key = 'prev_desired_curv'
|
|
numpy_inputs['prev_desired_curv'] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32)
|
|
elif 'prev_desired_curvs' in input_shapes:
|
|
prev_desired_curv_key = 'prev_desired_curvs'
|
|
numpy_inputs['prev_desired_curvs'] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32)
|
|
|
|
return numpy_inputs, prev_desired_curv_key
|
|
|
|
def __init__(self, context: CLContext):
|
|
# Dynamically build paths based on current model ID
|
|
params = Params()
|
|
model_id_raw = _get_param_str(params, "Model")
|
|
if not model_id_raw:
|
|
model_id_raw = _get_param_str(params, "DrivingModel", "sc")
|
|
model_id = (model_id_raw.strip() or "sc").lower()
|
|
|
|
model_version = _get_param_str(params, "ModelVersion")
|
|
if not model_version:
|
|
model_version = _get_param_str(params, "DrivingModelVersion")
|
|
|
|
model_dir = MODELS_PATH
|
|
use_builtin_model = model_id == "sc"
|
|
model_download_id = model_id
|
|
if use_builtin_model and (model_id_raw != model_id or _get_param_str(params, "DrivingModel") != model_id):
|
|
params.put("Model", model_id)
|
|
params.put("DrivingModel", model_id)
|
|
|
|
if use_builtin_model and not model_version:
|
|
model_version = "v11"
|
|
params.put("ModelVersion", model_version)
|
|
params.put("DrivingModelVersion", model_version)
|
|
# Use built-in files for defaults when a custom model isn't selected.
|
|
if use_builtin_model:
|
|
models_dir = Path(__file__).resolve().parents[2] / "selfdrive/modeld/models"
|
|
VISION_PKL_PATH = models_dir / "driving_vision_tinygrad.pkl"
|
|
POLICY_PKL_PATH = models_dir / "driving_policy_tinygrad.pkl"
|
|
OFF_POLICY_PKL_PATH = models_dir / "driving_off_policy_tinygrad.pkl"
|
|
VISION_METADATA_PATH = models_dir / "driving_vision_metadata.pkl"
|
|
POLICY_METADATA_PATH = models_dir / "driving_policy_metadata.pkl"
|
|
OFF_POLICY_METADATA_PATH = models_dir / "driving_off_policy_metadata.pkl"
|
|
else:
|
|
VISION_PKL_PATH = model_dir / f"{model_id}_driving_vision_tinygrad.pkl"
|
|
POLICY_PKL_PATH = model_dir / f"{model_id}_driving_policy_tinygrad.pkl"
|
|
OFF_POLICY_PKL_PATH = model_dir / f"{model_id}_driving_off_policy_tinygrad.pkl"
|
|
VISION_METADATA_PATH = model_dir / f"{model_id}_driving_vision_metadata.pkl"
|
|
POLICY_METADATA_PATH = model_dir / f"{model_id}_driving_policy_metadata.pkl"
|
|
OFF_POLICY_METADATA_PATH = model_dir / f"{model_id}_driving_off_policy_metadata.pkl"
|
|
|
|
def ensure_artifact(path: Path, suffix: str | None = None, optional: bool = False) -> Path | None:
|
|
if path.is_file():
|
|
return path
|
|
|
|
if use_builtin_model:
|
|
if optional:
|
|
cloudlog.warning(f"Optional builtin model artifact missing: {path}")
|
|
return None
|
|
raise FileNotFoundError(
|
|
f"Missing builtin model artifact: {path}. "
|
|
"Rebuild model artifacts locally (./build or scons target) and deploy them."
|
|
)
|
|
|
|
cloudlog.error(f"Missing model artifact {path}, downloading {model_download_id}...")
|
|
from openpilot.frogpilot.assets.model_manager import ModelManager
|
|
ModelManager(params, params_memory).download_model(model_download_id)
|
|
|
|
if path.is_file():
|
|
return path
|
|
|
|
if optional:
|
|
cloudlog.warning(f"Optional model artifact missing: {path}")
|
|
return None
|
|
|
|
raise FileNotFoundError(path)
|
|
|
|
# If ModelVersion is not set or not available, try to determine it from available model data
|
|
if not model_version:
|
|
cloudlog.warning(f"ModelVersion not available for model {model_id}, attempting to determine from model data")
|
|
try:
|
|
# Try to get version from the model versions JSON file
|
|
versions_file = model_dir / ".model_versions.json"
|
|
if versions_file.is_file():
|
|
import json
|
|
with open(versions_file, "r") as f:
|
|
version_map = json.load(f)
|
|
version_lookup_keys = [model_id]
|
|
if model_id and not model_id.endswith("2"):
|
|
version_lookup_keys.append(f"{model_id}2")
|
|
for key in version_lookup_keys:
|
|
if key in version_map:
|
|
model_version = version_map[key]
|
|
cloudlog.warning(f"Determined model version from JSON: {model_version} ({key})")
|
|
params.put("ModelVersion", model_version)
|
|
params.put("DrivingModelVersion", model_version)
|
|
break
|
|
else:
|
|
cloudlog.error("Model versions JSON file not found, defaulting to v8")
|
|
model_version = "v8"
|
|
except Exception as e:
|
|
cloudlog.error(f"Failed to determine model version: {e}, defaulting to v8")
|
|
model_version = "v8"
|
|
|
|
VISION_METADATA_PATH = ensure_artifact(VISION_METADATA_PATH, "driving_vision_metadata.pkl")
|
|
with open(VISION_METADATA_PATH, 'rb') as f:
|
|
vision_metadata = pickle.load(f)
|
|
self.vision_input_shapes = vision_metadata['input_shapes']
|
|
self.vision_input_names = list(self.vision_input_shapes.keys())
|
|
self.vision_output_slices = vision_metadata['output_slices']
|
|
vision_output_size = vision_metadata['output_shapes']['outputs'][1]
|
|
|
|
POLICY_METADATA_PATH = ensure_artifact(POLICY_METADATA_PATH, "driving_policy_metadata.pkl")
|
|
with open(POLICY_METADATA_PATH, 'rb') as f:
|
|
policy_metadata = pickle.load(f)
|
|
self.policy_input_shapes = policy_metadata['input_shapes']
|
|
self.policy_output_slices = policy_metadata['output_slices']
|
|
policy_output_size = policy_metadata['output_shapes']['outputs'][1]
|
|
# Add policy_generation attribute after loading policy_metadata
|
|
self.policy_generation = model_version or "v8"
|
|
self.is_v11 = (self.policy_generation == "v11")
|
|
self.is_v10 = (self.policy_generation == "v10")
|
|
self.is_v12 = (self.policy_generation == "v12")
|
|
self.is_v9 = (self.policy_generation == "v9")
|
|
self.mlsim = (self.policy_generation in ("v8", "v10", "v11", "v12"))
|
|
|
|
self.frames = {name: DrivingModelFrame(context, ModelConstants.TEMPORAL_SKIP) for name in self.vision_input_names}
|
|
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
|
|
|
|
self.full_features_buffer = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32)
|
|
self.full_desire = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32)
|
|
self.temporal_idxs = slice(-1-(ModelConstants.TEMPORAL_SKIP*(ModelConstants.INPUT_HISTORY_BUFFER_LEN-1)), None, ModelConstants.TEMPORAL_SKIP)
|
|
|
|
|
|
# policy inputs (built dynamically to support all generations)
|
|
self.numpy_inputs, self.prev_desired_curv_key = self._build_policy_inputs(self.policy_input_shapes)
|
|
|
|
# Off-policy model (optional)
|
|
self.off_policy_enabled = False
|
|
self.off_policy_input_shapes: dict[str, tuple[int, ...]] = {}
|
|
self.off_policy_output_slices: dict[str, slice] = {}
|
|
self.off_policy_numpy_inputs: dict[str, np.ndarray] = {}
|
|
self.off_policy_prev_desired_curv_key: str | None = None
|
|
self.off_policy_desire_key: str | None = None
|
|
self.off_policy_inputs: dict[str, Tensor] | None = None
|
|
self.off_policy_output: np.ndarray | None = None
|
|
|
|
off_policy_metadata = None
|
|
if self.policy_generation == "v12" or OFF_POLICY_METADATA_PATH.is_file() or OFF_POLICY_PKL_PATH.is_file():
|
|
resolved_off_policy_meta = ensure_artifact(OFF_POLICY_METADATA_PATH, "driving_off_policy_metadata.pkl", optional=True)
|
|
if resolved_off_policy_meta is not None:
|
|
with open(resolved_off_policy_meta, 'rb') as f:
|
|
off_policy_metadata = pickle.load(f)
|
|
|
|
if off_policy_metadata is not None:
|
|
self.off_policy_input_shapes = off_policy_metadata['input_shapes']
|
|
self.off_policy_output_slices = off_policy_metadata['output_slices']
|
|
off_policy_output_size = off_policy_metadata['output_shapes']['outputs'][1]
|
|
self.off_policy_numpy_inputs, self.off_policy_prev_desired_curv_key = self._build_policy_inputs(self.off_policy_input_shapes)
|
|
self.off_policy_desire_key = next((k for k in self.off_policy_numpy_inputs if k.startswith('desire')), None)
|
|
self.off_policy_inputs = {k: Tensor(v, device='NPY').realize() for k, v in self.off_policy_numpy_inputs.items()}
|
|
self.off_policy_output = np.zeros(off_policy_output_size, dtype=np.float32)
|
|
resolved_off_policy_pkl = ensure_artifact(OFF_POLICY_PKL_PATH, "driving_off_policy_tinygrad.pkl", optional=True)
|
|
if resolved_off_policy_pkl is not None:
|
|
with open(resolved_off_policy_pkl, "rb") as f:
|
|
self.off_policy_run = pickle.load(f)
|
|
self.off_policy_enabled = True
|
|
|
|
# Optional temporal buffer for previous desired curvature (allocate only if any model expects it)
|
|
if self.prev_desired_curv_key is not None or self.off_policy_prev_desired_curv_key is not None:
|
|
self.full_prev_desired_curv = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32)
|
|
|
|
|
|
# img buffers are managed in openCL transform code
|
|
self.vision_inputs: dict[str, Tensor] = {}
|
|
self.vision_output = np.zeros(vision_output_size, dtype=np.float32)
|
|
self.policy_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
|
|
self.policy_output = np.zeros(policy_output_size, dtype=np.float32)
|
|
self.parser = Parser()
|
|
self.off_policy_parser = Parser(ignore_missing=True)
|
|
|
|
VISION_PKL_PATH = ensure_artifact(VISION_PKL_PATH, "driving_vision_tinygrad.pkl")
|
|
with open(VISION_PKL_PATH, "rb") as f:
|
|
self.vision_run = pickle.load(f)
|
|
|
|
POLICY_PKL_PATH = ensure_artifact(POLICY_PKL_PATH, "driving_policy_tinygrad.pkl")
|
|
with open(POLICY_PKL_PATH, "rb") as f:
|
|
self.policy_run = pickle.load(f)
|
|
|
|
@property
|
|
def desire_key(self) -> str:
|
|
return next(key for key in self.numpy_inputs if key.startswith('desire'))
|
|
|
|
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], prepare_only: bool) -> dict[str, np.ndarray] | None:
|
|
# Model decides when action is completed, so desire input is just a pulse triggered on rising edge
|
|
inputs[self.desire_key][0] = 0
|
|
new_desire = np.where(inputs[self.desire_key] - self.prev_desire > .99, inputs[self.desire_key], 0)
|
|
self.prev_desire[:] = inputs[self.desire_key]
|
|
|
|
self.full_desire[0,:-1] = self.full_desire[0,1:]
|
|
self.full_desire[0,-1] = new_desire
|
|
self.numpy_inputs[self.desire_key][:] = self.full_desire.reshape((1,ModelConstants.INPUT_HISTORY_BUFFER_LEN,ModelConstants.TEMPORAL_SKIP,-1)).max(axis=2)
|
|
if self.off_policy_enabled and self.off_policy_desire_key is not None:
|
|
self.off_policy_numpy_inputs[self.off_policy_desire_key][:] = self.numpy_inputs[self.desire_key]
|
|
|
|
if 'traffic_convention' in self.numpy_inputs:
|
|
self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention']
|
|
if self.off_policy_enabled and 'traffic_convention' in self.off_policy_numpy_inputs:
|
|
self.off_policy_numpy_inputs['traffic_convention'][:] = inputs['traffic_convention']
|
|
|
|
if 'lateral_control_params' in self.numpy_inputs:
|
|
self.numpy_inputs['lateral_control_params'][:] = inputs['lateral_control_params']
|
|
if self.off_policy_enabled and 'lateral_control_params' in self.off_policy_numpy_inputs:
|
|
self.off_policy_numpy_inputs['lateral_control_params'][:] = inputs['lateral_control_params']
|
|
|
|
if prepare_only:
|
|
return None
|
|
|
|
imgs_cl = {name: self.frames[name].prepare(bufs[name], transforms[name].flatten()) for name in self.vision_input_names}
|
|
|
|
if TICI:
|
|
# The imgs tensors are backed by opencl memory, only need init once
|
|
for key in imgs_cl:
|
|
if key not in self.vision_inputs:
|
|
self.vision_inputs[key] = qcom_tensor_from_opencl_address(imgs_cl[key].mem_address, self.vision_input_shapes[key], dtype=dtypes.uint8)
|
|
else:
|
|
for key in imgs_cl:
|
|
frame_input = self.frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.vision_input_shapes[key])
|
|
self.vision_inputs[key] = Tensor(frame_input, dtype=dtypes.uint8).realize()
|
|
|
|
self.vision_output = self.vision_run(**self.vision_inputs).contiguous().realize().uop.base.buffer.numpy()
|
|
vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(self.vision_output, self.vision_output_slices))
|
|
|
|
self.full_features_buffer[0,:-1] = self.full_features_buffer[0,1:]
|
|
self.full_features_buffer[0,-1] = vision_outputs_dict['hidden_state'][0, :]
|
|
if 'features_buffer' in self.numpy_inputs:
|
|
self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[0, self.temporal_idxs]
|
|
if self.off_policy_enabled and 'features_buffer' in self.off_policy_numpy_inputs:
|
|
self.off_policy_numpy_inputs['features_buffer'][:] = self.full_features_buffer[0, self.temporal_idxs]
|
|
|
|
self.policy_output = self.policy_run(**self.policy_inputs).contiguous().realize().uop.base.buffer.numpy()
|
|
policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(self.policy_output, self.policy_output_slices))
|
|
|
|
# TODO model only uses last value now
|
|
if hasattr(self, 'full_prev_desired_curv') and 'desired_curvature' in policy_outputs_dict:
|
|
self.full_prev_desired_curv[0,:-1] = self.full_prev_desired_curv[0,1:]
|
|
self.full_prev_desired_curv[0,-1,:] = policy_outputs_dict['desired_curvature'][0, :]
|
|
|
|
if self.prev_desired_curv_key is not None:
|
|
# v9/v10/v11/v12 models expect zeros for prev_desired_curv(s); others use history
|
|
if self.is_v9 or self.is_v10 or self.is_v11 or self.is_v12:
|
|
self.numpy_inputs[self.prev_desired_curv_key][:] = 0 * self.full_prev_desired_curv[0, self.temporal_idxs]
|
|
else:
|
|
self.numpy_inputs[self.prev_desired_curv_key][:] = self.full_prev_desired_curv[0, self.temporal_idxs]
|
|
|
|
if self.off_policy_enabled and self.off_policy_prev_desired_curv_key is not None:
|
|
if self.is_v9 or self.is_v12:
|
|
self.off_policy_numpy_inputs[self.off_policy_prev_desired_curv_key][:] = 0 * self.full_prev_desired_curv[0, self.temporal_idxs]
|
|
else:
|
|
self.off_policy_numpy_inputs[self.off_policy_prev_desired_curv_key][:] = self.full_prev_desired_curv[0, self.temporal_idxs]
|
|
|
|
combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict}
|
|
if self.off_policy_enabled:
|
|
self.off_policy_output = self.off_policy_run(**self.off_policy_inputs).contiguous().realize().uop.base.buffer.numpy()
|
|
off_policy_outputs_dict = self.off_policy_parser.parse_policy_outputs(
|
|
self.slice_outputs(self.off_policy_output, self.off_policy_output_slices)
|
|
)
|
|
combined_outputs_dict.update(off_policy_outputs_dict)
|
|
if SEND_RAW_PRED:
|
|
raw_pred = [self.vision_output.copy(), self.policy_output.copy()]
|
|
if self.off_policy_enabled and self.off_policy_output is not None:
|
|
raw_pred.append(self.off_policy_output.copy())
|
|
combined_outputs_dict['raw_pred'] = np.concatenate(raw_pred)
|
|
|
|
return combined_outputs_dict
|
|
|
|
|
|
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)
|
|
|
|
cloudlog.warning("setting up CL context")
|
|
cl_context = CLContext()
|
|
cloudlog.warning("CL context ready; loading model")
|
|
model = ModelState(cl_context)
|
|
cloudlog.warning("models loaded, modeld starting")
|
|
|
|
# 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, cl_context)
|
|
vipc_client_extra = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False, cl_context)
|
|
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})")
|
|
|
|
# messaging
|
|
pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "frogpilotModelV2"])
|
|
sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay", "frogpilotPlan"])
|
|
|
|
publish_state = PublishState()
|
|
params = Params()
|
|
|
|
# setup filter to track dropped frames
|
|
frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.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()
|
|
|
|
|
|
if demo:
|
|
CP = get_demo_car_params()
|
|
else:
|
|
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
|
cloudlog.info("tinygrad_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()
|
|
|
|
# FrogPilot variables
|
|
frogpilot_toggles = get_frogpilot_toggles(sm)
|
|
|
|
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.)
|
|
lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
|
|
lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32)
|
|
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)
|
|
model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.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)
|
|
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}
|
|
|
|
inputs:dict[str, np.ndarray] = {
|
|
model.desire_key: vec_desire,
|
|
'traffic_convention': traffic_convention,
|
|
}
|
|
# Include optional inputs only if the loaded model expects them
|
|
if 'lateral_control_params' in model.numpy_inputs:
|
|
inputs['lateral_control_params'] = lateral_control_params
|
|
|
|
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')
|
|
frogpilot_modelv2_send = messaging.new_message('frogpilotModelV2')
|
|
drivingdata_send = messaging.new_message('drivingModelData')
|
|
posenet_send = messaging.new_message('cameraOdometry')
|
|
|
|
action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego, model.mlsim, model.is_v9, frogpilot_toggles)
|
|
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)
|
|
|
|
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, sm['frogpilotPlan'], frogpilot_toggles)
|
|
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
|
|
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
|
|
frogpilot_modelv2_send.frogpilotModelV2.turnDirection = DH.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('frogpilotModelV2', frogpilot_modelv2_send)
|
|
pm.send('drivingModelData', drivingdata_send)
|
|
pm.send('cameraOdometry', posenet_send)
|
|
last_vipc_frame_id = meta_main.frame_id
|
|
|
|
# Update FrogPilot parameters
|
|
if sm.updated['frogpilotPlan']:
|
|
frogpilot_toggles = get_frogpilot_toggles(sm)
|
|
|
|
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
|