diff --git a/scripts/reporter.py b/scripts/reporter.py index 93b71761a..9821a47cc 100755 --- a/scripts/reporter.py +++ b/scripts/reporter.py @@ -34,6 +34,11 @@ if __name__ == "__main__": for f in glob.glob(BASEDIR + MODEL_PATH + "/*.onnx"): fn = os.path.basename(f) - master = get_checkpoint(MASTER_PATH + MODEL_PATH + fn) + master_path = MASTER_PATH + MODEL_PATH + fn + if os.path.exists(master_path): + master = get_checkpoint(master_path) + master_col = f"[{master}](https://reporter.comma.life/experiment/{master})" + else: + master_col = "N/A (new model)" pr = get_checkpoint(BASEDIR + MODEL_PATH + fn) - print("|", fn, "|", f"[{master}](https://reporterv2.comma.life/{master})", "|", f"[{pr}](https://reporterv2.comma.life/{pr})", "|") + print("|", fn, "|", master_col, "|", f"[{pr}](https://reporter.comma.life/experiment/{pr})", "|") diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index dc3d23bf5..80a7df451 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -2,7 +2,7 @@ import glob import json import os import sys, subprocess -from SCons.Script import Value +from SCons.Script import Action, Value from openpilot.common.file_chunker import chunk_file, get_chunk_targets, get_existing_chunks from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE, DM_INPUT_SIZE @@ -87,14 +87,14 @@ frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ for usbgpu in [False, True] if USBGPU else [False]: target_pkl_path = File(modeld_pkl_path(usbgpu)).abspath file_prefix, cmd_flags = ('big_', usbgpu_tg_flags) if usbgpu else ('', tg_flags) - driving_onnx_deps = [p for m in [f'{file_prefix}driving_vision', f'{file_prefix}driving_policy'] + driving_onnx_deps = [p for m in [f'{file_prefix}driving_vision', f'{file_prefix}driving_on_policy'] for p in get_existing_chunks(File(f"models/{m}.onnx").abspath)] camera_res_args = ' '.join(f'{cw}x{ch}' for cw, ch in CAMERA_CONFIGS) cmd = (f'{cmd_flags} {mac_brew_string} python3 {modeld_dir}/compile_modeld.py ' f'--model-size {model_w}x{model_h} ' f'--camera-resolutions {camera_res_args} ' f'--vision-onnx {File(f"models/{file_prefix}driving_vision.onnx").abspath} ' - f'--policy-onnx {File(f"models/{file_prefix}driving_policy.onnx").abspath} ' + f'--on-policy-onnx {File(f"models/{file_prefix}driving_on_policy.onnx").abspath} ' f'--output {target_pkl_path} --frame-skip {frame_skip}') onnx_sizes_sum = sum(os.path.getsize(f) for f in driving_onnx_deps) chunk_targets = get_chunk_targets(target_pkl_path, estimate_pickle_max_size(onnx_sizes_sum)) @@ -103,7 +103,7 @@ for usbgpu in [False, True] if USBGPU else [False]: node = lenv.Command( chunk_targets, tinygrad_files + compile_modeld_script + driving_onnx_deps + [Value(chunk_targets), chunker_file], - [cmd, do_chunk], + [cmd, Action(do_chunk, " [CHUNK] $TARGET")], ) if usbgpu: lenv.SideEffect(usbgpu_lock, node) @@ -134,7 +134,8 @@ def tg_compile(flags, model_name): return lenv.Command( chunk_targets, [onnx_path] + tinygrad_files + [Value(chunk_targets), chunker_file, tg_devices_node], - [f'{pythonpath_string} {flags} python3 {Dir("#tinygrad_repo").abspath}/examples/openpilot/compile3.py {fn}.onnx {pkl}', do_chunk], + [f'{pythonpath_string} {flags} python3 {Dir("#tinygrad_repo").abspath}/examples/openpilot/compile3.py {fn}.onnx {pkl}', + Action(do_chunk, " [CHUNK] $TARGET")], ) tg_compile(tg_flags, 'dmonitoring_model') diff --git a/selfdrive/modeld/compile_modeld.py b/selfdrive/modeld/compile_modeld.py index f455e9f6e..2f91076ab 100755 --- a/selfdrive/modeld/compile_modeld.py +++ b/selfdrive/modeld/compile_modeld.py @@ -33,7 +33,7 @@ from tinygrad.engine.jit import TinyJit NV12Frame = namedtuple("NV12Frame", ['width', 'height', 'stride', 'y_height', 'uv_height', 'size']) WARP_INPUTS = ['img_q', 'big_img_q', 'tfm', 'big_tfm'] -POLICY_INPUTS = ['feat_q', 'desire_q', 'desire', 'traffic_convention'] +POLICY_INPUTS = ['feat_q', 'desire_q', 'desire', 'traffic_convention', 'action_t'] UV_SCALE_MATRIX = np.array([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 1]], dtype=np.float32) UV_SCALE_MATRIX_INV = np.linalg.inv(UV_SCALE_MATRIX) @@ -121,12 +121,15 @@ def make_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, devi fb = policy_input_shapes['features_buffer'] # (1, 25, 512) dp = policy_input_shapes['desire_pulse'] # (1, 25, 8) tc = policy_input_shapes['traffic_convention'] # (1, 2) + #TODO action_t is hardcoded to match tc for future compatibility + at = tc npy = { 'desire': np.zeros(dp[2], dtype=np.float32), 'traffic_convention': np.zeros(tc, dtype=np.float32), 'tfm': np.zeros((3, 3), dtype=np.float32), 'big_tfm': np.zeros((3, 3), dtype=np.float32), + 'action_t': np.zeros(at, dtype=np.float32), } input_queues = { 'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(), @@ -168,23 +171,31 @@ def make_warp(nv12, model_w, model_h, frame_skip): return warp_enqueue -def make_run_policy(vision_runner, policy_runner, vision_features_slice, frame_skip): +def make_run_policy(vision_runner, on_policy_runner, vision_features_slice, frame_skip): sample_desire_fn = partial(sample_desire, frame_skip=frame_skip) sample_skip_fn = partial(sample_skip, frame_skip=frame_skip) - def run_policy(img, big_img, feat_q, desire_q, desire, traffic_convention): + def run_policy(img, big_img, feat_q, desire_q, desire, traffic_convention, action_t): desire = desire.to(Device.DEFAULT) traffic_convention = traffic_convention.to(Device.DEFAULT) - Tensor.realize(desire, traffic_convention) + action_t = action_t.to(Device.DEFAULT) + Tensor.realize(desire, traffic_convention, action_t) desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn) vision_out = next(iter(vision_runner({'img': img, 'big_img': big_img}).values())).cast('float32') new_feat = vision_out[:, vision_features_slice].reshape(1, -1).unsqueeze(0) feat_buf = shift_and_sample(feat_q, new_feat, sample_skip_fn) - inputs = {'features_buffer': feat_buf, 'desire_pulse': desire_buf, 'traffic_convention': traffic_convention} - policy_out = next(iter(policy_runner(inputs).values())).cast('float32') - return vision_out, policy_out + inputs = { + 'features_buffer': feat_buf, + 'desire_pulse': desire_buf, + 'traffic_convention': traffic_convention, + 'action_t': action_t, + } + on_policy_out = next(iter(on_policy_runner(inputs).values())).cast('float32') + #off_policy_out = next(iter(off_policy_runner(inputs).values())).cast('float32') + return vision_out, on_policy_out + return run_policy @@ -258,31 +269,30 @@ if __name__ == "__main__": p.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True, help='camera resolutions WxH (one or more)') p.add_argument('--vision-onnx', required=True) - p.add_argument('--policy-onnx', required=True) + p.add_argument('--on-policy-onnx', required=True) p.add_argument('--output', required=True) p.add_argument('--frame-skip', type=int, required=True) args = p.parse_args() out = defaultdict(dict) - vision_path, policy_path = read_file_chunked_to_shm(args.vision_onnx), read_file_chunked_to_shm(args.policy_onnx) + vision_path, on_policy_path = read_file_chunked_to_shm(args.vision_onnx), read_file_chunked_to_shm(args.on_policy_onnx) model_w, model_h = args.model_size vision_runner = OnnxRunner(vision_path) - policy_runner = OnnxRunner(policy_path) - vision_metadata, policy_metadata = make_metadata_dict(vision_path), make_metadata_dict(policy_path) + on_policy_runner = OnnxRunner(on_policy_path) + vision_metadata, on_policy_metadata = make_metadata_dict(vision_path), make_metadata_dict(on_policy_path) - run_policy_jit = TinyJit(make_run_policy(vision_runner, policy_runner, vision_metadata['output_slices']['hidden_state'], args.frame_skip), prune=True) - - out['metadata']['vision'], out['metadata']['policy'] = vision_metadata, policy_metadata + run_policy_jit = TinyJit(make_run_policy(vision_runner, on_policy_runner, vision_metadata['output_slices']['hidden_state'], args.frame_skip), prune=True) + out['metadata']['vision'], out['metadata']['on_policy'] = vision_metadata, on_policy_metadata make_random_model_inputs = partial(make_random_images, keys=['img', 'big_img'], shape=vision_metadata['input_shapes']['img']) - out['run_policy'] = compile_jit(run_policy_jit, make_random_model_inputs, POLICY_INPUTS, args.frame_skip, vision_metadata, policy_metadata) + out['run_policy'] = compile_jit(run_policy_jit, make_random_model_inputs, POLICY_INPUTS, args.frame_skip, vision_metadata, on_policy_metadata) for cam_w, cam_h in args.camera_resolutions: nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h)) make_random_warp_inputs = partial(make_random_images, keys=['frame', 'big_frame'], shape=nv12.size, device=WARP_DEV) warp_enqueue = TinyJit(make_warp(nv12, model_w, model_h, args.frame_skip), prune=True) - out[(cam_w,cam_h)] = compile_jit(warp_enqueue, make_random_warp_inputs, WARP_INPUTS, args.frame_skip, vision_metadata, policy_metadata) + out[(cam_w,cam_h)] = compile_jit(warp_enqueue, make_random_warp_inputs, WARP_INPUTS, args.frame_skip, vision_metadata, on_policy_metadata) with open(args.output, "wb") as f: pickle.dump(out, f) diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index ff7e1d860..0fb09262d 100644 --- a/selfdrive/modeld/constants.py +++ b/selfdrive/modeld/constants.py @@ -38,6 +38,7 @@ class ModelConstants: LANE_LINES_WIDTH = 2 ROAD_EDGES_WIDTH = 2 PLAN_WIDTH = 15 + ACTION_WIDTH = 2 DESIRE_PRED_WIDTH = 8 LAT_PLANNER_SOLUTION_WIDTH = 4 DESIRED_CURV_WIDTH = 1 diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 8e14455e1..60454bf3e 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -36,18 +36,22 @@ 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) + 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) - - 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: @@ -82,7 +86,7 @@ class ModelState: self.vision_input_names = list(self.vision_input_shapes.keys()) self.vision_output_slices = vision_metadata['output_slices'] - policy_metadata = jits['metadata']['policy'] + policy_metadata = jits['metadata']['on_policy'] self.policy_input_shapes = policy_metadata['input_shapes'] self.policy_output_slices = policy_metadata['output_slices'] @@ -117,6 +121,7 @@ class ModelState: 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'][:,:] @@ -125,18 +130,18 @@ class ModelState: if prepare_only: return None - vision_output, policy_output = self.run_policy( + vision_output, on_policy_output = self.run_policy( **{k: self.input_queues[k] for k in POLICY_INPUTS}, img=img, big_img=big_img ) vision_output = vision_output.numpy().flatten() - policy_output = policy_output.numpy().flatten() + on_policy_output = on_policy_output.numpy().flatten() vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(vision_output, self.vision_output_slices)) - policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(policy_output, self.policy_output_slices)) + policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(on_policy_output, self.policy_output_slices)) combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict} if SEND_RAW_PRED: - combined_outputs_dict['raw_pred'] = np.concatenate([vision_output.copy(), policy_output.copy()]) + combined_outputs_dict['raw_pred'] = np.concatenate([vision_output.copy(), on_policy_output.copy()]) return combined_outputs_dict @@ -284,9 +289,14 @@ 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} + 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() @@ -299,9 +309,7 @@ def main(demo=False): drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - 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) - action = get_action_from_model(model_output, prev_action, lat_delay + frame_delay + action_delay, long_delay + frame_delay + action_delay, v_ego) + action = 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, diff --git a/selfdrive/modeld/models/big_driving_policy.onnx b/selfdrive/modeld/models/big_driving_on_policy.onnx similarity index 100% rename from selfdrive/modeld/models/big_driving_policy.onnx rename to selfdrive/modeld/models/big_driving_on_policy.onnx diff --git a/selfdrive/modeld/models/driving_policy.onnx b/selfdrive/modeld/models/driving_on_policy.onnx similarity index 100% rename from selfdrive/modeld/models/driving_policy.onnx rename to selfdrive/modeld/models/driving_on_policy.onnx diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py index a0b45d2a9..26c138b8e 100644 --- a/selfdrive/modeld/parse_model_outputs.py +++ b/selfdrive/modeld/parse_model_outputs.py @@ -110,11 +110,7 @@ class Parser: return outs def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: - plan_mhp = self.is_mhp(outs, 'plan', ModelConstants.IDX_N * ModelConstants.PLAN_WIDTH) - plan_in_N, plan_out_N = (ModelConstants.PLAN_MHP_N, ModelConstants.PLAN_MHP_SELECTION) if plan_mhp else (0, 0) - self.parse_mdn('plan', outs, in_N=plan_in_N, out_N=plan_out_N, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH)) - if 'planplus' in outs: - self.parse_mdn('planplus', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH)) + self.parse_mdn('plan', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH)) self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,)) return outs