correct temporal

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

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