ci: lfs pointers survive gc

these models will fail, look at workflow, likely need env setup now that we use recursion

.github/workflows/lfs-maintenance.yaml

@@ -33,7 +33,7 @@ jobs:
 
       - name: LFS Fetch
         run: |
-          git lfs fetch || echo "::warning::some objects are missing on commaai/openpilot-lfs; mirroring the rest"
+          git lfs fetch
 
       - name: Set up Git
         run: |
@@ -68,7 +68,6 @@ jobs:
           git config lfs.activitytimeout 300
           git config lfs.transfer.maxretries 5
           git config lfs.concurrenttransfers 4
-          git config lfs.allowincompletepush true
 
       - name: Push LFS
         id: sync-and-commit
@@ -109,19 +108,19 @@ jobs:
           p = subprocess.Popen(["git", "-C", repo, "cat-file", "--batch=%(objectsize)"],
                                stdin=subprocess.PIPE, stdout=subprocess.PIPE)
           def feed():
-            for sha in small:
-              p.stdin.write(sha.encode() + b"\n")
-            p.stdin.close()
+              for sha in small:
+                  p.stdin.write(sha.encode() + b"\n")
+              p.stdin.close()
           threading.Thread(target=feed, daemon=True).start()
           rx = re.compile(rb"^version https://git-lfs\.github\.com/spec/v1\noid sha256:([0-9a-f]{64})\nsize ([0-9]+)\n")
           seen = {}
           for _ in small:
-            n = int(p.stdout.readline()); body = p.stdout.read(n); p.stdout.read(1)
-            m = rx.match(body)
-            if m:
-              seen.setdefault(m.group(1).decode(), m.group(2).decode())
+              n = int(p.stdout.readline()); body = p.stdout.read(n); p.stdout.read(1)
+              m = rx.match(body)
+              if m:
+                  seen.setdefault(m.group(1).decode(), m.group(2).decode())
           for oid, size in sorted(seen.items()):
-            print(oid, size)
+              print(oid, size)
           EOF
           for repo in . sunnypilot; do
             python3 scan_pointers.py "$repo"

openpilot/cereal/custom.capnp

@@ -465,11 +465,6 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
   laneTurnDirection @0 :TurnDirection;
   leftLaneChangeEdgeBlock @1 :Bool;
   rightLaneChangeEdgeBlock @2 :Bool;
