Merge branch 'master' of https://github.com/sunnypilot/sunnypilot into sync

common/params_keys.h

@@ -236,6 +236,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"LagdValueCache", {PERSISTENT, FLOAT, "0.2"}},
     {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}},
+    {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
 
     // mapd
     {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}},

sunnypilot/modeld_v2/SConscript

@@ -1,3 +1,4 @@
+import os
 import glob
 
 Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'visionipc', 'transformations')
@@ -28,3 +29,38 @@ for pathdef, fn in {'TRANSFORM': 'transforms/transform.cl', 'LOADYUV': 'transfor
 cython_libs = envCython["LIBS"] + libs
 commonmodel_lib = lenv.Library('commonmodel', common_src)
 lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LIBS=[commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks)
+tinygrad_files = ["#"+x for x in glob.glob(env.Dir("#tinygrad_repo").relpath + "/**", recursive=True, root_dir=env.Dir("#").abspath) if 'pycache' not in x]
+
+# Get model metadata
+PC = not os.path.isfile('/TICI')
+if PC:
+  inputs = tinygrad_files + [File(Dir("#sunnypilot/modeld_v2").File("install_models_pc.py").abspath)]
+  outputs = []
+  model_dir = Dir("models").abspath
+  cmd = f'python3 {Dir("#sunnypilot/modeld_v2").abspath}/install_models_pc.py {model_dir}'
+
+  for model_name in ['supercombo', 'driving_vision', 'driving_policy']:
+    if File(f"models/{model_name}.onnx").exists():
+        inputs.append(File(f"models/{model_name}.onnx"))
+        inputs.append(File(f"models/{model_name}_tinygrad.pkl"))
+        outputs.append(File(f"models/{model_name}_metadata.pkl"))
+  if outputs:
+    lenv.Command(outputs, inputs, cmd)
+
+def tg_compile(flags, model_name):
+  pythonpath_string = 'PYTHONPATH="${PYTHONPATH}:' + env.Dir("#tinygrad_repo").abspath + '"'
+  fn = File(f"models/{model_name}").abspath
+  return lenv.Command(
+    fn + "_tinygrad.pkl",
+    [fn + ".onnx"] + tinygrad_files,
+    f'{pythonpath_string} {flags} python3 {Dir("#tinygrad_repo").abspath}/examples/openpilot/compile3.py {fn}.onnx {fn}_tinygrad.pkl'
+  )
+
+# Compile small models
+for model_name in ['supercombo', 'driving_vision', 'driving_policy']:
+  if File(f"models/{model_name}.onnx").exists():
+    flags = {
+      'larch64': 'DEV=QCOM',
+      'Darwin': f'DEV=CPU HOME={os.path.expanduser("~")} IMAGE=0', # tinygrad calls brew which needs a $HOME in the env
+    }.get(arch, 'DEV=CPU CPU_LLVM=1 IMAGE=0')
+    tg_compile(flags, model_name)

sunnypilot/modeld_v2/install_models_pc.py

@@ -0,0 +1,89 @@
+#!/usr/bin/env python3
+import sys
+import shutil
+import pickle
+import codecs
+import onnx
+from pathlib import Path
+
+from openpilot.system.hardware.hw import Paths
+
+
+def get_name_and_shape(value_info):
+  shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim])
+  return value_info.name, shape
+
+
+def get_metadata_value_by_name(model, name):
+  for prop in model.metadata_props:
+    if prop.key == name:
+      return prop.value
+  return None
+
+
+def generate_metadata_pkl(model_path, output_path):
+  try:
+    model = onnx.load(str(model_path))
+    output_slices = get_metadata_value_by_name(model, 'output_slices')
+
+    if output_slices:
+      metadata = {
+        'model_checkpoint': get_metadata_value_by_name(model, 'model_checkpoint'),
+        'output_slices': pickle.loads(codecs.decode(output_slices.encode(), "base64")),
+        'input_shapes': dict([get_name_and_shape(x) for x in model.graph.input]),
+        'output_shapes': dict([get_name_and_shape(x) for x in model.graph.output])
+      }
+      with open(output_path, 'wb') as f:
+        pickle.dump(metadata, f)
+      return True
+    else:
+      return False
+  except Exception:
+    return False
+
+
+def install_models(model_dir):
+  model_dir = Path(model_dir)
+  models = ["driving_policy", "driving_vision"]
+  found_models = []
+
+  for model in models:
+    if (model_dir / f"{model}.onnx").exists():
+      found_models.append(model)
+
+  if not found_models:
+    return
+
+  try:
+    custom_name = input(f"Found models ({', '.join(found_models)}). Enter model short name (e.g. wmiv4): ").strip()
+  except EOFError:
+    return
+
+  if not custom_name:
+    print("No name provided, skipping installation.")
+    return
+
+  dest_dir = Path(Paths.model_root())
+  dest_dir.mkdir(parents=True, exist_ok=True)
+
+  for model in found_models:
+    onnx_path = model_dir / f"{model}.onnx"
+    tinygrad_pkl = model_dir / f"{model}_tinygrad.pkl"
+    metadata_pkl = model_dir / f"{model}_metadata.pkl"
+
+    if not metadata_pkl.exists():
+      generate_metadata_pkl(onnx_path, metadata_pkl)
+
+    dest_tinygrad = dest_dir / f"{model}_{custom_name}_tinygrad.pkl"
+    dest_metadata = dest_dir / f"{model}_{custom_name}_metadata.pkl"
+
+    if tinygrad_pkl.exists():
+      shutil.move(str(tinygrad_pkl), str(dest_tinygrad))
+    if metadata_pkl.exists():
+      shutil.move(str(metadata_pkl), str(dest_metadata))
+
+if __name__ == "__main__":
+  if len(sys.argv) < 2:
+    print("Usage: install_models_pc.py <model_dir>")
+    sys.exit(1)
+  install_models(sys.argv[1])

