old desired_curv data: MLSIM V1 to Postal Service

sunnypilot/modeld/fill_model_msg.py

@@ -69,13 +69,16 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
                    net_output_data: dict[str, np.ndarray], v_ego: float, delay: float,
                    publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int,
                    frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float,
-                   valid: bool) -> None:
+                   valid: bool, generation: int) -> None:
   frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
   frame_drop_perc = frame_drop * 100
   extended_msg.valid = valid
   base_msg.valid = valid
 
-  desired_curv = float(get_curvature_from_plan(net_output_data['plan'][0], v_ego, delay))
+  if generation >= 7:
+    desired_curv = float(get_curvature_from_plan(net_output_data['plan'][0], v_ego, delay))
+  else:
+    desired_curv = float(net_output_data['desired_curvature'][0, 0])
 
   driving_model_data = base_msg.drivingModelData
 

sunnypilot/modeld/modeld.py

@@ -22,7 +22,7 @@ from openpilot.sunnypilot.modeld.fill_model_msg import fill_model_msg, fill_pose
 from openpilot.sunnypilot.modeld.constants import ModelConstants
 from openpilot.sunnypilot.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
 
-from openpilot.sunnypilot.modeld.runners.run_helpers import load_model, load_metadata, prepare_inputs, parse_runner_inputs
+from openpilot.sunnypilot.modeld.runners.run_helpers import load_model, load_metadata, prepare_inputs, get_model_generation
 
 PROCESS_NAME = "sunnypilot.modeld.modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -51,12 +51,14 @@ class ModelState:
     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.inputs = prepare_inputs()
+    # Used for MLSIM V0 to Null Pointer
+    # inputs including: lateral_control_params & prev_desired_curv
+    # outputs including: desired_curvature
+    self.prev_desired_curv_20hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN + 1, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32)
 
     model_paths = load_model()
     model_metadata = load_metadata()
+    self.inputs = prepare_inputs(model_metadata)
 
     self.output_slices = model_metadata['output_slices']
     net_output_size = model_metadata['output_shapes']['outputs'][1]
@@ -88,6 +90,9 @@ class ModelState:
 
     self.inputs['traffic_convention'][:] = inputs['traffic_convention']
 
+    if 'lateral_control_params' in inputs.keys():
+      self.inputs['lateral_control_params'][:] = inputs['lateral_control_params']
+
     self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs")))
     self.model.setInputBuffer("big_input_imgs", self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs")))
 
@@ -100,8 +105,14 @@ class ModelState:
     self.full_features_20Hz[:-1] = self.full_features_20Hz[1:]
     self.full_features_20Hz[-1] = outputs['hidden_state'][0, :]
 
+    if 'desired_curvature' in outputs.keys():
+      self.prev_desired_curv_20hz[:-1] = self.prev_desired_curv_20hz[1:]
+      self.prev_desired_curv_20hz[-1] = outputs['desired_curvature'][0, :]
+
     idxs = np.arange(-4,-100,-4)[::-1]
     self.inputs['features_buffer'][:] = self.full_features_20Hz[idxs].flatten()
+    if 'prev_desired_curv' in inputs.keys():
+      self.inputs['prev_desired_curv'][-ModelConstants.PREV_DESIRED_CURV_LEN:] = 0. * self.prev_desired_curv_20hz[-4, :]
     return outputs
 
 
@@ -173,6 +184,7 @@ def main(demo=False):
   steer_delay = CP.steerActuatorDelay + .2
 
   DH = DesireHelper()
+  generation = get_model_generation()
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -239,7 +251,13 @@ def main(demo=False):
     if prepare_only:
       cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames")
 
-    inputs = parse_runner_inputs(vec_desire, traffic_convention)
+    inputs: dict[str, np.ndarray] = {
+      'desire': vec_desire,
+      'traffic_convention': traffic_convention,
+    }
+
+    if 'lateral_control_params' in model.inputs.keys():
+      inputs['lateral_control_params'] = np.array([v_ego, steer_delay], dtype=np.float32)
 
     mt1 = time.perf_counter()
     model_output = model.run(buf_main, buf_extra, model_transform_main, model_transform_extra, inputs, prepare_only)
@@ -252,7 +270,8 @@ def main(demo=False):
       posenet_send = messaging.new_message('cameraOdometry')
       fill_model_msg(drivingdata_send, modelv2_send, model_output, v_ego, steer_delay,
                      publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
-                     frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen)
+                     frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen,
+                     generation)
 
       desire_state = modelv2_send.modelV2.meta.desireState
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]

