old desired_curv data: MLSIM V1 to Postal Service

* Update model manager logic and handle offroad transitions

Added is_onroad state tracking in SoftwarePanelSP to handle offroad transitions. Updated model manager conditions for improved bundle validation. Removed unnecessary clear operation for ModelManager_DownloadIndex during offroad transitions to optimize behavior.

* Using is_onroad softwarePanel

* Enable model label button only when conditions are met

Previously, the button's state update was misplaced, leading to potential issues with its interactive availability. The logic has been adjusted to ensure it is properly enabled or disabled based on onroad status and download progress. This change improves UX consistency and prevents unintended actions.

* Remove redundant setEnabled call for currentModelLblBtn

The setEnabled call was unnecessary as its functionality was not required in this context. Cleaning up this code improves readability and removes redundant operations. No changes to functionality or behavior were introduced.

---------

Co-authored-by: Jason Wen <haibin.wen3@gmail.com>

selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc

@@ -126,7 +126,6 @@ void SoftwarePanelSP::handleCurrentModelLblBtnClicked() {
     bundleNames.append(index_to_bundle[index]);
   }
 
-  currentModelLblBtn->setEnabled(!is_onroad);
   currentModelLblBtn->setValue(GetActiveModelName());
 
   const QString selectedBundleName = MultiOptionDialog::getSelection(

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

@@ -1,11 +1,9 @@
 #!/usr/bin/env python3
 import os
 import time
-import pickle
 import numpy as np
 import cereal.messaging as messaging
 from cereal import car, log
-from pathlib import Path
 from setproctitle import setproctitle
 from cereal.messaging import PubMaster, SubMaster
 from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
@@ -24,15 +22,11 @@ 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, get_model_generation
+
 PROCESS_NAME = "sunnypilot.modeld.modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
-MODEL_PATHS = {
-  ModelRunner.THNEED: Path(__file__).parent / 'models/supercombo.thneed',
-  ModelRunner.ONNX: Path(__file__).parent / 'models/supercombo.onnx'}
-
-METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'
-
 
 class FrameMeta:
   frame_id: int = 0
@@ -57,23 +51,21 @@ 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)
+    # 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)
 
-    # img buffers are managed in openCL transform code
-    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),
-      'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32),
-    }
-
-    with open(METADATA_PATH, 'rb') as f:
-      model_metadata = pickle.load(f)
+    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]
     self.output = np.zeros(net_output_size, dtype=np.float32)
     self.parser = Parser()
 
-    self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.GPU, False, context)
+    self.model = ModelRunner(model_paths, self.output, Runtime.GPU, False, context)
     self.model.addInput("input_imgs", None)
     self.model.addInput("big_input_imgs", None)
     for k,v in self.inputs.items():
@@ -98,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")))
 
@@ -110,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
 
 
@@ -183,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
@@ -249,10 +251,13 @@ def main(demo=False):
     if prepare_only:
       cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames")
 
-    inputs:dict[str, np.ndarray] = {
+    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)
@@ -265,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

@@ -0,0 +1,65 @@
+# Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+#
+# This file is part of sunnypilot and is licensed under the MIT License.
+# See the LICENSE.md file in the root directory for more details.
+
+import os
+import pickle
+import numpy as np
+from pathlib import Path
+from cereal import custom
+from openpilot.sunnypilot.modeld.runners import ModelRunner
+from openpilot.sunnypilot.models.helpers import get_active_bundle
+from openpilot.system.hardware import PC
+from openpilot.system.hardware.hw import Paths
+
+USE_ONNX = os.getenv('USE_ONNX', PC)
+
+CUSTOM_MODEL_PATH = Paths.model_root()
+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'}
+  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'}
+
+  return model_paths
+
+
+def load_metadata():
+  if bundle := get_active_bundle():
+    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
+
+  with open(metadata_path, 'rb') as f:
+    metadata = pickle.load(f)
+
+  return 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 metadata['input_shapes'].items()
+    if key not in ['input_imgs', 'big_input_imgs']
+  }
+
+  return inputs
+
+
+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 0  # default generation

sunnypilot/models/helpers.py

@@ -22,7 +22,7 @@ async def verify_file(file_path: str, expected_hash: str) -> bool:
   return sha256_hash.hexdigest().lower() == expected_hash.lower()
 
 
-def get_active_bundle(params: Params) -> custom.ModelManagerSP.ModelBundle:
+def get_active_bundle(params: Params = None) -> custom.ModelManagerSP.ModelBundle:
   """Gets the active model bundle from cache"""
   if params is None:
     params = Params()

sunnypilot/models/manager.py

@@ -49,11 +49,9 @@ class ModelManagerSP:
   async def _download_file(self, url: str, path: str, model) -> None:
     """Downloads a file with progress tracking"""
     self._download_start_times[model.fileName] = time.monotonic()
-    cloudlog.debug(f"Downloading {url} to {path}")
 
     async with aiohttp.ClientSession() as session:
       async with session.get(url) as response:
-        cloudlog.debug(f"Response status: {response.status}")
         response.raise_for_status()
         total_size = int(response.headers.get("content-length", 0))
         bytes_downloaded = 0
@@ -127,15 +125,12 @@ class ModelManagerSP:
     """Downloads all models in a bundle"""
     self.selected_bundle = model_bundle
     self.selected_bundle.status = custom.ModelManagerSP.DownloadStatus.downloading
-    cloudlog.debug(f"Downloading bundle {model_bundle.displayName} to {destination_path}")
     os.makedirs(destination_path, exist_ok=True)
 
     try:
-      cloudlog.debug(f"Downloading {len(self.selected_bundle.models)} models")
       tasks = [self._process_model(model, destination_path)
                for model in self.selected_bundle.models]
       await asyncio.gather(*tasks)
-      cloudlog.debug(f"Downloaded {len(self.selected_bundle.models)} models")
       self.selected_bundle.status = custom.ModelManagerSP.DownloadStatus.downloaded
       self.active_bundle = self.selected_bundle
       self.params.put("ModelManager_ActiveBundle", self.selected_bundle.to_bytes())
@@ -160,9 +155,7 @@ class ModelManagerSP:
         self.available_models = self.model_fetcher.get_available_models()
 
         if index_to_download := self.params.get("ModelManager_DownloadIndex", block=False, encoding="utf-8"):
-          cloudlog.debug(f"Downloading model with index {index_to_download}")
           if model_to_download := next((model for model in self.available_models if model.index == int(index_to_download)), None):
-            cloudlog.debug(f"Downloading model {model_to_download.displayName}")
             try:
               self.download(model_to_download, Paths.model_root())
             except Exception as e: