controls: Blendful lateral policy (alpha) (#2073)

CREDITS.md

@@ -45,3 +45,9 @@ Subaru angle support includes implementation maintained and integrated by **Jaco
 The inherited source covers Subaru angle platform/firmware definitions, `ES_Status` cruise state, angle controller/CAN integration, vehicle-model limits and native angle enforcement. StarPilot has additional platform-specific limits, driver handoff, discovery and AOL safety guards; this attribution does not claim identical tuning or complete parity with the reference fork. Original source commits, author names, emails and titles are recorded in [the Subaru origin history](opendbc_repo/opendbc/car/subaru/ORIGIN_HISTORY.tsv).
 
 The explicit StarPilot [jacobw Subaru adapter](opendbc_repo/opendbc/car/subaru/JACOBW.md) additionally integrates that branch's low-speed filter, target-first limiting and Crosstrek EPS envelope. Its preserved stricter guards and platform scope are documented alongside the code.
+
+## Experimental Blendful lane centering
+
+Blendful is authored by James Vecellio-Grant (Discountchubbs), with coauthors royjr <royjr96@gmail.com>, nayan <nayan8teen@gmail.com> and Jason Wen <haibin.wen3@gmail.com>. The algorithm and its tests come from commit `97135965effb3630ed55993903449edb73468d50`, “controls: Blendful lateral policy (alpha) (#2073)”. Original copyright notices and the referenced license are retained in [LICENSE.blendful.md](openpilot/starpilot/lateral/LICENSE.blendful.md). StarPilot adds the default-off developer selection and runtime compatibility guards.
+
+This project uses software from Haibin Wen and SUNNYPILOT LLC and is licensed under a custom license requiring permission for use.

openpilot/common/params_keys.h

@@ -106,6 +106,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"PauseAOLOnBrake", {PERSISTENT, FLOAT, "0.0"}},
     {"AolBrakePauseSpeedMps", {PERSISTENT, FLOAT}},
     {"LaneCentering", {PERSISTENT, BOOL, "0"}},
+    {"TryChubbsLaneCentering", {PERSISTENT, BOOL, "0"}},
     {"LaneCenteringPauseOnSignal", {PERSISTENT, BOOL, "1"}},
     {"LaneCenterOffset", {PERSISTENT, FLOAT, "0.0"}},
     {"LaneCenteringE2EAuthority", {PERSISTENT, FLOAT, "1.0"}},

openpilot/selfdrive/modeld/modeld.py

@@ -60,6 +60,8 @@ from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_drivi
 from openpilot.selfdrive.modeld.constants import LAT_SMOOTH_SECONDS, ModelConstants, Plan
 from openpilot.selfdrive.modeld.helpers import MODELS_DIR, chestnut_present, modeld_pkl_path, load_oob, wait_for_chestnut
 
+from openpilot.starpilot.lateral.chubbs_lane import ChubbsLaneCentering, selected as chubbs_selected
+
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
 LONG_SMOOTH_SECONDS = 0.3
@@ -467,6 +469,7 @@ def main(demo=False):
   initial_mono_ns, initial_boot_ns, initial_skew_ns = paired_clocks_ns()
   lane_clock_guard = ClockEpochGuard(initial_mono_ns, initial_boot_ns, initial_skew_ns)
   DH = DesireHelper(runtime_lane_policy)
+  chubbs = ChubbsLaneCentering(chubbs_selected(params))
   navigation_turn = TurnIntent(params, CP)
   lane_status_session = uuid.uuid4().hex
   lane_status_sequence = 0
@@ -602,6 +605,10 @@ def main(demo=False):
       posenet_send = messaging.new_message('cameraOdometry')
 
       action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
+      chubbs.apply(model_output, action, v_ego,
+                   active=bool(sm['carControl'].latActive and sm.all_checks(['carState', 'carControl']) and
+                               sm['carState'].canValid and not sm['carState'].canTimeout and not sm['carState'].steeringPressed),
+                   desire=desire, lane_change=DH.lane_change_state)
       filter_action(action, prev_action, v_ego, subaru_tau)
       prev_action = action
       fill_model_msg(modelv2_send, model_output, action,

openpilot/starpilot/galaxy/developer_settings.py

@@ -3,6 +3,7 @@
 from dataclasses import replace
 
 from openpilot.starpilot.longitudinal.voacc import KEY, DESCRIPTION
+from openpilot.starpilot.lateral.chubbs_lane import KEY as CHUBBS_KEY, DESCRIPTION as CHUBBS_DESCRIPTION
 from openpilot.starpilot.saved_document import commit_exact
 from openpilot.starpilot.saved_source import read_saved
 from openpilot.starpilot.ui.feature_settings_state import FeatureRow, FeatureSettingsState
@@ -55,9 +56,15 @@ class DeveloperSettingsOwner:
       FeatureRow(STARPORT_WIFI_KEY, "StarPort: Stay on Wi-Fi", "On" if enabled else "Off", b"1" if enabled else b"0", ("Off", "On"),
                  available=parked, reason=STARPORT_WIFI_DESCRIPTION if parked else "Park the vehicle to change this setting.",
                  default_value="On"),)
+    chubbs_raw, chubbs_readable = read_saved(self.params, CHUBBS_KEY, 8)
+    chubbs_valid = chubbs_readable and chubbs_raw in (None, b"0", b"1")
+    chubbs = FeatureRow(CHUBBS_KEY, "Try Chubbs Lane Centering",
+                        ("On" if chubbs_raw == b"1" else "Off") if chubbs_valid else "Invalid saved value",
+                        chubbs_raw, ("Off", "On") if chubbs_valid else (), available=parked and chubbs_valid,
+                        reason=CHUBBS_DESCRIPTION if parked else "Park the vehicle to change this setting.", default_value="Off")
     summary = "Changes apply next drive. Panda firmware changes require a device restart." if firmware_rows else "Changes apply next drive."
     return FeatureSettingsState("developer", "Developer driving options", summary,
-                                (row, reset_row, *firmware_rows, *starport_rows), parked)
+                                (row, chubbs, reset_row, *firmware_rows, *starport_rows), parked)
 
   def apply(self, request):
     if request.key == DRIVER_MONITORING_RESET:
@@ -76,9 +83,9 @@ class DeveloperSettingsOwner:
       return self.panda_wake is not None and self.panda_wake.apply(request)
     if request.key == STARPORT_WIFI_KEY:
       return self._apply_starport(request)
-    if request.key != KEY or request.value not in ("Off", "On") or request.expected not in (None, b"0", b"1"):
+    if request.key not in (KEY, CHUBBS_KEY) or request.value not in ("Off", "On") or request.expected not in (None, b"0", b"1"):
       return False
-    result = commit_exact(self.params, key=KEY, max_bytes=8, raw=b"1" if request.value == "On" else b"0",
+    result = commit_exact(self.params, key=request.key, max_bytes=8, raw=b"1" if request.value == "On" else b"0",
                           expected=request.expected, authorized=self.authorized, temp_prefix=".voacc-")
     return result.committed and result.verified
 

openpilot/starpilot/lateral/LICENSE.blendful.md

@@ -0,0 +1,21 @@
+# Custom MIT License
+
+Copyright (c) 2024, Haibin Wen, SUNNYPILOT LLC
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to view and modify the Software, subject to the following conditions:
+
+1. **Permission Required**: Permission Required for Commercial, For-Profit, or Closed Source Use: Use of the Software, in whole or in part, for any commercial purposes, for-profit projects, or in closed source projects requires explicit written permission from the original author(s).
+
+2. **Redistribution**: Any redistribution of the Software, modified or unmodified, must retain this license notice and the following acknowledgment:
+   "This software is licensed under a custom license requiring permission for use."
+
+3. **Visibility**: Any project that uses the Software must visibly mention the following acknowledgment:
+   "This project uses software from Haibin Wen and SUNNYPILOT LLC and is licensed under a custom license requiring permission for use."
+
+4. **No Warranty**: THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+Contact sunnypilot Support <support@sunnypilot.ai> for permission requests.
+
+---
+
+Haibin Wen, SUNNYPILOT LLC

openpilot/starpilot/lateral/blendful.py

@@ -0,0 +1,145 @@
+"""
+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 LICENSE.blendful.md in this directory for the full license.
+"""
+import numpy as np
+
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.modeld.constants import ModelConstants
+
+LANE_WIDTH_DEFAULT = 3.6  # m
+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
+YIELD = (0.2, 0.45)
+X_IDXS = np.array(ModelConstants.X_IDXS)
+
+
+class Blendful:
+  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.since_turning = 0.0
+    self.trust = 0.0
+    self.line_trust = 0.0
+    self.dk = 0.0 # delta kappa; difference between lane center and model plan
+    self.l_prob = 0.0
+    self.r_prob = 0.0
+    self.model_curvature = 0.0
+    self.smooth = 0.0 # smooths the correction target below .53 lat accel
+    self.curvature = 0.0
+
+  def update(self, model_output: dict[str, np.ndarray], v_ego: float, active: bool, model_curvature: float, enabled: bool = True) -> float:
+    self.l_prob = self.r_prob = 0.0
+    self.model_curvature = model_curvature
+    if not enabled:
+      if self.curvature or self.trust:
+        self.__init__()
+      return 0.0
+    fading = not active or v_ego < MIN_SPEED
+    if fading:
+      self.trust = self.line_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)
+    target = self.smooth = self.smooth + (target - self.smooth) * DT_MDL / (DT_MDL + np.interp(abs(model_curvature) * v_ego ** 2, [0.18, 0.53], [0.5, 0.0]))
+    build = 0.5 / max(v_ego, 1.0) ** 2 * DT_MDL  # jerk over 1 second
+    release = (0.5 if fading else 2.0) / max(v_ego, 1.0) ** 2 * DT_MDL  # lets go fast, but past zero it builds like any other pull
+    k = self.curvature
+    self.curvature = float(np.clip(target, max(k - release, -build) if k > 0 else k - build, min(k + release, build) if k < 0 else k + build))
+    return self.curvature
+
+  def _target(self, model_output: dict[str, np.ndarray], v_ego: float, model_curvature: float) -> float:
+    lookahead = max(v_ego * LOOKAHEAD_TIME, MIN_LOOKAHEAD)
+    lines = model_output['lane_lines'][0, :, :, 0]
+    stds = model_output['lane_lines_stds'][0, :, :, 0]
+
+    lone_weight = np.interp(abs(model_curvature) * v_ego ** 2, [0.18, 0.53], [0.5, 1.0])  # 0.5 on straights, 1.0 in curves
+    probs = self.line_probs.update(model_output['lane_lines_prob'][0, 1::2][1:3])
+    absent = (probs < 0.3) & (lone_weight > 0.5)
+    gone = self.line_gone.update((model_output['lane_lines_prob'][0, 1::2][1:3] < 0.3).astype(float))
+    missing = np.where(absent, 1.0, gone)
+    dropping = bool(np.any((probs < 0.6 * self.line_probs_slow.update(model_output['lane_lines_prob'][0, 1::2][1:3])) & ~absent))
+    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]))
+    self.l_prob, self.r_prob = l_prob, r_prob
+    # one line steadily on its own still centers at lone_weight. a line that is disappearing gives control back to laneless
+    if missing[1] > 0.8 and l_prob > 0.5:
+      lane_trust = l_prob * lone_weight
+    elif missing[0] > 0.8 and r_prob > 0.5:
+      lane_trust = r_prob * lone_weight
+    elif dropping:
+      lane_trust = min(l_prob, r_prob)
+    else:
+      lane_trust = l_prob + r_prob - l_prob * r_prob
+    if lone_weight > 0.5 and lane_trust < self.line_trust:
+      lane_trust = self.line_trust + (lane_trust - self.line_trust) * DT_MDL / (lone_weight - 0.5 + DT_MDL)
+    self.line_trust = lane_trust
+    if lane_trust <= 0.0:
+      self.trust = 0.0
+      return 0.0
+    if l_prob + r_prob <= 0.0:  # keep the last fit while trust lets go
+      self.trust = min(self.trust, lane_trust)
+      return float(self.trust * self.dk)
+
+    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]) # up to 50% plan y vs curvature parabola in sharp curves
+    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)
+    if min(gone) > 0.5:  # both lines gone: turns, intersections, relearn ideal model position
+      self.usual_offset.initialized = False
+    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
+    self.since_turning = 0.0 if abs(model_curvature) * lookahead ** 2 / 2 > 0.5 else self.since_turning + DT_MDL
+    if inside_curve:
+      self.going_around = False
+    elif away > YIELD[0] and self.since_turning > 2.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])
+
+    if lane_trust < self.trust:
+      dk = float(np.clip(dk, min(self.dk, 0.0), max(self.dk, 0.0)))
+    self.trust = lane_trust if lane_trust < self.trust else self.trust + (lane_trust - self.trust) * DT_MDL / (1.5 + DT_MDL)
+    self.dk = dk
+    return float(self.trust * dk)