sunnypilot/modeld/parse_model_outputs.py

@@ -96,6 +96,8 @@ class Parser:
                    out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
     if 'lat_planner_solution' in outs:
       self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
+    if 'desired_curvature' in outs:
+      self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
     for k in ['lead_prob', 'lane_lines_prob', 'meta']:
       self.parse_binary_crossentropy(k, outs)
     self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))

sunnypilot/modeld/runners/run_helpers.py

@@ -13,30 +13,30 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle
 from openpilot.system.hardware import PC
 from openpilot.system.hardware.hw import Paths
 
-USE_ONNX = int(os.getenv('USE_ONNX', str(int(PC))))
+USE_ONNX = os.getenv('USE_ONNX', PC)
 
 CUSTOM_MODEL_PATH = Paths.model_root()
-METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'
+METADATA_PATH = Path(__file__).parent / '../models/supercombo_metadata.pkl'
 
 ModelManager = custom.ModelManagerSP
 
 
 def load_model():
   if USE_ONNX:
-    model_paths = {ModelRunner.ONNX: Path(__file__).parent / 'models/supercombo.onnx'}
+    model_paths = {ModelRunner.ONNX: Path(__file__).parent / '../models/supercombo.onnx'}
   elif bundle := get_active_bundle():
     drive_model = next(model for model in bundle.models if model.type == ModelManager.Type.drive)
     model_paths = {ModelRunner.THNEED: f"{CUSTOM_MODEL_PATH}/{drive_model.fileName}"}
   else:
-    model_paths = {ModelRunner.THNEED: Path(__file__).parent / 'models/supercombo.thneed'}
+    model_paths = {ModelRunner.THNEED: Path(__file__).parent / '../models/supercombo.thneed'}
 
   return model_paths
 
 
 def load_metadata():
   if bundle := get_active_bundle():
-    drive_model = next(model for model in bundle.models if model.type == ModelManager.Type.metadata)
-    metadata_path = f"{CUSTOM_MODEL_PATH}/{drive_model.fileName}"
+    metadata_model = next(model for model in bundle.models if model.type == ModelManager.Type.metadata)
+    metadata_path = f"{CUSTOM_MODEL_PATH}/{metadata_model.fileName}"
   else:
     metadata_path = METADATA_PATH
 
@@ -46,22 +46,20 @@ def load_metadata():
   return metadata
 
 
-def prepare_inputs() -> dict[str, np.ndarray]:
-  model_metadata = load_metadata()
-
+def prepare_inputs(metadata) -> dict[str, np.ndarray]:
+  # img buffers are managed in openCL transform code
   inputs: dict[str, np.ndarray] = {
     key: np.zeros(shape, dtype=np.float32)
-    for key, shape in model_metadata['input_shapes'].items()
+    for key, shape in metadata['input_shapes'].items()
     if key not in ['input_imgs', 'big_input_imgs']
   }
 
   return inputs
 
 
-def parse_runner_inputs(vec_desire, traffic_convention) -> dict[str, np.ndarray]:
-  inputs: dict[str, np.ndarray] = {
-    'desire': vec_desire,
-    'traffic_convention': traffic_convention,
-  }
+def get_model_generation() -> int:
+  if bundle := get_active_bundle():
+    drive_model = next(model for model in bundle.models if model.type == ModelManager.Type.drive)
+    return drive_model.generation
 
-  return inputs
+  return 0  # default generation