sunnypilot/modeld_v2/modeld.py

@@ -28,7 +28,6 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle
 from openpilot.sunnypilot.models.runners.helpers import get_model_runner
 
 PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad"
-RECOVERY_POWER = 1.0 # The higher this number the more aggressively the model will recover to lanecenter, too high and it will ping-pong
 
 
 class FrameMeta:
@@ -63,6 +62,7 @@ class ModelState(ModelStateBase):
     self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0"))
     self.LONG_SMOOTH_SECONDS = float(overrides.get('long', ".0"))
     self.MIN_LAT_CONTROL_SPEED = 0.3
+    self.PLANPLUS_CONTROL: float = 1.0
 
     buffer_length = 5 if self.model_runner.is_20hz else 2
     self.frames = {name: DrivingModelFrame(context, buffer_length) for name in self.model_runner.vision_input_names}
@@ -158,7 +158,8 @@ class ModelState(ModelStateBase):
                             lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action:
     plan = model_output['plan'][0]
     if 'planplus' in model_output:
-      plan = plan + RECOVERY_POWER*model_output['planplus'][0]
+      recovery_power = self.PLANPLUS_CONTROL * (0.75 if v_ego > 20.0 else 1.0)
+      plan = plan + recovery_power * model_output['planplus'][0]
     desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
                                                      action_t=long_action_t)
     desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
@@ -283,6 +284,7 @@ def main(demo=False):
     v_ego = max(sm["carState"].vEgo, 0.)
     if sm.frame % 60 == 0:
       model.lat_delay = get_lat_delay(params, sm["liveDelay"].lateralDelay)
+      model.PLANPLUS_CONTROL = params.get("PlanplusControl", return_default=True)
     lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS
     if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)

sunnypilot/modeld_v2/tests/test_modeld.py