openpilot/starpilot/lateral/chubbs_lane.py

@@ -0,0 +1,52 @@
+"""Optional model-frame lane correction; final steering limits remain in Controls."""
+
+import math
+
+import numpy as np
+
+from openpilot.starpilot.lateral.blendful import Blendful, X_IDXS, LOOKAHEAD_TIME, MIN_LOOKAHEAD, MIN_SPEED
+from openpilot.starpilot.saved_source import read_saved
+
+KEY = "TryChubbsLaneCentering"
+DESCRIPTION = ("Experimental lane-line guidance instead of StarPilot lane centering. Uses compatible local model outputs; " +
+               "does not change steering limits. Applies next drive.")
+
+
+def selected(params):
+  raw, readable = read_saved(params, KEY, 8)
+  return readable and raw == b"1"
+
+
+class ChubbsLaneCentering:
+  def __init__(self, enabled):
+    self.enabled = enabled
+    self.policy = Blendful() if enabled else None
+
+  def apply(self, outputs, action, speed, *, active, desire, lane_change):
+    if not self.enabled:
+      return action
+    if not active or desire != 0 or lane_change != 0 or not math.isfinite(speed) or speed < MIN_SPEED:
+      self.policy = Blendful()
+      return action
+    arrays = {name: outputs.get(name) for name in ("lane_lines", "lane_lines_stds", "lane_lines_prob", "plan")}
+    shapes = {"lane_lines": (1, 4, 33, 2), "lane_lines_stds": (1, 4, 33, 2), "lane_lines_prob": (1, 8)}
+    valid = all(isinstance(value, np.ndarray) and value.dtype.kind in "fiu" and np.isfinite(value).all()
+                for value in arrays.values())
+    if valid:
+      plan, stds, probs = arrays["plan"], arrays["lane_lines_stds"], arrays["lane_lines_prob"]
+      lookahead = max(speed * LOOKAHEAD_TIME, MIN_LOOKAHEAD)
+      valid = (all(arrays[name].shape == shape for name, shape in shapes.items()) and
+               plan.ndim == 3 and plan.shape[:2] == (1, 33) and plan.shape[2] >= 3 and
+               np.all(np.diff(plan[0, :, 0]) > 0) and np.all(stds[:, 1:3] > 0) and
+               np.all((probs >= 0) & (probs <= 1)) and
+               np.count_nonzero((X_IDXS >= 0.5 * lookahead) & (X_IDXS <= 1.5 * lookahead)) >= 3 and
+               math.isfinite(action.desiredCurvature))
+    if not valid:
+      self.policy = Blendful()
+      return action
+    correction = self.policy.update(outputs, speed, True, action.desiredCurvature)
+    if math.isfinite(correction):
+      action.desiredCurvature += correction
+    else:
+      self.policy = Blendful()
+    return action

