diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index 1cf00b583e..cffcabbc90 100644 --- a/selfdrive/modeld/constants.py +++ b/selfdrive/modeld/constants.py @@ -15,7 +15,9 @@ class ModelConstants: # model inputs constants MODEL_FREQ = 20 FEATURE_LEN = 512 - FULL_HISTORY_BUFFER_LEN = 24 + TEMPORAL_SKIP = 4 + FULL_HISTORY_BUFFER_LEN = 20 * 5 - 1 + FULL_HISTORY_BUFFER_LEN_INPUT = 5 * 5 - 1 DESIRE_LEN = 8 TRAFFIC_CONVENTION_LEN = 2 LAT_PLANNER_STATE_LEN = 4 diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index f93d5c9572..2e200b101e 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -60,15 +60,17 @@ class ModelState: self.frames = {'input_imgs': DrivingModelFrame(context), 'big_input_imgs': DrivingModelFrame(context)} self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) - self.last_feat_age = 0 + 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+1), ModelConstants.DESIRE_LEN), dtype=np.float32) + self.full_prev_desired_curv = np.zeros((1, (ModelConstants.FULL_HISTORY_BUFFER_LEN+1), ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32) # img buffers are managed in openCL transform code self.numpy_inputs = { - 'desire': np.zeros((1, (ModelConstants.FULL_HISTORY_BUFFER_LEN+1), ModelConstants.DESIRE_LEN), dtype=np.float32), + 'desire': np.zeros((1, (ModelConstants.FULL_HISTORY_BUFFER_LEN_INPUT+1), ModelConstants.DESIRE_LEN), dtype=np.float32), 'traffic_convention': np.zeros((1, ModelConstants.TRAFFIC_CONVENTION_LEN), dtype=np.float32), 'lateral_control_params': np.zeros((1, ModelConstants.LATERAL_CONTROL_PARAMS_LEN), dtype=np.float32), - 'prev_desired_curv': np.zeros((1, (ModelConstants.FULL_HISTORY_BUFFER_LEN+1), ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32), - 'features_buffer': np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32), + 'prev_desired_curv': np.zeros((1, (ModelConstants.FULL_HISTORY_BUFFER_LEN_INPUT+1), ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32), + 'features_buffer': np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN_INPUT, ModelConstants.FEATURE_LEN), dtype=np.float32), } with open(METADATA_PATH, 'rb') as f: @@ -100,8 +102,9 @@ class ModelState: new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0) self.prev_desire[:] = inputs['desire'] - self.numpy_inputs['desire'][0,:-1] = self.numpy_inputs['desire'][0,1:] - self.numpy_inputs['desire'][0,-1] = new_desire + self.full_desire[0,:-1] = self.full_desire[0,1:] + self.full_desire[0,-1] = new_desire + self.numpy_inputs['desire'] = self.full_desire[-1-ModelConstants.FULL_HISTORY_BUFFER_LEN_INPUT*ModelConstants.TEMPORAL_SKIP::ModelConstants.TEMPORAL_SKIP] self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention'] self.numpy_inputs['lateral_control_params'][:] = inputs['lateral_control_params'] @@ -127,17 +130,15 @@ class ModelState: outputs = self.parser.parse_outputs(self.slice_outputs(self.output)) - if self.last_feat_age == 4: - print("rotating features") - self.numpy_inputs['features_buffer'][0,:-1] = self.numpy_inputs['features_buffer'][0,1:] - self.numpy_inputs['features_buffer'][0,-1] = outputs['hidden_state'][0, :] - self.last_feat_age = 0 - self.last_feat_age += 1 + self.full_features_buffer[0,:-1] = self.full_features_buffer[0,1:] + self.full_features_buffer[0,-1] = outputs['hidden_state'][0, :] + self.numpy_inputs['features_buffer'] = self.full_features_buffer[-ModelConstants.TEMPORAL_SKIP+1-(ModelConstants.FULL_HISTORY_BUFFER_LEN_INPUT-1)*ModelConstants.TEMPORAL_SKIP-1:-ModelConstants.TEMPORAL_SKIP+1:ModelConstants.TEMPORAL_SKIP] # TODO model only uses last value now - self.numpy_inputs['prev_desired_curv'][0,:-1] = self.numpy_inputs['prev_desired_curv'][0,1:] - self.numpy_inputs['prev_desired_curv'][0,-1,:] = outputs['desired_curvature'][0, :] + self.full_prev_desired_curv[0,:-1] = self.full_prev_desired_curv[0,1:] + self.full_prev_desired_curv[0,-1,:] = outputs['desired_curvature'][0, :] + self.numpy_inputs['prev_desired_curv'] = self.full_prev_desired_curv[-ModelConstants.TEMPORAL_SKIP+1-ModelConstants.FULL_HISTORY_BUFFER_LEN_INPUT*ModelConstants.TEMPORAL_SKIP-1: -ModelConstants.TEMPORAL_SKIP+1:ModelConstants.TEMPORAL_SKIP] return outputs @@ -283,7 +284,6 @@ def main(demo=False): } mt1 = time.perf_counter() - print("running main at", meta_main.frame_id) model_output = model.run(buf_main, buf_extra, model_transform_main, model_transform_extra, inputs, prepare_only) mt2 = time.perf_counter() model_execution_time = mt2 - mt1