@@ -1,102 +0,0 @@
-import numpy as np
-import random
-
-import cereal.messaging as messaging
-from msgq.visionipc import VisionIpcServer, VisionStreamType
-from opendbc.car.car_helpers import get_demo_car_params
-from openpilot.common.params import Params
-from openpilot.common.transformations.camera import DEVICE_CAMERAS
-from openpilot.common.realtime import DT_MDL
-from openpilot.system.manager.process_config import managed_processes
-from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state
-
-CAM = DEVICE_CAMERAS[("tici", "ar0231")].fcam
-IMG = np.zeros(int(CAM.width*CAM.height*(3/2)), dtype=np.uint8)
-IMG_BYTES = IMG.flatten().tobytes()
-
-
-class TestModeld:
-
-  def setup_method(self):
-    self.vipc_server = VisionIpcServer("camerad")
-    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, CAM.width, CAM.height)
-    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, CAM.width, CAM.height)
-    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, CAM.width, CAM.height)
-    self.vipc_server.start_listener()
-    Params().put("CarParams", get_demo_car_params().to_bytes())
-
-    self.sm = messaging.SubMaster(['modelV2', 'cameraOdometry'])
-    self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'liveCalibration'])
-
-    managed_processes['modeld'].start()
-    self.pm.wait_for_readers_to_update("roadCameraState", 10)
-
-  def teardown_method(self):
-    managed_processes['modeld'].stop()
-    del self.vipc_server
-
-  def _send_frames(self, frame_id, cams=None):
-    if cams is None:
-      cams = ('roadCameraState', 'wideRoadCameraState')
-
-    cs = None
-    for cam in cams:
-      msg = messaging.new_message(cam)
-      cs = getattr(msg, cam)
-      cs.frameId = frame_id
-      cs.timestampSof = int((frame_id * DT_MDL) * 1e9)
-      cs.timestampEof = int(cs.timestampSof + (DT_MDL * 1e9))
-      cam_meta = meta_from_camera_state(cam)
-
-      self.pm.send(msg.which(), msg)
-      self.vipc_server.send(cam_meta.stream, IMG_BYTES, cs.frameId,
-                            cs.timestampSof, cs.timestampEof)
-    return cs
-
-  def _wait(self):
-    self.sm.update(5000)
-    if self.sm['modelV2'].frameId != self.sm['cameraOdometry'].frameId:
-      self.sm.update(1000)
-
-  def test_modeld(self):
-    for n in range(1, 500):
-      cs = self._send_frames(n)
-      self._wait()
-
-      mdl = self.sm['modelV2']
-      assert mdl.frameId == n
-      assert mdl.frameIdExtra == n
-      assert mdl.timestampEof == cs.timestampEof
-      assert mdl.frameAge == 0
-      assert mdl.frameDropPerc == 0
-
-      odo = self.sm['cameraOdometry']
-      assert odo.frameId == n
-      assert odo.timestampEof == cs.timestampEof
-
-  def test_dropped_frames(self):
-    """
-      modeld should only run on consecutive road frames
-    """
-    frame_id = -1
-    road_frames = list()
-    for n in range(1, 50):
-      if (random.random() < 0.1) and n > 3:
-        cams = random.choice([(), ('wideRoadCameraState', )])
-        self._send_frames(n, cams)
-      else:
-        self._send_frames(n)
-        road_frames.append(n)
-      self._wait()
-
-      if len(road_frames) < 3 or road_frames[-1] - road_frames[-2] == 1:
-        frame_id = road_frames[-1]
-
-      mdl = self.sm['modelV2']
-      odo = self.sm['cameraOdometry']
-      assert mdl.frameId == frame_id
-      assert mdl.frameIdExtra == frame_id
-      assert odo.frameId == frame_id
-      if n != frame_id:
-        assert not self.sm.updated['modelV2']
-        assert not self.sm.updated['cameraOdometry']

sunnypilot/modeld_v2/tests/test_recovery_power.py

