diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 35063e6d37..c1111584f3 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -26,7 +26,6 @@ from openpilot.common.file_chunker import read_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 - PROCESS_NAME = "selfdrive.modeld.modeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -35,29 +34,29 @@ 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: - 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_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) + 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_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) - 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) - 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 + 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) + 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)) - return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature), - desiredAcceleration=float(desired_accel), - shouldStop=bool(should_stop)) class FrameMeta: frame_id: int = 0 @@ -77,20 +76,20 @@ class ModelState: self.WARP_DEV, self.QUEUE_DEV = input_devices['WARP_DEV'], input_devices['QUEUE_DEV'] jits = pickle.loads(read_file_chunked(modeld_pkl_path(usbgpu))) vision_metadata = jits['metadata']['vision'] - self.vision_input_shapes = vision_metadata['input_shapes'] + 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'] policy_metadata = jits['metadata']['policy'] - self.policy_input_shapes = policy_metadata['input_shapes'] + self.policy_input_shapes = policy_metadata['input_shapes'] self.policy_output_slices = policy_metadata['output_slices'] self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) self.frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ self.input_queues, self.npy = make_input_queues(self.vision_input_shapes, self.policy_input_shapes, self.frame_skip, device=self.QUEUE_DEV) - self.full_frames : dict[str, Tensor] = {} - self._blob_cache : dict[int, Tensor] = {} + self.full_frames: dict[str, Tensor] = {} + self._blob_cache: dict[int, Tensor] = {} 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'] @@ -101,7 +100,7 @@ class ModelState: 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: + 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] @@ -202,7 +201,6 @@ def main(demo=False): meta_main = FrameMeta() meta_extra = FrameMeta() - if demo: CP = get_demo_car_params() else: @@ -284,7 +282,7 @@ def main(demo=False): 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] = { + inputs: dict[str, np.ndarray] = { 'desire_pulse': vec_desire, 'traffic_convention': traffic_convention, }