From 2292e3082f1a0fca29dde49f6bc476a14b376658 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Mon, 30 Dec 2024 17:51:31 +0100 Subject: [PATCH] Refactor curvature handling in modeld input processing Simplified and unified the handling of "desired_curvature" by removing redundant code and centralizing logic. Improved clarity and maintainability by determining the appropriate input key dynamically and implementing consistent array updates. --- selfdrive/modeld/modeld.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index cadf8de100..d645835f7a 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -97,14 +97,19 @@ class ModelState: 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 + if "prev_desired_curvs" in self.numpy_inputs.keys(): - self.numpy_inputs['prev_desired_curvs'][:-1] = self.numpy_inputs['prev_desired_curvs'][1:] - self.numpy_inputs['prev_desired_curvs'][-1] = outputs['desired_curvature'][:, 0:1, None] # Reshape to (1,1,1) - if "prev_desired_curv" in self.numpy_inputs.keys(): - # First shift everything - self.numpy_inputs['prev_desired_curv'][:-ModelConstants.PREV_DESIRED_CURV_LEN] = self.numpy_inputs['prev_desired_curv'][ModelConstants.PREV_DESIRED_CURV_LEN:] - self.numpy_inputs['prev_desired_curv'][-ModelConstants.PREV_DESIRED_CURV_LEN:] = outputs['desired_curvature'][:, :1].reshape(1, -1, 1) + 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