openpilot/starpilot/lateral/lane_runtime.py

@@ -9,6 +9,7 @@ from openpilot.common.realtime import DT_CTRL
 from openpilot.common.params import UnknownKeyName
 from openpilot.starpilot.lateral.lane_centering import ControlMode, LaneCenteringRequest, LaneCenteringSettings, STRENGTH_BASELINE, STRENGTH_MAX
 from openpilot.starpilot.feature_capabilities import lateral_supported
+from openpilot.starpilot.lateral.chubbs_lane import selected as chubbs_selected
 
 
 def read_settings(params) -> LaneCenteringSettings:
@@ -55,6 +56,7 @@ class LaneCenteringHost:
 
   def __init__(self, params):
     self.params = params
+    self.chubbs_enabled = chubbs_selected(params)
     self.settings = LaneCenteringSettings()
     self.last_refresh_ns: int | None = None
     self.last_tick_ns: int | None = None
@@ -62,7 +64,7 @@ class LaneCenteringHost:
 
   def inputs_need_refresh(self, sm, *, now_ns: int) -> bool:
     """Leave time to receive queued state before checking its original age."""
-    return self.settings.enabled and any(
+    return not self.chubbs_enabled and self.settings.enabled and any(
       0 < int(sm.logMonoTime[name]) < now_ns - 15_000_000 for name in ('carState', 'selfdriveState')
     )
 
@@ -79,7 +81,7 @@ class LaneCenteringHost:
     if self.last_refresh_ns is None or now_ns < self.last_refresh_ns or now_ns - self.last_refresh_ns >= self.REFRESH_NS:
       self.settings = read_settings(self.params)
       self.last_refresh_ns = now_ns
-    if not self.settings.enabled:
+    if self.chubbs_enabled or not self.settings.enabled:
       self.last_refusal = 'host_disabled'
       return None
     if not sm['carState'].canValid:

openpilot/starpilot/lateral/tests/test_blendful.py

@@ -0,0 +1,112 @@
+"""
+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 ../LICENSE.blendful.md for the full license.
+"""
+import numpy as np
+
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.starpilot.lateral.blendful import Blendful
+from openpilot.selfdrive.modeld.constants import ModelConstants
+
+X = np.array(ModelConstants.X_IDXS)
+BUILD, RELEASE = 0.5, 2.0
+
+
+def frame(center=0.0, plan=0.0, k=0.0, probs=(0.9, 0.9), stds=(0.1, 0.1), width=3.5):
+  road = k * X ** 2 / 2
+  lines = np.zeros((1, 4, 33, 2))
+  lane_stds = np.ones((1, 4, 33, 2))
+  for i, off in enumerate((-1.5 * width, -width / 2, width / 2, 1.5 * width)):
+    lines[0, i, :, 0] = road + center + off
+  lane_stds[0, 1, :, 0], lane_stds[0, 2, :, 0] = stds
+  lane_probs = np.zeros((1, 8))
+  lane_probs[0, 3], lane_probs[0, 5] = probs
+  path = np.zeros((1, 33, 3))
+  path[0, :, 0] = X
+  path[0, :, 1] = road + plan * np.clip(X / 10, 0, 1)
+  return {'lane_lines': lines, 'lane_lines_stds': lane_stds, 'lane_lines_prob': lane_probs, 'plan': path}
+
+
+def drive(bf, seconds, v=20.0, k=0.0, **kw):
+  out = [bf.update(frame(k=k, **kw), v, True, k) for _ in range(int(seconds / DT_MDL))]
+  return np.array(out) * v ** 2  # m/s^2
+
+
+class TestBlendful(OpenpilotTestCase):
+  def setup_method(self):
+    self.bf = Blendful()
+
+  def test_centered_adds_nothing(self):
+    push = drive(self.bf, 5.0)
+    assert self.bf.trust > 0.9
+    assert np.abs(push).max() < 1e-3
+
+  def test_pulls_toward_lane_center(self):
+    push = drive(self.bf, 5.0, center=0.5)
+    assert push[-1] > 0.02  # + y side, where the lane center is
+    assert self.bf.trust > 0.9
+    assert np.all(np.diff(push) <= BUILD * DT_MDL + 1e-9)
+
+  def test_flipping_sides_builds_slowly(self):
+    k = 0.002  # 0.8 m/s^2 at 20 m/s
+    drive(self.bf, 5.0, k=k, center=0.5)
+    push = drive(self.bf, 5.0, k=k, center=-0.5)
+    rate = np.diff(push) / DT_MDL
+    assert push[-1] < 0
+    assert np.all(rate >= -RELEASE - 1e-6)
+    assert np.all(rate[push[:-1] <= 0] >= -BUILD - 1e-6)  # once past zero it builds
+    crossing = push[1:][(push[:-1] > 0) & (push[1:] <= 0)]
+    assert len(crossing) and np.all(crossing >= -BUILD * DT_MDL - 1e-9)
+
+  def test_weak_lines_drive_laneless(self):
+    clear = drive(Blendful(), 5.0, center=0.5)[-1]
+    faint = Blendful()
+    push = drive(faint, 5.0, center=0.5, probs=(0.1, 0.1))
+    assert faint.trust < 0.25 and abs(push[-1]) < 0.25 * clear  # faint lines = trust and push scale down
+    blurry = Blendful()
+    push = drive(blurry, 5.0, center=0.5, stds=(0.8, 0.8))
+    assert blurry.trust == 0.0 and np.abs(push).max() < 1e-6  # lines too uncertain = laneless
+
+  def test_lone_line_holds_through_a_blink_in_a_curve(self):
+    k, v = 1 / 20, 6.0
+    drive(self.bf, 6.0, v=v, k=k, plan=0.4, probs=(0.9, 0.05))
+    before = self.bf.trust
+    drive(self.bf, 0.25, v=v, k=k, plan=0.4, probs=(0.1, 0.05), stds=(1.0, 0.1))
+    bottom = self.bf.trust
+    drive(self.bf, 2.0, v=v, k=k, plan=0.4, probs=(0.9, 0.05))
+    assert before > 0.5
+    assert bottom > 0.5 * before  # keeps latched through flicker
+    assert self.bf.trust > 0.85 * before
+
+  def test_yields_when_the_model_goes_around(self):
+    drive(self.bf, 6.0)
+    push = drive(self.bf, 3.0, plan=0.8)
+    assert self.bf.going_around
+    assert self.bf.trust < 0.05
+    assert abs(push[-1]) < 0.01
+
+  def test_fit_does_not_grow_while_trust_falls(self):
+    drive(self.bf, 5.0, center=0.3)
+    for i in range(40):
+      dk, trust = self.bf.dk, self.bf.trust
+      p = 0.9 - 0.02 * i
+      self.bf.update(frame(center=0.3, plan=-0.02 * i, probs=(p, p)), 20.0, True, 0.0)
+      if self.bf.trust < trust:
+        assert abs(self.bf.dk) <= abs(dk) + 1e-12
+
+  def test_toggle_off_returns_zero_and_resets(self):
+    drive(self.bf, 5.0, center=0.5)
+    assert self.bf.curvature != 0.0
+    assert self.bf.update(frame(center=0.5), 20.0, True, 0.0, enabled=False) == 0.0
+    assert self.bf.curvature == 0.0 and self.bf.trust == 0.0
+
+  def test_fades_below_min_speed(self):
+    drive(self.bf, 5.0, v=10.0, center=0.5)
+    start = self.bf.curvature
+    out = np.array([self.bf.update(frame(center=0.5), 4.0, True, 0.0) for _ in range(100)])
+    assert self.bf.trust == 0.0
+    assert abs(out[0]) < abs(start)
+    assert abs(out[-1]) < 1e-6

openpilot/starpilot/lateral/tests/test_chubbs_lane.py

@@ -0,0 +1,137 @@
+import ast
+from pathlib import Path
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from openpilot.common.params import Params, ParamKeyType
+from openpilot.starpilot.galaxy.developer_settings import DeveloperSettingsOwner
+from openpilot.starpilot.lateral.chubbs_lane import KEY, ChubbsLaneCentering, selected
+from openpilot.starpilot.lateral.lane_runtime import LaneCenteringHost
+from openpilot.starpilot.lateral.tests.test_blendful import frame
+from openpilot.starpilot.ui.feature_settings_state import FeatureSettingsRequest
+
+
+def action():
+  return SimpleNamespace(desiredCurvature=0.0, desiredAcceleration=-0.3, shouldStop=True)
+
+
+def apply(policy, outputs=None, **overrides):
+  values = {"active": True, "desire": 0, "lane_change": 0}
+  values.update(overrides)
+  return policy.apply(frame(center=.5) if outputs is None else outputs, action(), 20., **values)
+
+
+def test_default_off_and_typed_persistence(tmp_path):
+  params = Params(str(tmp_path))
+  assert params.get_type(KEY) == ParamKeyType.BOOL
+  assert params.get_default_value(KEY) is False
+  assert not selected(params)
+  for raw in (b"", b"0", b"true", b"1\n", b"1" * 9):
+    Path(params.get_param_path(KEY)).write_bytes(raw)
+    assert not selected(params)
+  params.put_bool(KEY, True, block=True)
+  assert selected(params)
+  assert selected(Params(str(tmp_path)))
+
+
+def test_disabled_never_reads_outputs_or_changes_actions():
+  policy = ChubbsLaneCentering(False)
+  original = action()
+  assert policy.apply(None, original, 20., active=True, desire=0, lane_change=0) is original
+  assert vars(original) == vars(action())
+  assert policy.policy is None
+
+
+def test_centers_without_modifying_longitudinal():
+  policy = ChubbsLaneCentering(True)
+  result = None
+  for _ in range(120):
+    result = apply(policy)
+  assert result.desiredCurvature > 0
+  assert result.desiredAcceleration == -.3 and result.shouldStop
+  assert policy.policy.trust > .9
+
+
+@pytest.mark.parametrize("field,value", [("active", False), ("desire", 1), ("desire", 2), ("lane_change", 1)])
+def test_no_correction_during_inactive_turn_or_lane_change(field, value):
+  policy = ChubbsLaneCentering(True)
+  for _ in range(80):
+    apply(policy)
+  assert policy.policy.curvature != 0
+  assert apply(policy, **{field: value}).desiredCurvature == 0
+  assert policy.policy.curvature == 0
+
+
+@pytest.mark.parametrize("case", ["missing", "shape", "nan", "zero_std", "negative_std", "probability", "plan_x", "dtype"])
+def test_unsupported_or_corrupt_outputs_preserve_model_action(case):
+  outputs = frame(center=.5)
+  if case == "missing":
+    del outputs["lane_lines_stds"]
+  elif case == "shape":
+    outputs["lane_lines"] = outputs["lane_lines"][:, :3]
+  elif case == "nan":
+    outputs["lane_lines"][0, 1, 0, 0] = np.nan
+  elif case == "zero_std":
+    outputs["lane_lines_stds"][:] = 0
+  elif case == "negative_std":
+    outputs["lane_lines_stds"][:] = -1
+  elif case == "probability":
+    outputs["lane_lines_prob"][:] = 2
+  elif case == "plan_x":
+    outputs["plan"][0, :, 0] = 0
+  elif case == "dtype":
+    outputs["plan"] = outputs["plan"].astype(object)
+  result = apply(ChubbsLaneCentering(True), outputs)
+  assert vars(result) == vars(action())
+
+
+def test_normal_lane_controller_is_not_stacked_and_selection_is_frozen(tmp_path):
+  params = Params(str(tmp_path))
+  params.put_bool("LaneCentering", True, block=True)
+  params.put_bool(KEY, True, block=True)
+  host = LaneCenteringHost(params)
+  assert host.sample(None, now_ns=1, lateral_active=True, longitudinal_active=False) is None
+  params.put_bool(KEY, False, block=True)
+  assert host.sample(None, now_ns=2, lateral_active=True, longitudinal_active=False) is None
+  assert not LaneCenteringHost(params).chubbs_enabled
+
+
+def test_developer_row_and_parked_exact_write(tmp_path):
+  params = Params(str(tmp_path))
+  parked = [True]
+  owner = DeveloperSettingsOwner(params, lambda: parked[0])
+  row = next(row for row in owner.snapshot().rows if row.key == KEY)
+  assert row.label == "Try Chubbs Lane Centering" and row.value == "Off"
+  assert owner.apply(FeatureSettingsRequest(KEY, None, "On"))
+  assert selected(params)
+  assert not owner.apply(FeatureSettingsRequest(KEY, None, "Off"))
+  parked[0] = False
+  assert not owner.apply(FeatureSettingsRequest(KEY, b"1", "Off"))
+  assert selected(params)
+
+
+def test_actual_modeld_call_preserves_driver_override_and_desire():
+  source = Path(__file__).resolve().parents[3] / "selfdrive/modeld/modeld.py"
+  tree = ast.parse(source.read_text())
+  call = next(node for node in ast.walk(tree) if isinstance(node, ast.Expr) and isinstance(node.value, ast.Call)
+              and ast.unparse(node.value.func) == "chubbs.apply")
+  code = compile(ast.fix_missing_locations(ast.Module(body=[call], type_ignores=[])), str(source), "exec")
+  class State(dict):
+    def all_checks(self, services):
+      return True
+  state = State(carControl=SimpleNamespace(latActive=True),
+                carState=SimpleNamespace(canValid=True, canTimeout=False, steeringPressed=False))
+  policy = ChubbsLaneCentering(True)
+  for _ in range(100):
+    result = action()
+    scope = {"chubbs": policy, "model_output": frame(center=.5), "action": result, "v_ego": 20., "sm": state,
+             "desire": 0, "DH": SimpleNamespace(lane_change_state=0)}
+    exec(code, scope)
+  assert result.desiredCurvature > 0
+  state["carState"].steeringPressed = True
+  result = action()
+  scope["action"] = result
+  exec(code, scope)
+  assert result.desiredCurvature == 0

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "d07062ca2c894ec10ed8275a03960d435bcc4462fe1a622267353da99759723e", "schema_version": 1}
+{"protected_inventory_sha256": "2b91b90d7993a9b083ca82e08f9aa7e91e04dd4eb1b8fa8b6c77d4717916dd14", "schema_version": 1}