SGO

selfdrive/modeld/constants.py

@@ -15,7 +15,8 @@ class ModelConstants:
   # model inputs constants
   MODEL_FREQ = 20
   FEATURE_LEN = 512
-  HISTORY_BUFFER_LEN = 99
+  FULL_HISTORY_BUFFER_LEN = 99
+  HISTORY_BUFFER_LEN = 24
   DESIRE_LEN = 8
   TRAFFIC_CONVENTION_LEN = 2
   LAT_PLANNER_STATE_LEN = 4
@@ -72,14 +73,13 @@ class Plan:
 class Meta:
   ENGAGED = slice(0, 1)
   # next 2, 4, 6, 8, 10 seconds
-  GAS_DISENGAGE = slice(1, 41, 8)
-  BRAKE_DISENGAGE = slice(2, 41, 8)
-  STEER_OVERRIDE = slice(3, 41, 8)
-  HARD_BRAKE_3 = slice(4, 41, 8)
-  HARD_BRAKE_4 = slice(5, 41, 8)
-  HARD_BRAKE_5 = slice(6, 41, 8)
-  GAS_PRESS = slice(7, 41, 8)
-  BRAKE_PRESS = slice(8, 41, 8)
+  GAS_DISENGAGE = slice(1, 36, 7)
+  BRAKE_DISENGAGE = slice(2, 36, 7)
+  STEER_OVERRIDE = slice(3, 36, 7)
+  HARD_BRAKE_3 = slice(4, 36, 7)
+  HARD_BRAKE_4 = slice(5, 36, 7)
+  HARD_BRAKE_5 = slice(6, 36, 7)
+  GAS_PRESS = slice(7, 36, 7)
   # next 0, 2, 4, 6, 8, 10 seconds
-  LEFT_BLINKER = slice(41, 53, 2)
-  RIGHT_BLINKER = slice(42, 53, 2)
+  LEFT_BLINKER = slice(36, 48, 2)
+  RIGHT_BLINKER = slice(37, 48, 2)

selfdrive/modeld/fill_model_msg.py

@@ -164,8 +164,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_3].tolist()
   disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_4].tolist()
   disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_5].tolist()
-  disengage_predictions.gasPressProbs = net_output_data['meta'][0,Meta.GAS_PRESS].tolist()
-  disengage_predictions.brakePressProbs = net_output_data['meta'][0,Meta.BRAKE_PRESS].tolist()
 
   publish_state.prev_brake_5ms2_probs[:-1] = publish_state.prev_brake_5ms2_probs[1:]
   publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_5][0]

selfdrive/modeld/modeld.py

@@ -34,6 +34,10 @@
 
 METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'
 
+MODEL_WIDTH = 512
+MODEL_HEIGHT = 256
+MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 // 2
+
 class FrameMeta:
   frame_id: int = 0
   timestamp_sof: int = 0
@@ -55,6 +59,8 @@ def __init__(self, context: CLContext):
     self.frame = ModelFrame(context)
     self.wide_frame = ModelFrame(context)
     self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
+    self.full_features_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32)
+    self.desire_20Hz =  np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN + 1, ModelConstants.DESIRE_LEN), dtype=np.float32)
     self.inputs = {
       'desire': np.zeros(ModelConstants.DESIRE_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32),
       'traffic_convention': np.zeros(ModelConstants.TRAFFIC_CONVENTION_LEN, dtype=np.float32),
@@ -63,6 +69,11 @@ def __init__(self, context: CLContext):
       'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32),
     }
 
+    self.input_imgs_20hz = np.zeros(MODEL_FRAME_SIZE*5, dtype=np.float32)
+    self.big_input_imgs_20hz = np.zeros(MODEL_FRAME_SIZE*5, dtype=np.float32)
+    self.input_imgs = np.zeros(MODEL_FRAME_SIZE*2, dtype=np.float32)
+    self.big_input_imgs = np.zeros(MODEL_FRAME_SIZE*2, dtype=np.float32)
+
     with open(METADATA_PATH, 'rb') as f:
       model_metadata = pickle.load(f)
 
@@ -87,26 +98,42 @@ def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_
                 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['desire'][0] = 0
-    self.inputs['desire'][:-ModelConstants.DESIRE_LEN] = self.inputs['desire'][ModelConstants.DESIRE_LEN:]
-    self.inputs['desire'][-ModelConstants.DESIRE_LEN:] = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
+    new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
     self.prev_desire[:] = inputs['desire']
 
+    self.desire_20Hz[:-1] = self.desire_20Hz[1:]
+    self.desire_20Hz[-1] = new_desire
+    self.inputs['desire'][:] = self.desire_20Hz.reshape((25,4,-1)).max(axis=1).flatten()
+
     self.inputs['traffic_convention'][:] = inputs['traffic_convention']
     self.inputs['lateral_control_params'][:] = inputs['lateral_control_params']
 
-    # if getCLBuffer is not None, frame will be None
-    self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs")))
+    new_img = self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs"))
+    self.input_imgs_20hz[:-MODEL_FRAME_SIZE] = self.input_imgs_20hz[MODEL_FRAME_SIZE:]
+    self.input_imgs_20hz[-MODEL_FRAME_SIZE:] = new_img
+    self.input_imgs[:MODEL_FRAME_SIZE] = self.input_imgs_20hz[:MODEL_FRAME_SIZE]
+    self.input_imgs[MODEL_FRAME_SIZE:] = self.input_imgs_20hz[-MODEL_FRAME_SIZE:]
+    self.model.setInputBuffer("input_imgs", self.input_imgs)
     if wbuf is not None:
-      self.model.setInputBuffer("big_input_imgs", self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs")))
+      new_big_img = self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs"))
+      self.big_input_imgs_20hz[:-MODEL_FRAME_SIZE] = self.big_input_imgs_20hz[MODEL_FRAME_SIZE:]
+      self.big_input_imgs_20hz[-MODEL_FRAME_SIZE:] = new_big_img
+      self.big_input_imgs[:MODEL_FRAME_SIZE] = self.big_input_imgs_20hz[:MODEL_FRAME_SIZE]
+      self.big_input_imgs[MODEL_FRAME_SIZE:] = self.big_input_imgs_20hz[-MODEL_FRAME_SIZE:]
+      self.model.setInputBuffer("big_input_imgs", self.big_input_imgs)
+
 
     if prepare_only:
       return None
 
     self.model.execute()
     outputs = self.parser.parse_outputs(self.slice_outputs(self.output))
 
-    self.inputs['features_buffer'][:-ModelConstants.FEATURE_LEN] = self.inputs['features_buffer'][ModelConstants.FEATURE_LEN:]
-    self.inputs['features_buffer'][-ModelConstants.FEATURE_LEN:] = outputs['hidden_state'][0, :]
+    self.full_features_20Hz[:-1] = self.full_features_20Hz[1:]
+    self.full_features_20Hz[-1] = outputs['hidden_state'][0, :]
+    idxs = np.arange(-4,-100,-4)[::-1]
+    self.inputs['features_buffer'][:] = self.full_features_20Hz[idxs].flatten()
+    # TODO model only uses last value now, once that changes we need to input strided action history buffer
     self.inputs['prev_desired_curv'][:-ModelConstants.PREV_DESIRED_CURV_LEN] = self.inputs['prev_desired_curv'][ModelConstants.PREV_DESIRED_CURV_LEN:]
     self.inputs['prev_desired_curv'][-ModelConstants.PREV_DESIRED_CURV_LEN:] = outputs['desired_curvature'][0, :]
     return outputs
@@ -174,7 +201,6 @@ def main(demo=False):
     CP = convert_to_capnp(get_demo_car_params())
   else:
     CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
-
   cloudlog.info("modeld got CarParams: %s", CP.carName)
 
   # TODO this needs more thought, use .2s extra for now to estimate other delays
@@ -272,8 +298,6 @@ def main(demo=False):
       DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_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)

selfdrive/modeld/models/commonmodel.cc

@@ -7,7 +7,7 @@
 #include "common/clutil.h"
 
 ModelFrame::ModelFrame(cl_device_id device_id, cl_context context) {
-  input_frames = std::make_unique<float[]>(buf_size);
+  frame = std::make_unique<float[]>(MODEL_FRAME_SIZE);
 
   q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
   y_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, MODEL_WIDTH * MODEL_HEIGHT, NULL, &err));
@@ -23,20 +23,10 @@ float* ModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int
   transform_queue(&this->transform, q,
                   yuv_cl, frame_width, frame_height, frame_stride, frame_uv_offset,
                   y_cl, u_cl, v_cl, MODEL_WIDTH, MODEL_HEIGHT, projection);
-
-  if (output == NULL) {
-    loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, net_input_cl);
-
-    std::memmove(&input_frames[0], &input_frames[MODEL_FRAME_SIZE], sizeof(float) * MODEL_FRAME_SIZE);
-    CL_CHECK(clEnqueueReadBuffer(q, net_input_cl, CL_TRUE, 0, MODEL_FRAME_SIZE * sizeof(float), &input_frames[MODEL_FRAME_SIZE], 0, nullptr, nullptr));
-    clFinish(q);
-    return &input_frames[0];
-  } else {
-    loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, *output, true);
-    // NOTE: Since thneed is using a different command queue, this clFinish is needed to ensure the image is ready.
-    clFinish(q);
-    return NULL;
-  }
+  loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, net_input_cl);
+  CL_CHECK(clEnqueueReadBuffer(q, net_input_cl, CL_TRUE, 0, MODEL_FRAME_SIZE * sizeof(float), &frame[0], 0, nullptr, nullptr));
+  clFinish(q);
+  return &frame[0];
 }
 
 ModelFrame::~ModelFrame() {

selfdrive/modeld/models/commonmodel.h

@@ -25,12 +25,11 @@ class ModelFrame {
   const int MODEL_WIDTH = 512;
   const int MODEL_HEIGHT = 256;
   const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2;
-  const int buf_size = MODEL_FRAME_SIZE * 2;
 
 private:
   Transform transform;
   LoadYUVState loadyuv;
   cl_command_queue q;
   cl_mem y_cl, u_cl, v_cl, net_input_cl;
-  std::unique_ptr<float[]> input_frames;
+  std::unique_ptr<float[]> frame;
 };

selfdrive/modeld/models/commonmodel.pxd

@@ -13,6 +13,6 @@ cdef extern from "common/clutil.h":
 
 cdef extern from "selfdrive/modeld/models/commonmodel.h":
   cppclass ModelFrame:
-    int buf_size
+    int MODEL_FRAME_SIZE
     ModelFrame(cl_device_id, cl_context)
     float * prepare(cl_mem, int, int, int, int, mat3, cl_mem*)

selfdrive/modeld/models/commonmodel_pyx.pyx

@@ -42,4 +42,4 @@ cdef class ModelFrame:
       data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, output.mem)
     if not data:
       return None
-    return np.asarray(<cnp.float32_t[:self.frame.buf_size]> data)
+    return np.asarray(<cnp.float32_t[:self.frame.MODEL_FRAME_SIZE]> data)