From 4d2d3f69f514f707c159e5e476059c23fa38bf4e Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Mon, 19 Oct 2026 23:11:22 -0700 Subject: [PATCH] winner takes all trust system in lane lines --- openpilot/sunnypilot/modeld_v2/laneful.py | 28 +++++++++++++++-------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/openpilot/sunnypilot/modeld_v2/laneful.py b/openpilot/sunnypilot/modeld_v2/laneful.py index 40b869d92d..02f7331b6a 100644 --- a/openpilot/sunnypilot/modeld_v2/laneful.py +++ b/openpilot/sunnypilot/modeld_v2/laneful.py @@ -10,22 +10,21 @@ 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.7 # m +LANE_WIDTH_DEFAULT = 3.2914 # m (averaged narrow lane in downtown streets, ~10.8ft usable) LANE_WIDTH_CLIP = (2.8, 4.5) # m LOOKAHEAD_TIME = 1.5 # s MIN_LOOKAHEAD = 10.0 # m -MAX_ERROR = 1.0 # m, cap on how far laneful can pull MIN_SPEED = 5.0 # m/s -JERK_LIMIT = 0.5 # m/s^3 -YIELD = (0.5, 1.0) # m LINE_STD = (0.2, 0.6) POINT_STD = 0.1 X_IDXS = np.array(ModelConstants.X_IDXS) -class Laneful: +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) + 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.curvature = 0.0 @@ -34,7 +33,8 @@ class Laneful: self.curvature = 0.0 return 0.0 target = self._target(model_output, v_ego, model_curvature) - step = JERK_LIMIT / max(v_ego, 1.0) ** 2 * DT_MDL + releasing = 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 @@ -47,9 +47,19 @@ class Laneful: 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])) - lane_trust = (l_prob + r_prob) / 2 + # 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 if lane_trust <= 0.0: return 0.0 @@ -68,12 +78,12 @@ class Laneful: # 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), -MAX_ERROR, MAX_ERROR) + 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 off center than the plant is now is likely going around something center_now = (l_prob * (lines[1, 0] + half_width) + r_prob * (lines[2, 0] - half_width)) / (l_prob + r_prob) plan_y = np.interp(lookahead, model_output['plan'][0, :, 0], model_output['plan'][0, :, 1]) away = abs(plan_y - np.interp(lookahead, x, center)) - abs(center_now) - 0.25 * abs(model_curvature) * lookahead ** 2 / 2 - lane_trust *= np.interp(away, YIELD, [1.0, 0.0]) + lane_trust *= np.interp(away, (0.5, 1.0), [1.0, 0.0]) return float(lane_trust * dk)