From 79e44dbc6ea071a0311b9567444d89772ec9c9ff Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 19 Jan 2025 21:43:43 +0100 Subject: [PATCH] Add buffer length parameter for enhanced frame handling Introduce a configurable `buffer_length` parameter to `DrivingModelFrame` to support dynamic buffer sizes, enabling better handling of different frame rates like 20Hz. Updates include necessary adjustments in buffer initialization, copying logic, and related model inputs for improved compatibility and flexibility. --- selfdrive/modeld/modeld.py | 71 +++++++++++++++------ selfdrive/modeld/models/commonmodel.cc | 10 +-- selfdrive/modeld/models/commonmodel.h | 3 +- selfdrive/modeld/models/commonmodel_pyx.pyx | 2 +- 4 files changed, 61 insertions(+), 25 deletions(-) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 5783f6aa8e..b677eb3e15 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -57,23 +57,26 @@ class ModelState: prev_desire: np.ndarray # for tracking the rising edge of the pulse def __init__(self, context: CLContext): - self.frames = {'input_imgs': DrivingModelFrame(context), 'big_input_imgs': DrivingModelFrame(context)} + self.is_20hz = False + + buffer_length = 5 if self.is_20hz else 2 + self.frames = {'input_imgs': DrivingModelFrame(context, buffer_length), 'big_input_imgs': DrivingModelFrame(context, buffer_length)} 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) # 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), - '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), - } + self.numpy_inputs = {} with open(METADATA_PATH, 'rb') as f: model_metadata = pickle.load(f) - self.input_shapes = model_metadata['input_shapes'] - + self.input_shapes = model_metadata['input_shapes'] self.output_slices = model_metadata['output_slices'] + + for key, shape in self.input_shapes.items(): + if key not in self.frames: # Managed by opencl + self.numpy_inputs[key] = np.zeros(shape, dtype=np.float32) + net_output_size = model_metadata['output_shapes']['outputs'][1] self.output = np.zeros(net_output_size, dtype=np.float32) self.parser = Parser() @@ -85,6 +88,14 @@ class ModelState: else: self.onnx_cpu_runner = make_onnx_cpu_runner(MODEL_PATH) + net_output_size = model_metadata['output_shapes']['outputs'][1] + self.output = np.zeros(net_output_size, dtype=np.float32) + + num_elements = self.numpy_inputs['features_buffer'].shape[1] + step_size = int(-100 / num_elements) + self.full_features_20Hz_idxs = np.arange(step_size, step_size * (num_elements + 1), step_size)[::-1] + self.desire_reshape_dims = (self.numpy_inputs['desire'].shape[0], self.numpy_inputs['desire'].shape[1], -1, self.numpy_inputs['desire'].shape[2]) + def slice_outputs(self, model_outputs: np.ndarray) -> dict[str, np.ndarray]: parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in self.output_slices.items()} if SEND_RAW_PRED: @@ -98,11 +109,19 @@ 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 + if not self.is_20hz: + len = inputs['desire'].shape[0] + self.numpy_inputs['desire'][0, :-1] = self.numpy_inputs['desire'][0, 1:] + self.numpy_inputs['desire'][0, -1, :len] = new_desire[:len] + else: + self.desire_20Hz[:-1] = self.desire_20Hz[1:] + self.desire_20Hz[-1] = new_desire + self.numpy_inputs['desire'][:] = self.desire_20Hz.reshape(self.desire_reshape_dims).max(axis=2) + + for key in self.numpy_inputs: + if key in inputs and key not in ['desire']: + self.numpy_inputs[key][:] = inputs[key] - self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention'] - self.numpy_inputs['lateral_control_params'][:] = inputs['lateral_control_params'] imgs_cl = {'input_imgs': self.frames['input_imgs'].prepare(buf, transform.flatten()), 'big_input_imgs': self.frames['big_input_imgs'].prepare(wbuf, transform_wide.flatten())} @@ -125,13 +144,27 @@ class ModelState: outputs = self.parser.parse_outputs(self.slice_outputs(self.output)) - 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, :] + if not self.is_20hz: + feature_len = outputs['hidden_state'].shape[1] + self.numpy_inputs['features_buffer'][0, :-1] = self.numpy_inputs['features_buffer'][0, 1:] + self.numpy_inputs['features_buffer'][0, -1, :feature_len] = outputs['hidden_state'][0, :feature_len] + else: + self.full_features_20Hz[:-1] = self.full_features_20Hz[1:] + self.full_features_20Hz[-1] = outputs['hidden_state'][0, :] + self.numpy_inputs['features_buffer'][:] = self.full_features_20Hz[self.full_features_20Hz_idxs] + if "desired_curvature" in outputs: + input_name_prev = None - # 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, :] + if "prev_desired_curvs" in self.numpy_inputs.keys(): + input_name_prev = 'prev_desired_curvs' + elif "prev_desired_curv" in self.numpy_inputs.keys(): + input_name_prev = 'prev_desired_curv' + + if input_name_prev is not None: + len = outputs['desired_curvature'][0].size + self.numpy_inputs[input_name_prev][0, :-len, 0] = self.numpy_inputs[input_name_prev][0, len:, 0] + self.numpy_inputs[input_name_prev][0, -len:, 0] = outputs['desired_curvature'][0] return outputs diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc index 9973d18588..7870363b80 100644 --- a/selfdrive/modeld/models/commonmodel.cc +++ b/selfdrive/modeld/models/commonmodel.cc @@ -5,13 +5,14 @@ #include "common/clutil.h" -DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context) : ModelFrame(device_id, context) { +DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context, uint8_t buffer_length) : ModelFrame(device_id, context), buffer_length(buffer_length) { input_frames = std::make_unique(buf_size); input_frames_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err)); - img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, 2*frame_size_bytes, NULL, &err)); - region.origin = 1 * frame_size_bytes; + img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buffer_length*frame_size_bytes, NULL, &err)); + region.origin = (buffer_length - 1) * frame_size_bytes; region.size = frame_size_bytes; last_img_cl = CL_CHECK_ERR(clCreateSubBuffer(img_buffer_20hz_cl, CL_MEM_READ_WRITE, CL_BUFFER_CREATE_TYPE_REGION, ®ion, &err)); + printf("Buffer length: %d, region origin: %lu, region size: %lu\n", buffer_length, region.origin, region.size); loadyuv_init(&loadyuv, context, device_id, MODEL_WIDTH, MODEL_HEIGHT); init_transform(device_id, context, MODEL_WIDTH, MODEL_HEIGHT); @@ -20,7 +21,8 @@ DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context) cl_mem* DrivingModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) { run_transform(yuv_cl, MODEL_WIDTH, MODEL_HEIGHT, frame_width, frame_height, frame_stride, frame_uv_offset, projection); - for (int i = 0; i < 1; i++) { + for (int i = 0; i < (buffer_length - 1); i++) { + printf("Moving %d to %d from src_offset %lu to dst_offset %lu with size %lu\n", i+1, i, (i+1)*frame_size_bytes, i*frame_size_bytes, frame_size_bytes); CL_CHECK(clEnqueueCopyBuffer(q, img_buffer_20hz_cl, img_buffer_20hz_cl, (i+1)*frame_size_bytes, i*frame_size_bytes, frame_size_bytes, 0, nullptr, nullptr)); } loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, last_img_cl); diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 14409943e4..46bdb27496 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -64,7 +64,7 @@ protected: class DrivingModelFrame : public ModelFrame { public: - DrivingModelFrame(cl_device_id device_id, cl_context context); + DrivingModelFrame(cl_device_id device_id, cl_context context, uint8_t buffer_length = 2); ~DrivingModelFrame(); cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection); @@ -73,6 +73,7 @@ public: const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2; const int buf_size = MODEL_FRAME_SIZE * 2; const size_t frame_size_bytes = MODEL_FRAME_SIZE * sizeof(uint8_t); + const uint8_t buffer_length; private: LoadYUVState loadyuv; diff --git a/selfdrive/modeld/models/commonmodel_pyx.pyx b/selfdrive/modeld/models/commonmodel_pyx.pyx index 7b3a5bb342..c02dcf9960 100644 --- a/selfdrive/modeld/models/commonmodel_pyx.pyx +++ b/selfdrive/modeld/models/commonmodel_pyx.pyx @@ -59,7 +59,7 @@ cdef class ModelFrame: cdef class DrivingModelFrame(ModelFrame): cdef cppDrivingModelFrame * _frame - def __cinit__(self, CLContext context): + def __cinit__(self, CLContext context, int buffer_length=2): self._frame = new cppDrivingModelFrame(context.device_id, context.context) self.frame = (self._frame) self.buf_size = self._frame.buf_size