@@ -0,0 +1,61 @@
+import numpy as np
+
+from cereal import log
+
+from openpilot.sunnypilot.modeld_v2.constants import Plan
+from openpilot.sunnypilot.modeld_v2.modeld import ModelState
+import openpilot.sunnypilot.modeld_v2.modeld as modeld
+
+
+class MockStruct:
+  def __init__(self, **kwargs):
+    for k, v in kwargs.items():
+      setattr(self, k, v)
+
+
+def test_recovery_power_scaling():
+  state = MockStruct(
+    PLANPLUS_CONTROL=1.0,
+    LONG_SMOOTH_SECONDS=0.3,
+    LAT_SMOOTH_SECONDS=0.1,
+    MIN_LAT_CONTROL_SPEED=0.3,
+    mlsim=True,
+    generation=12,
+    constants=MockStruct(T_IDXS=np.arange(100), DESIRE_LEN=8)
+  )
+  prev_action = log.ModelDataV2.Action()
+  recorded_vel: list = []
+
+  def mock_accel(plan_vel, plan_accel, t_idxs, action_t=0.0):
+    recorded_vel.append(plan_vel.copy())
+    return 0.0, False
+
+  modeld.get_accel_from_plan = mock_accel
+  modeld.get_curvature_from_output = lambda *args: 0.0
+  plan = np.random.rand(1, 100, 15).astype(np.float32)
+  planplus = np.random.rand(1, 100, 15).astype(np.float32)
+
+  model_output: dict = {
+    'plan': plan.copy(),
+    'planplus': planplus.copy()
+  }
+
+  test_cases: list = [
+    # (control, v_ego, expected_factor)
+    (0.55, 20.0, 1.0),
+    (1.0, 25.0, .75),
+    (1.5, 25.1, 0.75),
+    (2.0, 20.0, 1.0),
+    (0.75, 19.0, 1.0),
+    (0.8, 25.1, 0.75),
+  ]
+
+  for control, v_ego, factor in test_cases:
+    state.PLANPLUS_CONTROL = control
+    recorded_vel.clear()
+    ModelState.get_action_from_model(state, model_output, prev_action, 0.0, 0.0, v_ego)
+
+    expected_recovery_power = control * factor
+    expected_plan_vel = plan[0, :, Plan.VELOCITY][:, 0] + expected_recovery_power * planplus[0, :, Plan.VELOCITY][:, 0]
+
+    np.testing.assert_allclose(recorded_vel[0], expected_plan_vel, rtol=1e-5, atol=1e-6)

sunnypilot/models/runners/helpers.py

@@ -2,25 +2,17 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle
 from openpilot.sunnypilot.models.runners.model_runner import ModelRunner
 from openpilot.sunnypilot.models.runners.tinygrad.tinygrad_runner import TinygradRunner, TinygradSplitRunner
 from openpilot.sunnypilot.models.runners.constants import ModelType
-from openpilot.system.hardware import TICI
 
-if not TICI:
-  from openpilot.sunnypilot.models.runners.onnx.onnx_runner import ONNXRunner
 
 def get_model_runner() -> ModelRunner:
   """
   Factory function to create and return the appropriate ModelRunner instance.
 
-  Selects between ONNXRunner (for non-TICI platforms) and TinygradRunner
-  (for TICI platforms), choosing TinygradSplitRunner if separate vision/policy
+  Selects TinygradRunner, choosing TinygradSplitRunner if separate vision/policy
   models are detected in the active bundle.
 
   :return: An instance of a ModelRunner subclass (ONNXRunner, TinygradRunner, or TinygradSplitRunner).
   """
-  if not TICI:
-    return ONNXRunner()
-
-  # On TICI platforms, use Tinygrad runners
   bundle = get_active_bundle()
   if bundle and bundle.models:
     model_types = {m.type.raw for m in bundle.models}

sunnypilot/sunnylink/athena/sunnylinkd.py

@@ -62,7 +62,7 @@ def handle_long_poll(ws: WebSocket, exit_event: threading.Event | None) -> None:
               threading.Thread(target=ws_ping, args=(ws, end_event), name='ws_ping'),
               threading.Thread(target=ws_queue, args=(end_event,), name='ws_queue'),
               threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler'),
-              # threading.Thread(target=sunny_log_handler, args=(end_event, comma_prime_cellular_end_event), name='log_handler'),
+              threading.Thread(target=sunny_log_handler, args=(end_event, comma_prime_cellular_end_event), name='log_handler'),
               threading.Thread(target=stat_handler, args=(end_event, Paths.stats_sp_root(), True), name='stat_handler'),
             ] + [
               threading.Thread(target=jsonrpc_handler, args=(end_event, partial(startLocalProxy, end_event),), name=f'worker_{x}')

sunnypilot/sunnylink/params_metadata.json

@@ -826,6 +826,13 @@
     "title": "Panda Som Reset Triggered",
     "description": ""
   },
+  "PlanplusControl": {
+    "title": "Plan Plus Controls",
+    "description": "Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover to lanecenter, too high and it will ping-pong",
+    "min": 0.0,
+    "max": 2.0,
+    "step": 0.1
+  },
   "PrimeType": {
     "title": "Prime Type",
     "description": ""