From d59a37d37feebf1671ca8998d4e45ce40101b706 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 25 Oct 2026 19:17:20 -0500 Subject: [PATCH] Track single lane lines with smooth transitions and predicted clearance --- opendbc_repo/opendbc/car/hyundai/interface.py | 8 + openpilot/selfdrive/controls/controlsd.py | 14 +- openpilot/starpilot/lateral/lane_centering.py | 57 ++++- openpilot/starpilot/lateral/lane_clearance.py | 144 +++++++++++++ openpilot/starpilot/lateral/lane_tracking.py | 200 ++++++++++++++++++ .../lateral/tests/test_global_lane_runtime.py | 29 +++ .../lateral/tests/test_lane_centering.py | 156 ++++++++++++++ .../starpilot/lateral/tests/test_lane_host.py | 83 ++++++++ .../lateral/tests/test_lane_runtime.py | 60 +++++- prebuilt.json | 2 +- 10 files changed, 744 insertions(+), 9 deletions(-) create mode 100644 openpilot/starpilot/lateral/lane_clearance.py create mode 100644 openpilot/starpilot/lateral/lane_tracking.py diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index 3518f02b99..9c0b43f115 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -33,6 +33,14 @@ class CarInterface(CarInterfaceBase): CarController = CarController RadarInterface = RadarInterface + @staticmethod + def get_lateral_body_dimensions(cp): + # Hyundai's published pre-facelift IONIQ 6 body dimensions, metres. + # https://www.hyundai.news/uk/models/electrified/ioniq-6/press-kit/hyundai-ioniq-6-delivers-a-new-electric-mobility-experience.html + if cp.carFingerprint == CAR.HYUNDAI_IONIQ_6: + return 1.880, 4.855 + return None + @staticmethod def startup_required(cp): from opendbc.car.hyundai.g90_startup import required diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index 4c2fc2c03f..e6d3ed2868 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -37,6 +37,7 @@ from openpilot.starpilot.lateral.lane_centering import ( LaneCenteringController, LaneCenteringInput, LaneCenteringRequest, LaneCenteringResult, ) from openpilot.starpilot.lateral.lane_feedback import feedback_message +from openpilot.starpilot.lateral.lane_clearance import current_delay, interface_dimensions from openpilot.starpilot.lateral.lane_change_preferences import LiveLaneChangePolicy from openpilot.starpilot.lateral.lane_change_smoothing import LaneChangeSmoother, limit_rate from openpilot.starpilot.lateral.lane_runtime import LaneCenteringHost, runtime_supported as lane_runtime_supported @@ -95,7 +96,7 @@ class Controls: self.curvature = 0.0 self.desired_curvature = 0.0 self.model_turn_assist = ModelTurnAssist() - self.lane_centering_controller = LaneCenteringController() + self.lane_centering_controller = LaneCenteringController(body=interface_dimensions(self.CI, self.CP)) self.lateral_pause = LateralPause(read_lateral_pause(self.params), self.params) self.lane_change_preferences = LiveLaneChangePolicy(self.params, now_ns=0 if os.getenv('REPLAY') == '1' else time.monotonic_ns()) @@ -364,6 +365,7 @@ class Controls: lane_base_curvature = new_desired_curvature self.lane_centering_applied = 0.0 self.last_lane_centering_result = None + lane_delay = None if lane_centering is None and self.lane_centering_host is not None: now_ns = int(self.sm.logMonoTime['carState']) if os.getenv('REPLAY') == '1' else time.monotonic_ns() lane_centering = self.lane_centering_host.sample(self.sm, now_ns=now_ns, @@ -374,6 +376,8 @@ class Controls: self.lane_centering_controller.reset() self.last_lane_centering_result = LaneCenteringResult(None, 0.0, 0, "invalid_request") else: + lane_now_ns = int(self.sm.logMonoTime['carState']) if os.getenv('REPLAY') == '1' else time.monotonic_ns() + lane_delay = current_delay(self.sm, lane_now_ns) result = self.lane_centering_controller.update(LaneCenteringInput( model=model_v2, model_sample_id=self.sm.logMonoTime['modelV2'], model_valid=bool(self.sm.all_checks(['modelV2'])), base_curvature=new_desired_curvature, @@ -381,6 +385,8 @@ class Controls: settings=lane_centering.settings, elapsed_seconds=lane_centering.elapsed_seconds, turn_signal_active=bool(CS.leftBlinker or CS.rightBlinker), driver_override=bool(CS.steeringPressed), time_discontinuity=lane_centering.time_discontinuity, + measured_curvature=self.curvature, + lateral_delay_s=lane_delay, )) self.last_lane_centering_result = result if result.candidate_curvature is not None: @@ -398,6 +404,12 @@ class Controls: self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, previous_curvature, new_desired_curvature, lp.roll) if self.last_lane_centering_result is not None and self.last_lane_centering_result.correction != 0.0: baseline_curvature, _ = clip_curvature(CS.vEgo, previous_curvature, limit_rate(CS.vEgo, previous_curvature, lane_base_curvature, factor), lp.roll) + guarded = self.lane_centering_controller.guard_applied( + model_v2, baseline_curvature, self.desired_curvature, self.curvature, CS.vEgo, + lane_delay) + if guarded != self.desired_curvature: + self.desired_curvature = guarded + curvature_limited = True self.lane_centering_applied = self.desired_curvature - baseline_curvature from openpilot.starpilot.car.subaru.smoothing import seconds as subaru_smooth_seconds, control_maximum lat_delay = self.sm["lateralDelay"].lateralDelay + LAT_SMOOTH_SECONDS + subaru_smooth_seconds(CS.vEgo, control_maximum(self)) diff --git a/openpilot/starpilot/lateral/lane_centering.py b/openpilot/starpilot/lateral/lane_centering.py index cabe3092c6..cf52f163b1 100644 --- a/openpilot/starpilot/lateral/lane_centering.py +++ b/openpilot/starpilot/lateral/lane_centering.py @@ -15,6 +15,8 @@ import numpy as np from openpilot.cereal import log from openpilot.common.scalar_clip import clip_scalar +from openpilot.starpilot.lateral.lane_tracking import SingleLineTracker +from openpilot.starpilot.lateral.lane_clearance import BodyDimensions, limit_correction, limit_tracking STRENGTH_BASELINE = 1.5 @@ -80,6 +82,8 @@ class LaneCenteringInput: turn_signal_active: bool = False driver_override: bool = False time_discontinuity: bool = False + measured_curvature: float | None = None + lateral_delay_s: float | None = None @dataclass(frozen=True) @@ -167,7 +171,7 @@ def _single_boundary_target(model: Any, paths, qualified, lookahead: float, offs return target -def raw_correction(model: Any, speed_mps: float, offset_m: float, e2e_authority: float, *, _paths=None) -> float | None: +def raw_correction(model: Any, speed_mps: float, offset_m: float, e2e_authority: float, *, _paths=None, _single_target=None) -> float | None: """Return a finite raw curvature correction, or None for unqualified model data.""" speed = _finite_number(speed_mps) offset = _finite_number(offset_m) @@ -209,7 +213,14 @@ def raw_correction(model: Any, speed_mps: float, offset_m: float, e2e_authority: target_y = (left + right) * 0.5 + clip_scalar(offset, -safe_offset, safe_offset) else: std_path = _path_xy(model.position.x, model.position.yStd) if _paths is None else _paths[3] - target_y = _single_boundary_target(model, (left_path, right_path, model_path, std_path), qualified, lookahead, offset) + if _single_target is not None: + known_path = left_path if qualified[0] else right_path + path_std = _at(std_path, lookahead) if std_path is not None else None + if (known_path is None or _at(known_path, lookahead) is None or + path_std is None or not 0. <= path_std <= E2E_MAX_PATH_STD): + return None + target_y = (_single_target if _single_target is not None else + _single_boundary_target(model, (left_path, right_path, model_path, std_path), qualified, lookahead, offset)) if target_y is None: return None error = target_y - model_y @@ -239,21 +250,35 @@ def _reader_geometry_key(model): try: return (tuple(model.laneLineProbs), tuple(model.laneLineStds), *((tuple(line.x), tuple(line.y)) for line in (model.laneLines[1], model.laneLines[2])), - tuple(model.position.x), tuple(model.position.y), tuple(model.position.yStd)) + tuple(model.position.x), tuple(model.position.y), tuple(model.position.yStd), + tuple(model.roadEdgeStds), tuple((tuple(edge.x), tuple(edge.y)) for edge in model.roadEdges)) except (AttributeError, IndexError, TypeError, ValueError, OverflowError): return None class LaneCenteringController: - def __init__(self) -> None: + def __init__(self, *, body: BodyDimensions | None = None) -> None: self._correction = 0.0 self._geometry_cache = None + self._tracker = SingleLineTracker() + self._body = body + self._clearance_observation = None def reset(self) -> None: self._correction = 0.0 self._geometry_cache = None + self._tracker.reset() def _result(self, base: float, reason: str) -> LaneCenteringResult: + if self._body is not None and self._correction != 0.: + source = self._clearance_observation + measured = _finite_number(source.measured_curvature) if source is not None else None + delay = _finite_number(source.lateral_delay_s) if source is not None else None + guarded = (0. if measured is None or delay is None else + limit_correction(source.model, base, self._correction, measured, source.speed_mps, delay, self._body)) + if guarded != self._correction: + self._correction = guarded + reason = 'clearance_limited' correction = self._correction return LaneCenteringResult(base + correction, correction, (correction > 0) - (correction < 0), reason) @@ -263,10 +288,29 @@ class LaneCenteringController: self._correction = 0.0 return self._result(base, reason) + def guard_applied(self, model, base: float, candidate: float, measured: float, speed: float, delay) -> float: + if base == candidate or self._body is None and self._tracker.path is None: + return candidate + checked_delay = _finite_number(delay) + delta = candidate - base + allowed = 0. if checked_delay is None else delta + if checked_delay is not None: + if self._tracker.path is not None: + x, y = self._tracker.path + source = self._clearance_observation + offset = source.settings.offset_m if source is not None else 0. + allowed = limit_tracking((x, y + offset), base, allowed, measured, speed, checked_delay) + if self._body is not None: + allowed = limit_correction(model, base, allowed, measured, speed, checked_delay, self._body) + if allowed != delta: + self._correction *= abs(allowed / delta) + return base + allowed + def update(self, observation: LaneCenteringInput) -> LaneCenteringResult: if not isinstance(observation, LaneCenteringInput): self.reset() return LaneCenteringResult(None, 0.0, 0, "invalid_input") + self._clearance_observation = observation base = _finite_number(observation.base_curvature) speed = _finite_number(observation.speed_mps) dt = _finite_number(observation.elapsed_seconds) @@ -294,6 +338,7 @@ class LaneCenteringController: self.reset() return self._result(base, "driver_override") if settings.pause_on_signal and observation.turn_signal_active: + self._tracker.reset() return self._fade(base, dt, "signal_release") try: if observation.model.meta.laneChangeState != log.LaneChangeState.off: @@ -316,7 +361,9 @@ class LaneCenteringController: self._geometry_cache = (observation.model, cache_key, paths) else: self._geometry_cache = None - raw = raw_correction(observation.model, speed, offset, authority, _paths=paths) + tracked = self._tracker.target(observation.model, observation.model_sample_id, speed, dt, offset, revision=geometry) + raw = None if tracked.blocked else raw_correction(observation.model, speed, offset, authority, + _paths=paths, _single_target=tracked.y) if raw is None or not math.isfinite(raw): return self._fade(base, dt, "unqualified_model") target = clip_scalar(raw, -0.004, 0.004) * 0.30 diff --git a/openpilot/starpilot/lateral/lane_clearance.py b/openpilot/starpilot/lateral/lane_clearance.py new file mode 100644 index 0000000000..958cdadfe0 --- /dev/null +++ b/openpilot/starpilot/lateral/lane_clearance.py @@ -0,0 +1,144 @@ +"""Compare nominal and corrected motion against current observed road edges.""" + +from dataclasses import dataclass +import math + +import numpy as np + + +@dataclass(frozen=True) +class BodyDimensions: + width: float + length: float + + @property + def valid(self): + return ( + type(self.width) is float + and type(self.length) is float + and math.isfinite(self.width) + and math.isfinite(self.length) + and 1.3 <= self.width <= 2.6 + and 2.5 <= self.length <= 7.0 + ) + + +def interface_dimensions(interface, cp) -> BodyDimensions | None: + provider = getattr(interface, 'get_lateral_body_dimensions', None) + if not callable(provider): + return None + dimensions = provider(cp) + if dimensions is None: + return None + body = BodyDimensions(*dimensions) + return body if body.valid else None + + +def current_delay(sm, now_ns: int) -> float | None: + from openpilot.cereal.services import SERVICE_LIST + stamp = int(sm.logMonoTime['lateralDelay']) + if (not sm.all_checks(['lateralDelay']) or not 0 < stamp <= now_ns or + now_ns - stamp > int(2.5e9 / SERVICE_LIST['lateralDelay'].frequency)): + return None + value = float(sm['lateralDelay'].lateralDelay) + return value if math.isfinite(value) and 0. <= value <= 1. else None + + +def _center_motion(curvature: float, measured: float, speed: float, delay: float): + horizon = min(35.0, max(8.0, speed * 1.5)) + ds = horizon / 16.0 + distances = (np.arange(16, dtype=float) + 0.5) * ds + # Test the measured-to-command handoff rather than substituting the auxiliary + # model position path for the commanded motion. + blend = np.clip((distances / speed - delay) / 0.2, 0.0, 1.0) + ks = measured + (curvature - measured) * blend + headings = np.cumsum(ks * ds) + midpoint = headings - ks * ds * 0.5 + x, y = np.cumsum(np.cos(midpoint) * ds), np.cumsum(np.sin(midpoint) * ds) + return x, y, headings + + +def _motion(curvature: float, measured: float, speed: float, delay: float, body: BodyDimensions): + x, y, headings = _center_motion(curvature, measured, speed, delay) + # Model coordinates originate below the camera. With no measured mounting + # offset, full length/width on EACH side conservatively cover any camera + # position inside the body rectangle. These are not assumed axle coordinates. + longitudinal = np.array([-body.length, -body.length, body.length, body.length]) + lateral = np.array([-body.width, body.width, -body.width, body.width]) + c, s = np.cos(headings)[:, None], np.sin(headings)[:, None] + return x[:, None] + c * longitudinal - s * lateral, y[:, None] + s * longitudinal + c * lateral + + +def limit_tracking(path, base: float, correction: float, measured: float, speed: float, delay: float) -> float: + """A line-tracking contribution must improve its predicted spacing objective.""" + if not all(math.isfinite(v) for v in (base, correction, measured, speed, delay)) or speed < 5.0 or not 0.0 <= delay <= 1.0: + return 0.0 + x, y, _ = _center_motion(base, measured, speed, delay) + selected = (x >= max(2.0, path[0][0])) & (x <= path[0][-1]) + if np.count_nonzero(selected) < 4: + return 0.0 + original = float(np.mean((y[selected] - np.interp(x[selected], *path)) ** 2)) + for factor in (1.0, 0.75, 0.5, 0.25): + cx, cy, _ = _center_motion(base + correction * factor, measured, speed, delay) + if not np.all((cx[selected] >= path[0][0]) & (cx[selected] <= path[0][-1])): + continue + candidate = float(np.mean((cy[selected] - np.interp(cx[selected], *path)) ** 2)) + if candidate <= original + 1e-12: + return correction * factor + return 0.0 + + +def limit_correction(model, base: float, correction: float, measured: float, speed: float, delay: float, body: BodyDimensions) -> float: + """Retain the largest tested correction that does not worsen edge intrusion. + + This guards only the added lane contribution. The original model command and + native curvature, jerk, authority and driver-override limits remain upstream. + """ + if not body.valid or not all(math.isfinite(v) for v in (base, correction, measured, speed, delay)): + return 0.0 + if speed < 5.0 or not 0.0 <= delay <= 1.0 or abs(base) > 0.2 or abs(measured) > 0.2: + return 0.0 + if correction == 0.0: + return 0.0 + edges = [] + try: + for index in (0, 1): + if len(model.roadEdges) <= index or len(model.roadEdgeStds) <= index: + continue + std = float(model.roadEdgeStds[index]) + if not math.isfinite(std) or std < 0.0: + return 0.0 + if std > 0.3: + continue + edge = model.roadEdges[index] + x, y = np.asarray(edge.x, dtype=float), np.asarray(edge.y, dtype=float) + if x.ndim != 1 or y.ndim != 1 or len(x) < 2 or len(x) != len(y) or not np.isfinite(x).all() or not np.isfinite(y).all() or np.any(np.diff(x) <= 0): + return 0.0 + edges.append((index, x, y, np.gradient(y, x), std)) + except (AttributeError, IndexError, TypeError, ValueError, OverflowError): + return 0.0 + if not edges: + return correction + original_x, original_y = _motion(base, measured, speed, delay, body) + for factor in (1.0, 0.75, 0.5, 0.25): + candidate_x, candidate_y = _motion(base + correction * factor, measured, speed, delay, body) + accepted = True + for side, x, y, slope, std in edges: + # Use the same future body samples in both arms; never extrapolate an edge + # behind the camera or beyond the observed geometry. + rows = np.all((original_x >= x[0]) & (original_x <= x[-1]), axis=1) + if np.count_nonzero(rows) < 4 or not np.all((candidate_x[rows] >= x[0]) & (candidate_x[rows] <= x[-1])): + accepted = False + break + ox, oy = original_x[rows].ravel(), original_y[rows].ravel() + cx, cy = candidate_x[rows].ravel(), candidate_y[rows].ravel() + sign = 1.0 if side == 0 else -1.0 + old = float(np.min(sign * (oy - np.interp(ox, x, y)) / np.hypot(1.0, np.interp(ox, x, slope)))) + new = float(np.min(sign * (cy - np.interp(cx, x, y)) / np.hypot(1.0, np.interp(cx, x, slope)))) + margin = 0.1 + std + if min(new, margin) < min(old, margin) - 1e-6: + accepted = False + break + if accepted: + return correction * factor + return 0.0 diff --git a/openpilot/starpilot/lateral/lane_tracking.py b/openpilot/starpilot/lateral/lane_tracking.py new file mode 100644 index 0000000000..e253a46467 --- /dev/null +++ b/openpilot/starpilot/lateral/lane_tracking.py @@ -0,0 +1,200 @@ +"""Current-line spacing preferences with bounded acquisition and ownership.""" + +from dataclasses import dataclass +import math + +import numpy as np + + +@dataclass(frozen=True) +class TrackingTarget: + y: float | None = None + blocked: bool = False + + +class SingleLineTracker: + ACQUIRE_SECONDS = 0.20 + MAX_SOURCE_GAP = 0.15 + WIDTH_MAX_AGE = 5.0 + WIDTH_MAX_DISTANCE = 75.0 + MAX_PATH_DISAGREEMENT = 0.8 + MIN_CLEARANCE = 1.1 + + def __init__(self): + self.reset() + + def reset(self): + self.clock = 0.0 + self.travel = 0.0 + self.width = None + self.width_at = self.width_travel = 0.0 + self.side = None + self.spacing = None + self.pending = None + self.acquired_at = 0.0 + self.last_good_at = None + self.last_id = None + self.last_revision = None + self.frame_at = 0.0 + self.last_near = None + self.last_slope = None + self.path = None + self.blocked = False + + def _clear_line(self): + self.side = self.spacing = self.pending = self.path = None + self.last_near = self.last_slope = self.last_good_at = None + + @staticmethod + def _points(path, *, uncertainty=False): + x, y = np.asarray(path.x, dtype=float), np.asarray(path.yStd if uncertainty else path.y, dtype=float) + if x.ndim != 1 or y.ndim != 1 or len(x) < 2 or len(x) != len(y) or not np.isfinite(x).all() or not np.isfinite(y).all() or np.any(np.diff(x) <= 0): + return None + return x, y + + @staticmethod + def _at(points, x): + return float(np.interp(x, *points)) if points[0][0] <= x <= points[0][-1] else None + + def target(self, model, sample_id: int, speed: float, dt: float, offset: float, *, revision=None) -> TrackingTarget: + self.clock += dt + self.travel += speed * dt + lookahead = min(35.0, max(8.0, speed)) + revision = id(model) if revision is None else revision + if sample_id == self.last_id and revision == self.last_revision: + if self.clock - self.frame_at > self.MAX_SOURCE_GAP: + self._clear_line() + self.blocked = False + return TrackingTarget() + return self._target(lookahead, offset) + previous_id, self.last_id = self.last_id, sample_id + self.last_revision, self.frame_at = revision, self.clock + if previous_id is not None and sample_id < previous_id: + self.reset() + self.last_id = sample_id + return TrackingTarget(blocked=True) + try: + probs, stds = model.laneLineProbs, model.laneLineStds + if len(probs) < 3 or len(stds) < 3 or len(model.laneLines) < 3: + raise ValueError + if not all(math.isfinite(float(v)) for v in (probs[1], probs[2], stds[1], stds[2])): + raise ValueError + good = [0.6 <= probs[i] <= 1.0 and 0.0 <= stds[i] <= 0.3 for i in (1, 2)] + lines = [self._points(model.laneLines[i]) if good[i - 1] else None for i in (1, 2)] + if all(good): + if any(line is None for line in lines): + raise ValueError + valid_lines = [line for line in lines if line is not None] + samples = (lookahead * 0.25, lookahead * 0.5, lookahead) + widths = [] + for distance in samples: + left, right = (self._at(line, distance) for line in lines) + if left is None or right is None or not 2.6 <= right - left <= 4.8: + raise ValueError + slope = sum(float(np.interp(distance, x, np.gradient(y, x))) for x, y in valid_lines) * 0.5 + widths.append((right - left) / math.hypot(1.0, slope)) + width = float(np.median(widths)) + if not 2.6 <= width <= 4.8: + raise ValueError + self.width, self.width_at, self.width_travel = width, self.clock, self.travel + self._clear_line() + self.blocked = False + return TrackingTarget() + if not any(good): + self.path = None + if self.last_good_at is not None and self.clock - self.last_good_at > self.MAX_SOURCE_GAP: + self._clear_line() + return TrackingTarget(blocked=True) + side = 0 if good[0] else 1 + line = lines[side] + position = self._points(model.position) + uncertainty = self._points(model.position, uncertainty=True) + if line is None or position is None or uncertainty is None: + raise ValueError + x, y = line + slopes = np.gradient(y, x) + samples = (lookahead * 0.25, lookahead * 0.5, lookahead) + distances = [] + for distance in samples: + line_y, path_y, std = self._at(line, distance), self._at(position, distance), self._at(uncertainty, distance) + if line_y is None or path_y is None or std is None or not 0.0 <= std <= 0.35: + raise ValueError + signed = (path_y - line_y) * (1 if side == 0 else -1) + normal = signed / math.hypot(1.0, float(np.interp(distance, x, slopes))) + if not 0.5 <= normal <= 2.4: + raise ValueError + distances.append(normal) + near = self._at(line, samples[0]) + slope = float(np.interp(samples[0], x, slopes)) + gap = self.last_good_at is None or self.clock - self.last_good_at > self.MAX_SOURCE_GAP + jump = ( + self.side == side + and self.last_near is not None + and self.last_slope is not None + and (abs(near - self.last_near) > 0.35 or abs(slope - self.last_slope) > 0.12) + ) + if self.side != side or gap or jump: + self._clear_line() + self.side = side + self.acquired_at = self.clock + self.pending = max(self.MIN_CLEARANCE, float(np.median(distances))) + if ( + not jump + and self.width is not None + and self.clock - self.width_at <= self.WIDTH_MAX_AGE + and self.travel - self.width_travel <= self.WIDTH_MAX_DISTANCE + ): + self.spacing = self.width * 0.5 + elif self.spacing is None and self.clock - self.acquired_at >= self.ACQUIRE_SECONDS: + assert self.pending is not None + if abs(float(np.median(distances)) - self.pending) > 0.25: + self.acquired_at = self.clock + self.pending = max(self.MIN_CLEARANCE, float(np.median(distances))) + else: + self.spacing = self.pending + self.last_good_at, self.last_near, self.last_slope = self.clock, near, slope + self.blocked = jump + if self.spacing is None: + self.path = None + return TrackingTarget(blocked=jump) + signed_spacing = self.spacing if side == 0 else -self.spacing + norm = np.hypot(1.0, slopes) + target_x, target_y = x - signed_spacing * slopes / norm, y + signed_spacing / norm + if np.any(np.diff(target_x) <= 0): + raise ValueError + self.path = target_x, target_y + target = self._target(lookahead, offset) + model_y = self._at(position, lookahead) + if target.y is None or abs(target.y - model_y) > self.MAX_PATH_DISAGREEMENT: + raise ValueError + # An observed road edge vetoes a conflicting target. A distant edge is + # not interpreted as the missing lane line or a lane-width measurement. + for index in (0, 1): + if len(model.roadEdges) <= index or len(model.roadEdgeStds) <= index: + continue + std = float(model.roadEdgeStds[index]) + if not math.isfinite(std) or std < 0.0: + raise ValueError + if std > 0.3: + continue + edge = self._points(model.roadEdges[index]) + if edge is None: + raise ValueError + for distance in samples: + edge_y, target_y = self._at(edge, distance), self._at(self.path, distance) + if edge_y is None or target_y is None: + raise ValueError + clearance = (target_y - edge_y) if index == 0 else (edge_y - target_y) + if clearance < self.MIN_CLEARANCE + max(-0.3, min(0.3, offset)) * (1 if index == 1 else -1): + raise ValueError + return target + except (AttributeError, IndexError, TypeError, ValueError, OverflowError): + self._clear_line() + self.blocked = True + return TrackingTarget(blocked=True) + + def _target(self, lookahead: float, offset: float) -> TrackingTarget: + if self.path is None or self.blocked: + return TrackingTarget(blocked=self.blocked) + value = self._at(self.path, lookahead) + return TrackingTarget(None if value is None else value + max(-0.3, min(0.3, offset))) diff --git a/openpilot/starpilot/lateral/tests/test_global_lane_runtime.py b/openpilot/starpilot/lateral/tests/test_global_lane_runtime.py index 17d8588e6c..27b9f04383 100644 --- a/openpilot/starpilot/lateral/tests/test_global_lane_runtime.py +++ b/openpilot/starpilot/lateral/tests/test_global_lane_runtime.py @@ -106,6 +106,35 @@ def test_toyota_full_card_startup_admits_normal_lane_host_without_replay_opt_in( assert controls.lane_centering_host is not None +@pytest.mark.parametrize('identity', GENERIC_STEERING) +def test_actual_controller_paths_track_one_current_line_and_withdraw_for_fault(identity): + from openpilot.starpilot.lateral.tests.test_lane_centering import single + with OpenpilotPrefix(), mock.patch.dict(os.environ, {'SIMULATION': '1', 'REPLAY': '1', + 'LANE_CENTERING_REPLAY_RUNTIME': '0', 'AOL_REPLAY_RUNTIME': '0'}), \ + mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): + params = Params() + cp = native_steering_params(identity) + params.put('CarParams', cp.to_bytes(), block=True) + params.put_bool('LaneCentering', True, block=True) + params.put('LaneCenteringE2EAuthority', 0., block=True) + controls = Controls() + for tick in range(20): + feed(controls, 2_000_000_000 + tick * 10_000_000, tick) + controls.state_control() + with mock.patch('openpilot.starpilot.lateral.tests.test_lane_runtime.model', + side_effect=lambda:single(line=-1.2, edge=10.)): + for tick in range(20, 90): + feed(controls, 2_000_000_000 + tick * 10_000_000, tick) + command, _ = controls.state_control() + assert command.latActive + assert controls.last_lane_centering_result.correction > 0. + assert controls.lane_centering_applied > 0. + feed(controls, 2_900_000_000, 90, fault=True) + command, _ = controls.state_control() + assert not command.latActive + assert controls.lane_centering_applied == 0. + + def test_actual_secoc_configuration_requires_its_key_without_affecting_nonsecoc(): cp = interfaces[TOYOTA.TOYOTA_SIENNA_4TH_GEN].get_non_essential_params(TOYOTA.TOYOTA_SIENNA_4TH_GEN) assert cp.secOcRequired and not cp.secOcKeyAvailable diff --git a/openpilot/starpilot/lateral/tests/test_lane_centering.py b/openpilot/starpilot/lateral/tests/test_lane_centering.py index 109220fc36..bd7ec4d78a 100644 --- a/openpilot/starpilot/lateral/tests/test_lane_centering.py +++ b/openpilot/starpilot/lateral/tests/test_lane_centering.py @@ -8,6 +8,8 @@ from typing import Any, cast import unittest from unittest.mock import patch +import numpy as np + from openpilot.cereal import messaging from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature from openpilot.starpilot.lateral.lane_centering import (ControlMode, LaneCenteringController, LaneCenteringInput, @@ -438,3 +440,157 @@ class TestSingleBoundary(unittest.TestCase): self.assertEqual(faded.reason, 'signal_release') self.assertLess(faded.correction, prior) + +class TestSingleLineTracking(unittest.TestCase): + def test_spacing_preference_cannot_bypass_current_line_geometry_or_path_uncertainty(self): + m = single(line=-1.2, edge=10.) + m.laneLines[1].x = XS[:10] + m.laneLines[1].y = [-1.2] * 10 + self.assertIsNone(raw_correction(m, 20., 0., 0., _single_target=.6)) + m = single(line=-1.2, edge=10.) + m.position.yStd = [.36] * len(XS) + self.assertIsNone(raw_correction(m, 20., 0., 0., _single_target=.6)) + + def test_trusted_width_transfers_to_current_line_across_wide_road(self): + controller = LaneCenteringController() + for frame in range(10): + controller.update(observation(model(left=-1.8, right=1.8), model_sample_id=1_000_000_000 + frame * 50_000_000, + elapsed_seconds=.05)) + for frame in range(10, 30): + m = single(line=-1.2, edge=10.) + result = controller.update(observation(m.as_reader(), model_sample_id=1_000_000_000 + frame * 50_000_000, + elapsed_seconds=.05)) + self.assertEqual(result.reason, 'qualified') + self.assertGreater(result.correction, .0002) + assert controller._tracker.spacing is not None + self.assertAlmostEqual(controller._tracker.spacing, 1.8, places=6) + + def test_normal_offset_tracks_a_sloping_line_in_both_directions(self): + from openpilot.starpilot.lateral.lane_tracking import SingleLineTracker + for side in (0, 1): + tracker = SingleLineTracker() + for frame in range(10): + m = model(left=-1.8, right=1.8) + for index, intercept in ((1, -1.8), (2, 1.8)): + m.laneLines[index].y = [intercept + .15 * x for x in XS] + m.position.y = [.15 * x for x in XS] + tracker.target(m, frame, 20., .05, 0.) + m.laneLineProbs = [0., .95 if side == 0 else .05, .05 if side == 0 else .95, 0.] + for index, intercept in ((1, -1.8), (2, 1.8)): + m.laneLines[index].y = [intercept + .2 + .15 * x for x in XS] + target = tracker.target(m, 11, 20., .05, 0.) + self.assertFalse(target.blocked) + assert target.y is not None + self.assertAlmostEqual(target.y, 3.2, places=5) + + def test_new_line_needs_acquisition_and_jumps_discard_spacing(self): + from openpilot.starpilot.lateral.lane_tracking import SingleLineTracker + tracker = SingleLineTracker() + for frame in range(8): + target = tracker.target(single(line=-1.7, edge=10.), frame, 20., .05, 0.) + if frame < 4: + self.assertIsNone(target.y) + assert tracker.spacing is not None + self.assertAlmostEqual(tracker.spacing, 1.7, places=6) + jump = tracker.target(single(line=-1.1, edge=10.), 9, 20., .05, 0.) + self.assertTrue(jump.blocked) + self.assertIsNone(jump.y) + self.assertIsNone(tracker.spacing) + + def test_missing_sources_and_driver_override_cannot_keep_line_history(self): + controller = LaneCenteringController() + for frame in range(12): + source = observation(single(line=-1.7, edge=10.), model_sample_id=frame + 1, elapsed_seconds=.05) + controller.update(source) + self.assertIsNotNone(controller._tracker.spacing) + controller.update(replace(source, driver_override=True)) + self.assertIsNone(controller._tracker.spacing) + for frame in range(20, 32): + controller.update(replace(source, model_sample_id=frame)) + controller.update(replace(source, turn_signal_active=True, model_sample_id=33)) + self.assertIsNone(controller._tracker.spacing) + + def test_line_loss_and_reacquisition_remain_filtered_and_bounded(self): + controller = LaneCenteringController() + previous = 0. + for frame in range(100): + m = model(left=-1.8, right=1.8) if frame < 10 or frame >= 70 else single(line=-1.2, edge=10.) + if 45 <= frame < 50: + m.laneLineProbs = [0., .05, .05, 0.] + result = controller.update(observation(m, model_sample_id=frame + 1, elapsed_seconds=.05)) + self.assertLessEqual(abs(result.correction), MAX_CORRECTION) + self.assertLessEqual(abs(result.correction - previous), 2 * MAX_CORRECTION * (1 - math.exp(-.05 / .2)) + 1e-15) + if 45 <= frame < 50: + self.assertEqual(result.reason, 'unqualified_model') + self.assertLessEqual(abs(result.correction), abs(previous)) + previous = result.correction + + +class TestVehicleClearance(unittest.TestCase): + def test_final_command_guard_rejects_worsening_tracking_and_preserves_limits(self): + from openpilot.starpilot.lateral.lane_clearance import BodyDimensions + controller = LaneCenteringController(body=BodyDimensions(1.88, 4.855)) + m = single(edge=3.) + controller._correction = .001 + result = controller.guard_applied(m, .006, .007, .006, 20., .1) + self.assertEqual(result, .006) + self.assertEqual(controller._correction, 0.) + controller._tracker.path = (np.array(XS), np.full(len(XS), .3)) + self.assertEqual(controller.guard_applied(m, -.006, -.007, -.006, 20., .1), -.006) + + def test_commanded_curvature_is_checked_independently_of_model_position(self): + from openpilot.starpilot.lateral.lane_clearance import BodyDimensions, limit_correction + body = BodyDimensions(1.88, 4.855) + m = single(line=-1.8, edge=3.) + self.assertEqual(m.position.y[20], 0.) + # Same perceived path, different actual commanded motion: adding rightward + # steering to an already rightward command must not worsen edge intrusion. + self.assertEqual(limit_correction(m, .006, .001, .006, 20., .1, body), 0.) + self.assertEqual(limit_correction(m, .006, -.001, .006, 20., .1, body), -.001) + + def test_wide_clear_corridor_keeps_the_requested_contribution(self): + from openpilot.starpilot.lateral.lane_clearance import BodyDimensions, limit_correction + m = single(line=-1.8, edge=10.) + m.roadEdges[0].y = [-10.] * len(XS) + body = BodyDimensions(1.88, 4.855) + for correction in (-.001, .001): + self.assertEqual(limit_correction(m, 0., correction, 0., 20., .1, body), correction) + + def test_short_or_malformed_edge_does_not_gain_extrapolated_clearance(self): + from openpilot.starpilot.lateral.lane_clearance import BodyDimensions, limit_correction + for bad in ('short', 'nan', 'reversed', 'negative_std'): + with self.subTest(bad=bad): + m = single(edge=10.) + if bad == 'short': + m.roadEdges[1].x = XS[:5] + m.roadEdges[1].y = [10.] * 5 + elif bad == 'nan': + m.roadEdges[1].y = [math.nan] * len(XS) + elif bad == 'reversed': + m.roadEdges[1].x = list(reversed(XS)) + else: + m.roadEdgeStds = [.1, -.1] + self.assertEqual(limit_correction(m, 0., .001, 0., 20., .1, BodyDimensions(1.88, 4.855)), 0.) + + def test_body_guard_withdraws_extra_correction_without_changing_nominal_command(self): + from openpilot.starpilot.lateral.lane_clearance import BodyDimensions + controller = LaneCenteringController(body=BodyDimensions(1.88, 4.855)) + m = single(edge=10.) + m.roadEdges[0].y = [-10.] * len(XS) + for frame in range(30): + source = observation(m, model_sample_id=frame + 1, elapsed_seconds=.05, + measured_curvature=0., lateral_delay_s=.1) + result = controller.update(source) + self.assertGreater(result.correction, 0.) + refused = controller.update(replace(source, measured_curvature=None)) + self.assertEqual(refused.reason, 'clearance_limited') + self.assertEqual(refused.correction, 0.) + self.assertEqual(refused.candidate_curvature, source.base_curvature) + self.assertEqual(controller.update(replace(source, driver_override=True)).correction, 0.) + + def test_dimensions_are_explicit_and_invalid_values_are_rejected(self): + from openpilot.starpilot.lateral.lane_clearance import BodyDimensions, interface_dimensions + self.assertIsNone(interface_dimensions(object(), object())) + for width, length in ((True, 4.8), (1.8, math.nan), (0., 4.8), (1.8, 20.)): + self.assertFalse(BodyDimensions(width, length).valid) + diff --git a/openpilot/starpilot/lateral/tests/test_lane_host.py b/openpilot/starpilot/lateral/tests/test_lane_host.py index 76eb77b62b..b581943131 100644 --- a/openpilot/starpilot/lateral/tests/test_lane_host.py +++ b/openpilot/starpilot/lateral/tests/test_lane_host.py @@ -218,3 +218,86 @@ if __name__ == "__main__": probe(parser.parse_args().probe) else: unittest.main() + + +def run_tracking_plant(side, road_curvature, enabled): + import numpy as np + from openpilot.starpilot.lateral import lane_centering as lane + from openpilot.starpilot.lateral.lane_clearance import BodyDimensions + from openpilot.starpilot.lateral.tests import test_lane_centering as fixtures + from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature as clip + + controller = lane.LaneCenteringController(body=BodyDimensions(1.88, 4.855)) + speed, dt = 20.0, 0.01 + y = heading = previous = 0.0 + measured = road_curvature + delayed = [road_curvature] * 10 + samples = [] + for tick in range(1200): + if tick % 5 == 0: + nominal = road_curvature - 0.001 * (y - (-0.6 if side == 0 else 0.6)) - 0.08 * heading + m = fixtures.model(path_std=0.02) + for index, intercept in enumerate((-5.0, -1.8, 1.8, 5.0)): + m.laneLines[index].y = [intercept - y - heading * x + 0.5 * road_curvature * x * x for x in fixtures.XS] + m.position.y = [0.5 * nominal * x * x for x in fixtures.XS] + m.laneLineProbs = [0.0, 0.95 if tick < 100 or side == 0 else 0.05, 0.95 if tick < 100 or side == 1 else 0.05, 0.0] + m.init('roadEdges', 2) + for edge, intercept in zip(m.roadEdges, (-5.0, 5.0), strict=True): + edge.x = fixtures.XS + edge.y = [intercept - y - heading * x + 0.5 * road_curvature * x * x for x in fixtures.XS] + m.roadEdgeStds = [0.1, 0.1] + m.action.desiredCurvature = nominal + model = m.as_reader() + stamp = 1_000_000_000 + tick * 10_000_000 + nominal = model.action.desiredCurvature + desired = nominal + if enabled: + result = controller.update( + lane.LaneCenteringInput( + model, + stamp, + True, + nominal, + speed, + lane.ControlMode.LATERAL_ONLY, + True, + lane.LaneCenteringSettings(True, 0.0, 0.0, True), + dt, + measured_curvature=measured, + lateral_delay_s=0.1, + ) + ) + assert result.candidate_curvature is not None + desired = result.candidate_curvature + command, _ = clip(speed, previous, desired, 0.0) + baseline, _ = clip(speed, previous, nominal, 0.0) + if enabled: + command = controller.guard_applied(model, baseline, command, measured, speed, 0.1) + assert abs(command) <= 3.0 / speed**2 + 1e-12 + assert abs(command - previous) <= 5.0 / speed**2 * dt + 1e-12 + previous = command + delayed.append(command) + measured += (delayed.pop(0) - measured) * dt / 0.1 + road_speed = speed * math.cos(heading) / (1 - road_curvature * y) + y += speed * math.sin(heading) * dt + heading += (speed * measured - road_curvature * road_speed) * dt + samples.append((y, heading, command)) + return { + 'meanFinalAbsOffset': float(np.mean([abs(r[0]) for r in samples[-200:]])), + 'minimumLineClearance': min(1.8 - abs(r[0]) for r in samples), + 'maxAbsHeading': max(abs(r[1]) for r in samples), + 'finalOffset': y, + } + + + +class TestLaneTrackingPlant(unittest.TestCase): + def test_single_line_tracking_reduces_bias_with_kinematic_feedback(self): + for side in (0, 1): + for curvature in (0., .004, -.004): + with self.subTest(side=side, curvature=curvature): + original = run_tracking_plant(side, curvature, False) + candidate = run_tracking_plant(side, curvature, True) + self.assertLess(candidate['meanFinalAbsOffset'], original['meanFinalAbsOffset'] * .8) + self.assertGreater(candidate['minimumLineClearance'], original['minimumLineClearance']) + self.assertLessEqual(candidate['maxAbsHeading'], original['maxAbsHeading'] + .001) diff --git a/openpilot/starpilot/lateral/tests/test_lane_runtime.py b/openpilot/starpilot/lateral/tests/test_lane_runtime.py index 9ddec649b7..31e1f17f57 100644 --- a/openpilot/starpilot/lateral/tests/test_lane_runtime.py +++ b/openpilot/starpilot/lateral/tests/test_lane_runtime.py @@ -71,8 +71,12 @@ def publish_corolla_params(params): def feed(controls, timestamp, tick, *, active=True, enabled=True, fault=False, override=False, signal=False, model_age=0, can_valid=True, can_timeout=False, aol_lat_only=False, native_session='lane-aol-session', speed=20., - drive_id=500_000_000, native_allowed=True, independent=False): + drive_id=500_000_000, native_allowed=True, independent=False, delay_fast=False): events = [] + if not controls.sm.seen['lateralDelay']: + initial = messaging.new_message('lateralDelay', valid=True, logMonoTime=timestamp - 250_000_000) + initial.lateralDelay.lateralDelay = .1 + controls.sm.update_msgs((timestamp - 250_000_000) / 1e9, [initial.as_reader()]) def msg(service, size=None): event = messaging.new_message(service, size, valid=True, logMonoTime=timestamp) @@ -97,7 +101,8 @@ def feed(controls, timestamp, tick, *, active=True, enabled=True, fault=False, o lp = msg('vehicleParameters') lp.stiffnessFactor, lp.steerRatio = 1.0, controls.CP.steerRatio msg('longitudinalPlan').aTarget = 0.5 - msg('lateralDelay').lateralDelay = 0.1 + if tick % 25 == 0 or delay_fast: + msg('lateralDelay').lateralDelay = 0.1 msg('onroadEvents', 0) if tick % 5 == 0: event = messaging.new_message('modelV2', valid=True, logMonoTime=timestamp - model_age) @@ -135,6 +140,55 @@ def feed(controls, timestamp, tick, *, active=True, enabled=True, fault=False, o class LaneRuntimeTests(unittest.TestCase): + def test_body_dimensions_follow_selected_vehicle_without_retuning_geometry(self): + from openpilot.starpilot.lateral.lane_clearance import BodyDimensions, interface_dimensions + stock, tagged = ioniq_candidate() + for cp in (stock, tagged): + before = cp.to_dict() + self.assertEqual(interface_dimensions(HyundaiCarInterface, cp), BodyDimensions(1.88, 4.855)) + self.assertEqual(cp.to_dict(), before) + other = cp.as_reader().as_builder() + other.carFingerprint = HyundaiCAR.HYUNDAI_IONIQ_5 + self.assertIsNone(interface_dimensions(HyundaiCarInterface, other)) + self.assertIsNone(interface_dimensions(HondaCarInterface, stock)) + + def test_body_guard_rejects_incorrect_delay_rate_with_lateral_axis_still_active(self): + with OpenpilotPrefix(), mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0', 'REPLAY': '1', + 'AOL_REPLAY_RUNTIME': '0'}), \ + mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'): + params = Params() + _, tagged = ioniq_candidate() + params.put('CarParams', tagged.to_bytes(), block=True) + params.put_bool('LaneCentering', True, block=True) + params.put('LaneCenteringE2EAuthority', 0., block=True) + controls = Controls() + for tick in range(35): + feed(controls, 2_000_000_000 + tick * 10_000_000, tick) + command, _ = controls.state_control() + self.assertTrue(command.latActive) + self.assertGreater(controls.last_lane_centering_result.correction, 0.) + for tick in range(35, 45): + feed(controls, 2_000_000_000 + tick * 10_000_000, tick, delay_fast=True) + command, _ = controls.state_control() + self.assertTrue(command.latActive) + self.assertFalse(controls.sm.all_checks(['lateralDelay'])) + self.assertEqual(controls.last_lane_centering_result.correction, 0.) + self.assertEqual(controls.lane_centering_applied, 0.) + + def test_clearance_delay_uses_original_header_and_rejects_stale_or_future_source(self): + from openpilot.starpilot.lateral.lane_clearance import current_delay + frame = SimpleNamespace(logMonoTime={'lateralDelay':1_000_000_000}, all_checks=lambda services:True) + class Source: + logMonoTime = frame.logMonoTime + all_checks = staticmethod(frame.all_checks) + def __getitem__(self, service): + return SimpleNamespace(lateralDelay=.1) + source = Source() + self.assertEqual(current_delay(source, 1_250_000_000), .1) + self.assertIsNone(current_delay(source, 1_626_000_000)) + self.assertIsNone(current_delay(source, 999_999_999)) + self.assertEqual(source.logMonoTime['lateralDelay'], 1_000_000_000) + def test_steering_pause_original_timing_source_loss_and_units(self): from openpilot.starpilot.lateral.pause import LateralPause, PauseSettings, read_settings as read_pause state = SimpleNamespace(vEgo=0., leftBlinker=True, rightBlinker=False, canValid=True, canTimeout=False) @@ -350,6 +404,8 @@ class LaneRuntimeTests(unittest.TestCase): lateral_active=True, longitudinal_active=False)) params.put_bool('LaneCentering', True, block=True) controls = disabled + self.assertEqual(controls.lane_centering_controller._body.width, 1.88) + self.assertEqual(controls.lane_centering_controller._body.length, 4.855) self.assertIsNotNone(controls.lane_centering_host) for tick in range(35): now = 2_000_000_000 + tick * 10_000_000 diff --git a/prebuilt.json b/prebuilt.json index 3494a12031..e2cf36d950 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "0163f04e2bf1dcb4e8e5506975bb5595cccd28d4e825f890ea0de84ced601598", "schema_version": 1} +{"protected_inventory_sha256": "a62389cbb4d693d3eed387a5115464a5f17a69cc7619aca095e87762a3212195", "schema_version": 1}