-  lanefulCurvature @3 :Float32;
-  lanefulTrust @4 :Float32;
-  lanefulLaneWidth @5 :Float32;
-  lanefulLeftProb @6 :Float32;
-  lanefulRightProb @7 :Float32;
 
   enum TurnDirection {
     none @0;

openpilot/common/params_keys.h

@@ -256,7 +256,6 @@ 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"}},
-    {"Laneful", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
 
     // mapd

openpilot/selfdrive/modeld/modeld.py

@@ -9,6 +9,7 @@ os.environ['GMMU'] = '0' # for chestnut fast loading, noop for qcom
 os.environ.setdefault('AM_POWER_LIMIT', '100')
 os.environ.setdefault('AM_PERF_LEVEL', 'high')
 from tinygrad.device import Buffer, Device
+from tinygrad.engine.realize import lower_and_compile
 from tinygrad.dtype import DType, dtypes
 from tinygrad.engine.realize import lower_and_compile
 from tinygrad.tensor import Tensor

openpilot/selfdrive/navigation/helpers.py

@@ -1,4 +1,7 @@
 import time
+from typing import Any
+
+import numpy as np
 
 
 def latest_gps_source(sm) -> str | None:
@@ -7,3 +10,34 @@ def latest_gps_source(sm) -> str | None:
   sources = [s for s in ('gpsLocation', 'gpsLocationExternal')
              if sm.valid[s] and sm[s].hasFix and 0 <= now - sm.recv_time[s] < 2]
   return max(sources, key=lambda s: sm.logMonoTime[s]) if sources else None
+
+
+# convert mapbox steps to valhalla enums the model was trained on
+VALHALLA_COMMAND_COUNT = 46
+VALHALLA_SIDED_TYPES = {  # (straight, right, left)
+  'depart': (1, 2, 3), 'arrive': (4, 5, 6),
+  'on ramp': (17, 18, 19), 'off ramp': (20, 20, 21), 'fork': (22, 23, 24),
+  'roundabout': (26, 26, 26), 'rotary': (26, 26, 26), 'roundabout turn': (26, 26, 26),
+  'exit roundabout': (27, 27, 27), 'exit rotary': (27, 27, 27), 'notification': (28, 28, 28),
+}
+VALHALLA_TURN_TYPES = {'straight': 8, 'slight right': 9, 'right': 10, 'sharp right': 11, 'uturn': 13,
+                       'sharp left': 14, 'left': 15, 'slight left': 16}  # turn, continue, new name, end of road
+
+
+def valhalla_type(maneuver_type: str, modifier: str) -> int | None:
+  if maneuver_type == 'merge':
+    return None  # Valhalla gives no maneuver at a merge, so the model never saw one
+  if maneuver_type in VALHALLA_SIDED_TYPES:
+    straight, right, left = VALHALLA_SIDED_TYPES[maneuver_type]
+    return right if 'right' in modifier else left if 'left' in modifier else straight
+  return VALHALLA_TURN_TYPES.get(modifier, 8)
+
+
+def navigation_input(maneuvers: Any, traveled: float) -> np.ndarray:
+  navigation = np.zeros(VALHALLA_COMMAND_COUNT + 1, dtype=np.float32)
+  for maneuver in maneuvers:
+    command, distance = valhalla_type(maneuver.type, maneuver.modifier), maneuver.distance - traveled
+    if command is not None and command > 6 and distance >= 0:
+      navigation[command], navigation[-1] = 1, distance / 100
+      break
+  return navigation

openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py

@@ -206,8 +206,6 @@ class AugmentedRoadView(CameraView):
   def _handle_mouse_release(self, mouse_pos: MousePos):
     # Don't trigger click callback if bookmark was triggered
     if not self._bookmark_icon.interacting():
-      if self._model_renderer.laneful.handle_tap(mouse_pos):
-        return
       super()._handle_mouse_release(mouse_pos)
 
   def _render(self, _):

openpilot/selfdrive/ui/mici/onroad/model_renderer.py

@@ -141,7 +141,6 @@ class ModelRenderer(Widget, ModelRendererSP):
     if ui_state.status != UIStatus.DISENGAGED:
       self._draw_lane_lines()
       self._draw_path()
-      self.laneful.draw(self._rect)
 
     if render_lead_indicator:
       self._update_leads(sm)
@@ -220,7 +219,6 @@ class ModelRenderer(Widget, ModelRendererSP):
     # Update road edges using raw points
     for road_edge in self._road_edges:
       road_edge.projected_points = self._map_line_to_polygon(road_edge.raw_points, line_width_factor, 0.0, max_idx)
-    self.laneful.update(self, max_idx)
 
     # Update path using raw points
     if lead and lead.present:

openpilot/selfdrive/ui/sunnypilot/mici/layouts/laneful.py

@@ -1,21 +0,0 @@
-"""
-Copyright (c) 2021-, 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.
-"""
-from openpilot.selfdrive.ui.mici.widgets.button import BigParamControl
-from openpilot.system.ui.widgets.scroller import NavScroller
-
-
-class LanefulLayoutMici(NavScroller):
-  def __init__(self):
-    super().__init__()
-    self._toggle = BigParamControl("laneful", "Laneful",
-                                   description="Keeps the car centered in its lane.\n" +
-                                               "Leans on the lane lines when they are clear, drives laneless when they are not.")
-    self._scroller.add_widget(self._toggle)
-
-  def show_event(self):
-    super().show_event()
-    self._toggle.refresh()

openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py

@@ -11,7 +11,6 @@ from openpilot.selfdrive.ui.mici.widgets.button import BigCircleButton
 from openpilot.selfdrive.ui.mici.widgets.dialog import BigConfirmationDialog, BigDialog
 from openpilot.selfdrive.ui.sunnypilot.mici.layouts.sunnylink import SunnylinkLayoutMici
 from openpilot.selfdrive.ui.sunnypilot.mici.layouts.models import ModelsLayoutMici
-from openpilot.selfdrive.ui.sunnypilot.mici.layouts.laneful import LanefulLayoutMici
 from openpilot.selfdrive.ui.ui_state import ui_state
 from openpilot.system.ui.lib.application import gui_app, FontWeight
 from openpilot.system.ui.lib.multilang import tr
@@ -51,10 +50,6 @@ class SettingsLayoutSP(OP.SettingsLayout):
     models_btn = SettingsBigButton(tr("models"), "", gui_app.texture("../../sunnypilot/selfdrive/assets/offroad/icon_models.png", ICON_SIZE, ICON_SIZE))
     models_btn.set_click_callback(lambda: gui_app.push_widget(models_panel))
 
-    laneful_panel = LanefulLayoutMici()
-    laneful_btn = SettingsBigButton(tr("laneful"), "", gui_app.texture("icons_mici/settings/device/lkas.png", ICON_SIZE, ICON_SIZE))
-    laneful_btn.set_click_callback(lambda: gui_app.push_widget(laneful_panel))
-
     # onroad: enable button sits at the front (left of toggles)
     self._enable_offroad_btn_onroad = BigCircleButton(self.icon_offroad_enable, red=True)
     self._enable_offroad_btn_onroad.set_click_callback(lambda: self._handle_always_offroad(True))
@@ -72,8 +67,7 @@ class SettingsLayoutSP(OP.SettingsLayout):
     items = self._scroller._items.copy()
 
     items.insert(1, models_btn)
-    items.insert(2, laneful_btn)
-    items.insert(6, sunnylink_btn)
+    items.insert(5, sunnylink_btn)
 
     # front slots (only one ever visible at a time): exit-always-offroad, then enable-onroad
     items.insert(0, self._enable_offroad_btn_onroad)

openpilot/selfdrive/ui/sunnypilot/mici/onroad/laneful.py

@@ -1,120 +0,0 @@
-"""
-Copyright (c) 2021-, 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.
-
-Laneful visuals: faint cyan lane-center target, thin white line down the middle of the model path,
-and a top-center readout of the pull, from the curvature, trust, line probs and lane width modeld publishes in modelDataV2SP.
-"""
-import numpy as np
-import pyray as rl
-from openpilot.common.filter_simple import FirstOrderFilter
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.ui.ui_state import ui_state
-from openpilot.sunnypilot.modeld_v2.laneful import LOOKAHEAD_TIME, MIN_LOOKAHEAD, MAX_ERROR
-from openpilot.system.ui.lib.application import gui_app, FontWeight
-from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient
-from openpilot.system.ui.lib.text_measure import measure_text_cached
-
-CENTER_COLOR = (0, 200, 255)
-PATH_COLOR = (255, 255, 255)
-
-
-
-class LanefulRenderer:
-  def __init__(self):
-    self._center_points = np.empty((0, 2), dtype=np.float32)
-    self._path_points = np.empty((0, 2), dtype=np.float32)
-    self._font = gui_app.font(FontWeight.BOLD)
-    # smoothed copies for the readout so the numbers don't flicker
-    self._pull_display = FirstOrderFilter(0.0, 0.5, DT_MDL)
-    self._offset_display = FirstOrderFilter(0.0, 0.5, DT_MDL)
-    self._target_frac = 0.0
-    self._alpha = FirstOrderFilter(0.0, 0.3, DT_MDL)
-    self._readout_rect: rl.Rectangle | None = None
-    self._settings_page = None
-
-  def update(self, renderer, max_idx: int) -> None:
-    """Called from the model renderer's update with its lane lines and path already in raw points."""
-    laneful = ui_state.sm['modelDataV2SP']
-    laneful_curvature = laneful.lanefulCurvature
-    self._alpha.update(float(laneful.lanefulTrust > 0.0))
-
-    left, right = renderer._lane_lines[1].raw_points, renderer._lane_lines[2].raw_points
-    stds = list(ui_state.sm['modelV2'].laneLineStds)
-    if self._alpha.x < 0.01 or len(left) == 0 or len(right) != len(left) or len(stds) < 3:
-      self._center_points = np.empty((0, 2), dtype=np.float32)
-      self._path_points = np.empty((0, 2), dtype=np.float32)
-      return
-
-    lookahead = max(ui_state.sm['carState'].vEgo * LOOKAHEAD_TIME, MIN_LOOKAHEAD)
-    l_var = stds[1] ** 2 / max(laneful.lanefulLeftProb, 1e-4)
-    r_var = stds[2] ** 2 / max(laneful.lanefulRightProb, 1e-4)
-    half_width = laneful.lanefulLaneWidth / 2
-    center_y = (r_var * (left[:, 1] + half_width) + l_var * (right[:, 1] - half_width)) / (l_var + r_var)
-
-    # target and middle of the model path share the path's points, so both end at the same distance
-    path = renderer._path.raw_points
-    center = path.copy()
-    center[:, 1] = np.interp(path[:, 0], left[:, 0], center_y)
-    end_idx = renderer._get_path_length_idx(path[:, 0], left[max_idx, 0])
-    self._center_points = renderer._map_line_to_polygon(center, 0.15, renderer._path_offset_z, end_idx)
-    self._path_points = renderer._map_line_to_polygon(path, 0.15, renderer._path_offset_z, end_idx)
-    self._pull_display.update(laneful_curvature * lookahead ** 2 / 2)  # how far laneful moves the car at the lookahead
-    if laneful.lanefulTrust > 0.0:
-      self._target_frac = self._offset_display.update(float(np.clip(np.interp(lookahead, path[:, 0], center[:, 1] - path[:, 1]) / MAX_ERROR, -1.0, 1.0)))
-
-  def _color(self, rgb, alpha: float) -> rl.Color:
-    return rl.Color(*rgb, int(alpha * self._alpha.x))
-
-  def _fade(self, rgb, alpha: float) -> Gradient:
-    return Gradient(start=(0.0, 1.0), end=(0.0, 0.0), colors=[self._color(rgb, alpha), self._color(rgb, alpha), self._color(rgb, 0)], stops=[0.0, 0.6, 1.0])
-
-  def draw(self, rect: rl.Rectangle) -> None:
-    if self._path_points.size:
-      draw_polygon(rect, self._center_points, gradient=self._fade(CENTER_COLOR, 255))
-      draw_polygon(rect, self._path_points, gradient=self._fade(PATH_COLOR, 255))
-    self._readout_rect = None
-    if self._path_points.size:
-      self._draw_readout(rect)
-
-  def handle_tap(self, pos) -> bool:
-    """Tapping the readout opens the laneful settings page. Returns True if the tap was consumed."""
-    r = self._readout_rect
-    if r is None or not (r.x - 20 <= pos.x <= r.x + r.width + 20 and r.y - 20 <= pos.y <= r.y + r.height + 20):
-      return False
-    if self._settings_page is None:
-      from openpilot.selfdrive.ui.sunnypilot.mici.layouts.laneful import LanefulLayoutMici
-      self._settings_page = LanefulLayoutMici()
-    gui_app.push_widget(self._settings_page)
-    return True
-
-  def _draw_readout(self, rect: rl.Rectangle) -> None:
-    """Top-center pill: pull toward lane center in cm at the lookahead, arrow = side,
-    bar grows from center toward the lane-center target, full at laneful's 1 m cap."""
-    pull = self._pull_display.x
-    w, h = 190, 76
-    x, y = rect.x + (rect.width - w) / 2, rect.y + 10
-    cx, cy = x + w / 2, y + 30
-    self._readout_rect = rl.Rectangle(x, y, w, h)
-    rl.draw_rectangle_rounded(self._readout_rect, 0.5, 12, self._color((0, 0, 0), 170))
-
-    text = f"{abs(pull) * 100:.0f}"
-    text_w = measure_text_cached(self._font, text, 44, 0).x
-    rl.draw_text_ex(self._font, text, rl.Vector2(cx - text_w / 2, cy - 24), 44, 0, self._color((255, 255, 255), 255))
-
-    active = self._color(CENTER_COLOR, np.clip(abs(pull) / 0.1, 0.0, 1.0) * 255)
-    idle = self._color((255, 255, 255), 40)
-    lx, rx = x + 18, x + w - 18
-    rl.draw_triangle(rl.Vector2(lx, cy), rl.Vector2(lx + 16, cy + 12), rl.Vector2(lx + 16, cy - 12), active if pull < 0 else idle)
-    rl.draw_triangle(rl.Vector2(rx - 16, cy - 12), rl.Vector2(rx - 16, cy + 12), rl.Vector2(rx, cy), active if pull > 0 else idle)
-
-    track = rl.Rectangle(x + 24, y + h - 16, w - 48, 6)
-    rl.draw_rectangle_rounded(track, 1.0, 6, self._color((255, 255, 255), 40))
-    rl.draw_rectangle(int(cx - 1), int(track.y - 2), 2, int(track.height + 4), self._color((255, 255, 255), 120))
-    fill = track.width / 2 * abs(self._target_frac)
-    if fill > 1:
-      fill_x = cx if self._target_frac > 0 else cx - fill
-      rl.draw_rectangle_rounded(rl.Rectangle(fill_x, track.y, fill, track.height), 1.0, 6, self._color(CENTER_COLOR, 255))
-

openpilot/selfdrive/ui/sunnypilot/mici/onroad/model_renderer.py

@@ -6,7 +6,6 @@ See the LICENSE.md file in the root directory for more details.
 """
 import pyray as rl
 from openpilot.selfdrive.ui.ui_state import UIStatus
-from openpilot.selfdrive.ui.sunnypilot.mici.onroad.laneful import LanefulRenderer
 from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath
 
 LANE_LINE_COLORS_SP = {
@@ -18,4 +17,3 @@ LANE_LINE_COLORS_SP = {
 class ModelRendererSP:
   def __init__(self):
     self.rainbow_path = RainbowPath()
-    self.laneful = LanefulRenderer()

openpilot/selfdrive/ui/sunnypilot/ui_state.py

@@ -36,7 +36,7 @@ class UIStateSP:
     self.is_sp_release: bool = self.params.get_bool("IsReleaseSpBranch")
     self.sm_services_ext = [
       "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
-      "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay", "modelDataV2SP"
+      "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay"
     ]
 
     self.sunnylink_state = SunnylinkState()

openpilot/sunnypilot/modeld_v2/frame_fusion.py

@@ -9,14 +9,16 @@ import itertools
 import math
 from functools import partial
 
+import numpy as np
+
 from openpilot.common.basedir import BASEDIR
 from openpilot.sunnypilot.modeld_v2.helpers import _dtype, read_parts
 
 
-def pack_layout(shapes: dict[str, tuple], frame_size: int) -> tuple[dict[str, int], int]:
+def pack_layout(specs: dict[str, tuple], frame_size: int) -> tuple[dict[str, int], int]:
   offsets, offset = {}, 0
-  for name, shape in ({"tfm": (2, 3, 3)} | shapes).items():
-    offsets[name], offset = offset, offset + -(-math.prod(shape) * 4 // 128) * 128
+  for name, (shape, dtype) in ({"tfm": ((2, 3, 3), np.float32)} | specs).items():
+    offsets[name], offset = offset, offset + -(-math.prod(shape) * np.dtype(dtype).itemsize // 128) * 128
   return offsets | {"frames": offset}, offset + 2 * frame_size
 
 
@@ -38,7 +40,7 @@ def frame_part(warp_path: str, parts: dict, extras: dict) -> dict:
     return {}
   (model, model_blob), (warp, warp_blob) = parts["run"], read_parts(warp_path)[0]["run"]
   states = sorted(name for name in extras["input_specs"] if f"next_{name}" in extras["metadata"]["output_shapes"])
-  host = {name: shape for name, (shape, _, _) in extras["input_specs"].items() if name not in states and name != "new_img"}
+  host = {name: (shape, dtype) for name, (shape, dtype, _) in extras["input_specs"].items() if name not in states and name != "new_img"}
   frame_size = next(s["numel"] for s in warp["inputs"] if s["name"] == "input_frame") // 2
   offsets, packed = pack_layout(host, frame_size)
   shift, upload, u8 = len(model_blob), len(model["buffers"]) + len(warp["buffers"]), repr(_dtype("dtypes.uint8"))

openpilot/sunnypilot/modeld_v2/laneful.py

@@ -1,121 +0,0 @@
-"""
-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 numpy as np
-
-from openpilot.common.filter_simple import FirstOrderFilter
-from openpilot.common.realtime import DT_MDL
-from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
-
-LANE_WIDTH_DEFAULT = 3.6  # m (reseeded to model's lane width upon initialization)
-LANE_WIDTH_CLIP = (2.8, 4.5)  # m
-LOOKAHEAD_TIME = 1.5  # s
-MIN_LOOKAHEAD = 10.0  # m
-MIN_SPEED = 5.0  # m/s
-LINE_STD = (0.2, 0.6)
-POINT_STD = 0.1
-MAX_ERROR = 1.0
-YIELD = (0.2, 0.45)
-X_IDXS = np.array(ModelConstants.X_IDXS)
-
-
-class Laneful: # TODO-SP: Rename this to blendful or something similar because it is not pure laneful anymore.
-  def __init__(self):
-    self.lane_width = FirstOrderFilter(LANE_WIDTH_DEFAULT, 10.0, DT_MDL, initialized=False)
-    self.line_gone = FirstOrderFilter(np.zeros(2), 2.0, DT_MDL)
-    self.line_probs_slow = FirstOrderFilter(np.zeros(2), 3.0, DT_MDL)
-    self.line_probs = FirstOrderFilter(np.zeros(2), 0.5, DT_MDL)
-    self.usual_offset = FirstOrderFilter(0.0, 10.0, DT_MDL, initialized=False)
-    self.going_around = False
-    self.trust = 0.0
-    self.curvature = 0.0
-    self.l_prob = 0.0
-    self.r_prob = 0.0
-
-  def update(self, model_output: dict[str, np.ndarray], v_ego: float, active: bool, model_curvature: float) -> float:
-    self.l_prob = self.r_prob = 0.0
-    fading = not active or v_ego < MIN_SPEED
-    if fading:
-      self.trust = 0.0
-      self.usual_offset.initialized = False
-      self.lane_width.initialized = False
-      self.going_around = False
-    target = 0.0 if fading else self._target(model_output, v_ego, model_curvature)
-    releasing = not fading and (abs(target) < abs(self.curvature) or target * self.curvature < 0)
-    step = (2.0 if releasing else 0.5) / max(v_ego, 1.0) ** 2 * DT_MDL  # jerk over 1 second
-    self.curvature = float(np.clip(target, self.curvature - step, self.curvature + step))
-    return self.curvature
-
-  def _target(self, model_output: dict[str, np.ndarray], v_ego: float, model_curvature: float) -> float:
-    if v_ego < MIN_SPEED or 'lane_lines' not in model_output:
-      return 0.0
-
-    lookahead = max(v_ego * LOOKAHEAD_TIME, MIN_LOOKAHEAD)
-    lines = model_output['lane_lines'][0, :, :, 0]
-    stds = model_output['lane_lines_stds'][0, :, :, 0]
-
-    probs = self.line_probs.update(model_output['lane_lines_prob'][0, 1::2][1:3])
-    gone = self.line_gone.update((model_output['lane_lines_prob'][0, 1::2][1:3] < 0.3).astype(float))
-    dropping = bool(np.any(probs < 0.6 * self.line_probs_slow.update(model_output['lane_lines_prob'][0, 1::2][1:3])))
-    l_prob = float(probs[0] * np.interp(np.interp(0.5 * lookahead, X_IDXS, stds[1]), LINE_STD, [1.0, 0.0]))
-    r_prob = float(probs[1] * np.interp(np.interp(0.5 * lookahead, X_IDXS, stds[2]), LINE_STD, [1.0, 0.0]))
-    # one line steadily on its own still centers at half say conf. a line that is disappearing gives control back to laneless
-    if gone[1] > 0.8 and l_prob > 0.5:
-      lane_trust = l_prob / 2
-    elif gone[0] > 0.8 and r_prob > 0.5:
-      lane_trust = r_prob / 2
-    elif dropping:
-      lane_trust = min(l_prob, r_prob)
-    else:
-      lane_trust = l_prob + r_prob - l_prob * r_prob
-    self.l_prob, self.r_prob = l_prob, r_prob
-    if lane_trust <= 0.0:
-      self.trust = 0.0
-      return 0.0
-
-    win = (X_IDXS >= 0.5 * lookahead) & (X_IDXS <= 1.5 * lookahead)
-    x = X_IDXS[win]
-    half_width = self.lane_width.x / 2
-    if l_prob * r_prob > 0.5:
-      width = np.interp(lookahead, X_IDXS, lines[2]) - np.interp(lookahead, X_IDXS, lines[1])
-      self.lane_width.update(float(np.clip(width, *LANE_WIDTH_CLIP)))
-      half_width = np.clip(lines[2, win] - lines[1, win], *LANE_WIDTH_CLIP) / 2
-    l_var = stds[1, win] ** 2 / max(l_prob, 1e-4)
-    r_var = stds[2, win] ** 2 / max(r_prob, 1e-4)
-    center = (r_var * (lines[1, win] + half_width) + l_var * (lines[2, win] - half_width)) / (l_var + r_var)
-    weight = 1.0 / (l_var * r_var / (l_var + r_var) + POINT_STD ** 2)
-
-    # Fit curvature to place the plant in center: e ~ dk x^2 / 2
-    plan_x = np.interp(x, model_output['plan'][0, :, 0], model_output['plan'][0, :, 1])
-    plan_share = np.interp(abs(model_curvature) * lookahead ** 2 / 2, [0.5, 2.0], [0.3, 0.5]) # blending up to 50% plan with action during sharp curvature
-    error = np.clip(center - ((1 - plan_share) * model_curvature * x ** 2 / 2 + plan_share * plan_x), -1.0, 1.0)
-    dk = 2.0 * np.sum(weight * error * x ** 2) / np.sum(weight * x ** 4)
-
-    # A model planning to end up further from its usual spot in the lane than the plant is now is likely going around something
-    hw_now = np.clip(lines[2, 0] - lines[1, 0], *LANE_WIDTH_CLIP) / 2 if l_prob * r_prob > 0.5 else self.lane_width.x / 2
-    center_now = (l_prob * (lines[1, 0] + hw_now) + r_prob * (lines[2, 0] - hw_now)) / (l_prob + r_prob)
-    plan_y = np.interp(lookahead, model_output['plan'][0, :, 0], model_output['plan'][0, :, 1])
-    offset = plan_y - np.interp(lookahead, x, center)
-    usual = self.usual_offset.x if self.usual_offset.initialized else offset
-    swerve = abs(offset - usual) - 0.25 * abs(model_curvature) * lookahead ** 2 / 2
-    away = swerve - abs(-center_now - usual)
-    lane_curvature = 2.0 * np.polyfit(x, center, 2)[0]
-    inside_curve = abs(lane_curvature) * lookahead ** 2 / 2 > 0.1 and (offset - usual) * lane_curvature > 0
-    if inside_curve:
-      self.going_around = False
-    elif away > YIELD[0]:
-      self.going_around = True
-    elif swerve < YIELD[0] / 2:
-      self.going_around = False
-    self.usual_offset.update_alpha(20.0 if self.going_around else 10.0)
-    self.usual_offset.update(offset)
-    if inside_curve:
-      lane_trust *= np.interp(abs(offset) - abs(center_now) - 0.25 * abs(model_curvature) * lookahead ** 2 / 2, (0.5, 1.0), [1.0, 0.0])
-    else:
-      lane_trust *= np.interp(swerve, (YIELD[0] / 2, YIELD[0]), [1.0, 0.0]) if self.going_around else np.interp(away, YIELD, [1.0, 0.0])
-
-    self.trust = lane_trust if lane_trust < self.trust else self.trust + (lane_trust - self.trust) * DT_MDL / (1.5 + DT_MDL)
-    return float(self.trust * dk)

openpilot/sunnypilot/modeld_v2/model_adapters.py

@@ -172,12 +172,12 @@ class NativeTinygradAdapter(BaseModelAdapter):
 
     self.input_queues = {name: Tensor(np.zeros(shape, dtype=dtype), device=self.model_device).realize()
                          for name, (shape, dtype) in self.input_shapes.items() if name in self.state_pairs}
-    shapes = {name: shape for name, (shape, _) in self.input_shapes.items() if name not in self.state_pairs and name != 'new_img'}
-    offsets, packed = pack_layout(shapes, self.warp_frame_size)
+    specs = {name: spec for name, spec in self.input_shapes.items() if name not in self.state_pairs and name != 'new_img'}
+    offsets, packed = pack_layout(specs, self.warp_frame_size)
     self.packed_input = np.zeros(packed, dtype=np.uint8)
     self.host = Tensor(self.packed_input, device='NPY')
-    self.numpy_inputs = {name: np.ndarray(shape, dtype=np.float32, buffer=self.packed_input, offset=offsets[name])
-                         for name, shape in ({'tfm': (2, 3, 3)} | shapes).items()}
+    self.numpy_inputs = {name: np.ndarray(shape, dtype=dtype, buffer=self.packed_input, offset=offsets[name])
+                         for name, (shape, dtype) in ({'tfm': ((2, 3, 3), np.dtype(np.float32))} | specs).items()}
     self.frames = self.packed_input[offsets['frames']:].reshape(2, self.warp_frame_size)
 
     self.outputs = {name: Tensor(np.zeros(shape, dtype=dtype), device=device).realize() for name, (shape, dtype, device) in self.jits['output_specs'].items()}

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -7,7 +7,6 @@ See the LICENSE.md file in the root directory for more details.
 """
 
 from collections.abc import Callable
-from functools import partial
 import gc
 import os
 os.environ['GMMU'] = '0'
@@ -36,11 +35,11 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.common.transformations.model import get_warp_matrix
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value
+from openpilot.selfdrive.navigation.helpers import navigation_input
 from openpilot.selfdrive.modeld.modeld import ChestnutGpuState
 
 from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output
 from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser
-from openpilot.sunnypilot.modeld_v2.laneful import Laneful
 from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
 from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
 from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
@@ -174,6 +173,9 @@ class ModelState(ModelStateBase):
     for key in ('traffic_convention', 'lateral_control_params', 'action_t'):
       if key in self.numpy_inputs and key in inputs:
         self.numpy_inputs[key][:] = inputs[key]
+    for name in ('nav_map', 'navigation'):
+      if name in self.numpy_inputs:
+        self.numpy_inputs[name][:] = inputs.get(name, 0)
 
     if self.adapter.is_native:
       for i, key in enumerate(self._vision_input_names):
@@ -230,8 +232,7 @@ class ModelState(ModelStateBase):
     return outputs
 
   def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
-                            lat_action_t: float, long_action_t: float, v_ego: float,
-                            laneful: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
+                            lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action:
     if 'action' not in model_output:
       plan = model_output['plan'][0]
       desired_accel = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
@@ -243,8 +244,6 @@ class ModelState(ModelStateBase):
     else:
       desired_accel = model_output['action'][0, 1]
       desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2
-    if laneful is not None:
-      desired_curvature += laneful(desired_curvature)
 
     stop = v_ego < 0.3 and desired_accel < 0.1
     desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
@@ -258,6 +257,10 @@ class ModelState(ModelStateBase):
     return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature), desiredAcceleration=float(desired_accel), shouldStop=bool(stop))
 
 
+def takes_nav(*models) -> bool:
+  return any(name in m.numpy_inputs for m in models if m is not None for name in ('nav_map', 'navigation'))
+
+
 def main(demo=False):
   cloudlog.warning("modeld init")
 
@@ -287,6 +290,7 @@ def main(demo=False):
   vipc_client_main_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if main_wide_camera else VisionStreamType.VISION_STREAM_NARROW_ROAD
   vipc_client_main = VisionIpcClient("camerad", vipc_client_main_stream, True)
   vipc_client_extra = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False)
+  vipc_client_map = VisionIpcClient("mapsd", VisionStreamType.VISION_STREAM_MAP, True)
   cloudlog.warning(f"vision stream set up, main_wide_camera: {main_wide_camera}, use_extra_client: {use_extra_client}")
 
   while not vipc_client_main.connect(False):
@@ -337,8 +341,9 @@ def main(demo=False):
   # messaging
   pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutGpuState"] if CHESTNUT else [])
   pm = PubMaster(pub_socks)
+  nav = takes_nav(model, small_model)
   sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState",
-                  "carControl", "controlsState", "lateralDelay"])
+                  "carControl", "controlsState", "lateralDelay"] + (["navInstruction"] if nav else []))
 
   publish_state = PublishState()
   chestnut_state = ChestnutGpuState(pm, model.chestnut) if CHESTNUT else None
@@ -371,8 +376,7 @@ def main(demo=False):
   DH = DesireHelper()
   meta_constants = load_meta_constants()
   RELC = RoadEdgeLaneChangeController()
-  laneful = Laneful()
-  laneful_enabled = False
+  nav_map, nav_map_time = None, 0.
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -416,7 +420,6 @@ def main(demo=False):
       model.lat_delay = get_lat_delay(params, sm["lateralDelay"].lateralDelay)
       model.PLANPLUS_CONTROL = params.get("PlanplusControl", return_default=True)
       camera_offset_helper.set_offset(params.get("CameraOffset", return_default=True))
-      laneful_enabled = params.get_bool("Laneful")
     lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS
     if sm.updated["extrinsicsCalibration"] and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["extrinsicsCalibration"].rpyCalib, dtype=np.float32)
@@ -463,6 +466,16 @@ def main(demo=False):
     if 'action_t' in model.numpy_inputs:
       inputs['action_t'] = np.array([lat_action_t, long_action_t], dtype=np.float32)
 
+    now = time.monotonic()
+    if nav and (vipc_client_map.is_connected() or vipc_client_map.connect(False)):
+      if (buf_map := vipc_client_map.recv(0)) is not None:
+        nav_map = np.frombuffer(buf_map.data, dtype=np.uint8, count=buf_map.width * buf_map.height).reshape(buf_map.height, buf_map.width).copy()
+        nav_map_time = now
+    if nav_map is not None and now - nav_map_time < 0.5: # map timeout
+      inputs['nav_map'] = nav_map
+    if nav and sm.valid['navInstruction'] and now - sm.recv_time['navInstruction'] < 2:
+      inputs['navigation'] = navigation_input(sm['navInstruction'].allManeuvers, v_ego * (now - sm.recv_time['navInstruction']))
+
     mt1 = time.perf_counter()
     try:
       send_chestnut = (chestnut_state is not None and
@@ -490,9 +503,7 @@ def main(demo=False):
 
       if not sm['carControl'].latActive:
         prev_action.desiredCurvature = sm['controlsState'].curvature
-      laneful_active = laneful_enabled and desire == log.Desire.none and DH.lane_change_state == log.LaneChangeState.off
-      action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego,
-                                           partial(laneful.update, model_output, v_ego, laneful_active))
+      action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
       prev_action = action
       fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
                      publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
@@ -509,11 +520,6 @@ def main(demo=False):
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
       mdv2sp_send.valid = modelv2_send.valid
       mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
-      mdv2sp_send.modelDataV2SP.lanefulCurvature = laneful.curvature
-      mdv2sp_send.modelDataV2SP.lanefulTrust = float(laneful.trust)
-      mdv2sp_send.modelDataV2SP.lanefulLaneWidth = float(laneful.lane_width.x)
-      mdv2sp_send.modelDataV2SP.lanefulLeftProb = laneful.l_prob
-      mdv2sp_send.modelDataV2SP.lanefulRightProb = laneful.r_prob
       drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
       drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction