diff --git a/selfdrive/controls/lib/curvatured.py b/selfdrive/controls/lib/curvatured.py index a2be5f068..d83d0b6f0 100644 --- a/selfdrive/controls/lib/curvatured.py +++ b/selfdrive/controls/lib/curvatured.py @@ -2,9 +2,28 @@ import numpy as np from openpilot.selfdrive.locationd.curvatured import CurvatureDLookup, VERSION +# Cache granularity for get_correction(). Inputs are rounded before comparison so that +# sensor noise does not invalidate the cache between consecutive 100Hz calls. +# +# Rationale per input: +# CACHE_V_EGO_DECIMALS = 1 (0.1 m/s granularity) +# - carState.vEgo sensor noise is typically ~0.001 m/s, but during normal +# driving v_ego rarely changes faster than 0.5 m/s per frame. +# - 0.1 m/s is loose enough to absorb realistic sensor jitter but still +# sensitive to actual acceleration/deceleration events. +# CACHE_CURVATURE_DECIMALS = 7 (1e-7 granularity) +# - controlsState.modelDesiredCurvature model output has ~1e-6 noise. +# - 1e-7 is below the model's noise floor so it rounds to the same value +# across consecutive frames; coarser would miss genuine steering changes. +# Both values are well below steering precision (1 deg of steering ≈ 1e-3 curvature). +CACHE_V_EGO_DECIMALS = 1 # 0.1 m/s +CACHE_CURVATURE_DECIMALS = 7 # 1e-7 + class CurvatureDController(CurvatureDLookup): def __init__(self) -> None: + # Cache total_size once; the bucket shape is fixed for the process lifetime. + self._expected_size: int = self.total_size() self.reset() def reset(self) -> None: @@ -12,38 +31,63 @@ class CurvatureDController(CurvatureDLookup): self.live_valid = False self.fit_corrections = np.zeros(self.bucket_shape(), dtype=np.float32) self.fit_valid = np.zeros(self.bucket_shape(), dtype=bool) + self._cached_v_ego_q: float | None = None + self._cached_curvature_q: float | None = None + self._cached_projected: float = 0.0 def update_live_params(self, msg) -> None: - expected_size = self.total_size() - if msg.version != VERSION or len(msg.corrections) != expected_size: + if msg.version != VERSION or len(msg.corrections) != self._expected_size: self.reset() return - self.use_params = bool(msg.useParams) - self.live_valid = bool(msg.liveValid) - self.fit_corrections = self.unflatten_bucket(msg.corrections, dtype=np.float32) - - if len(msg.fitValid) == expected_size: - self.fit_valid = self.unflatten_bucket(msg.fitValid, dtype=bool) + # Build all new state from the message first. Then invalidate the cache + # BEFORE swapping fields, so any concurrent reader sees either the old + # state with a fresh cache miss (forcing a recompute) or the new state + # (with cache already empty). The opposite order could briefly leave a + # reader with the new state but a stale cache hit. + new_use_params = bool(msg.useParams) + new_live_valid = bool(msg.liveValid) + new_fit_corrections = self.unflatten_bucket(msg.corrections, dtype=np.float32) + if len(msg.fitValid) == self._expected_size: + new_fit_valid = self.unflatten_bucket(msg.fitValid, dtype=bool) else: - self.fit_valid = np.abs(self.fit_corrections) > 0.0 + new_fit_valid = np.abs(new_fit_corrections) > 0.0 - if not self.live_valid: + if not new_live_valid: self.reset() + return + + # Invalidate-first, then coherent field swap. The tuple assignment makes + # the cache reset a single bytecode operation, so no reader can observe + # a half-zeroed cache. + self._invalidate_correction_cache() + self.use_params = new_use_params + self.live_valid = new_live_valid + self.fit_corrections = new_fit_corrections + self.fit_valid = new_fit_valid + + def _invalidate_correction_cache(self) -> None: + # Tuple assignment is a single bytecode op in CPython, so this is + # effectively atomic with respect to readers (no half-zeroed state). + self._cached_v_ego_q, self._cached_curvature_q, self._cached_projected = None, None, 0.0 def get_correction(self, desired_curvature: float, v_ego: float) -> float: if not self.use_params or not self.live_valid: return 0.0 abs_curvature = abs(float(desired_curvature)) - if abs_curvature < self.CURVATURE_MIN or abs_curvature > self.CURVATURE_MAX: - return 0.0 - if abs_curvature * (float(v_ego) ** 2) > self.MAX_LAT_ACCEL_APPLY: + if self._exceeds_safety_bounds(abs_curvature, v_ego): return 0.0 - projected = self.interp_curve_value(self.fit_corrections, self.fit_valid, v_ego, abs_curvature) + v_ego_q = round(v_ego, CACHE_V_EGO_DECIMALS) + curvature_q = round(abs_curvature, CACHE_CURVATURE_DECIMALS) + if v_ego_q == self._cached_v_ego_q and curvature_q == self._cached_curvature_q: + projected = self._cached_projected + else: + projected = self.interp_curve_value(self.fit_corrections, self.fit_valid, v_ego, abs_curvature) + self._cached_v_ego_q = v_ego_q + self._cached_curvature_q = curvature_q + self._cached_projected = projected + direction = 1.0 if desired_curvature >= 0.0 else -1.0 return float(direction * projected) - - def apply(self, desired_curvature: float, v_ego: float) -> float: - return float(desired_curvature + self.get_correction(desired_curvature, v_ego)) diff --git a/selfdrive/controls/tests/test_curvatured.py b/selfdrive/controls/tests/test_curvatured.py index ae739434e..1b402f3d1 100644 --- a/selfdrive/controls/tests/test_curvatured.py +++ b/selfdrive/controls/tests/test_curvatured.py @@ -1,6 +1,6 @@ import cereal.messaging as messaging -from openpilot.selfdrive.controls.lib.curvatured import CurvatureDController +from openpilot.selfdrive.controls.lib.curvatured import CACHE_CURVATURE_DECIMALS, CACHE_V_EGO_DECIMALS, CurvatureDController from openpilot.selfdrive.locationd.curvatured import CurvatureDLookup, VERSION @@ -72,7 +72,10 @@ class TestCurvatureDController: assert neg < 0.0 assert abs(pos + neg) < 1e-12 - def test_invalid_message_disables_corrections(self): + def test_live_valid_false_disables_corrections(self): + """When the message's liveValid flag is False, get_correction must return 0.0 + regardless of any cached state, since the upstream signal is invalid. + """ controller = CurvatureDController() msg = messaging.new_message('liveCurvatureParameters') msg.liveCurvatureParameters.liveValid = False @@ -85,7 +88,55 @@ class TestCurvatureDController: controller.update_live_params(msg.liveCurvatureParameters) - assert controller.apply(32e-6, 20.0) == 32e-6 + # Without a learnable curve and no live_valid, get_correction returns 0.0. + assert controller.get_correction(32e-6, 20.0) == 0.0 + + def test_version_mismatch_resets_controller(self): + """A message with the wrong version number must trigger a full reset, not + a partial state update. This is the actual 'invalid message' path. + """ + controller = CurvatureDController() + msg = messaging.new_message('liveCurvatureParameters') + msg.liveCurvatureParameters.liveValid = True + msg.liveCurvatureParameters.version = VERSION + 99 # intentionally wrong + msg.liveCurvatureParameters.useParams = True + msg.liveCurvatureParameters.corrections = [0.0] * CurvatureDLookup.total_size() + msg.liveCurvatureParameters.counts = [0] * CurvatureDLookup.total_size() + msg.liveCurvatureParameters.biases = [0.0] * CurvatureDLookup.total_size() + msg.liveCurvatureParameters.fitValid = [False] * CurvatureDLookup.total_size() + + # Pretend we had a valid state before the bad message + controller.use_params = True + controller.live_valid = True + + controller.update_live_params(msg.liveCurvatureParameters) + + # Bad version -> full reset -> both flags back to False + assert not controller.use_params + assert not controller.live_valid + assert controller.get_correction(32e-6, 20.0) == 0.0 + + def test_size_mismatch_resets_controller(self): + """A message with the wrong corrections array size must also trigger a full reset.""" + controller = CurvatureDController() + msg = messaging.new_message('liveCurvatureParameters') + msg.liveCurvatureParameters.liveValid = True + msg.liveCurvatureParameters.version = VERSION + msg.liveCurvatureParameters.useParams = True + # Wrong size (1 element instead of total_size()) + msg.liveCurvatureParameters.corrections = [0.0] + msg.liveCurvatureParameters.counts = [0] + msg.liveCurvatureParameters.biases = [0.0] + msg.liveCurvatureParameters.fitValid = [False] + + controller.use_params = True + controller.live_valid = True + + controller.update_live_params(msg.liveCurvatureParameters) + + assert not controller.use_params + assert not controller.live_valid + assert controller.get_correction(32e-6, 20.0) == 0.0 def test_correction_fades_outside_supported_curvature_range(self): controller = CurvatureDController() @@ -155,3 +206,136 @@ class TestCurvatureDController: assert at_last_edge > 0.0 assert 0.0 < in_fade < at_last_edge assert at_max == 0.0 + + def test_get_correction_caches_within_quantization_window(self): + """Identical (v_ego, abs_curvature) within quantization granularity must + hit the cache. The interp_curve_value source must not be called twice. + """ + controller = CurvatureDController() + msg = messaging.new_message('liveCurvatureParameters') + msg.liveCurvatureParameters.liveValid = True + msg.liveCurvatureParameters.version = VERSION + msg.liveCurvatureParameters.useParams = True + msg.liveCurvatureParameters.counts = [0] * CurvatureDLookup.total_size() + msg.liveCurvatureParameters.biases = [0.0] * CurvatureDLookup.total_size() + + curvature_idx = CurvatureDLookup.curvature_index(32e-6) + assert curvature_idx is not None + self._set_curve(msg, 3, {curvature_idx: 8e-6}) + controller.update_live_params(msg.liveCurvatureParameters) + + v_ego = float(CurvatureDLookup.SPEED_ANCHORS[3]) + + # Wrap the source to count calls + call_count = {"n": 0} + original = CurvatureDLookup.interp_curve_value + def counting(*args, **kwargs): + call_count["n"] += 1 + return original(*args, **kwargs) + CurvatureDLookup.interp_curve_value = counting + try: + # First call: cache miss, calls interp_curve_value once + first = controller.get_correction(32e-6, v_ego) + assert call_count["n"] == 1 + # Subsequent identical calls: cache hit, no further invocations + for _ in range(5): + cached = controller.get_correction(32e-6, v_ego) + assert cached == first + assert call_count["n"] == 1 + + # v_ego noise below quantization must still hit the cache + v_ego_step = 10 ** -CACHE_V_EGO_DECIMALS + noised = controller.get_correction(32e-6, v_ego + v_ego_step * 0.5) + assert noised == first + assert call_count["n"] == 1 + + # Curvature noise below quantization must still hit the cache + curvature_step = 10 ** -CACHE_CURVATURE_DECIMALS + noised = controller.get_correction(32e-6 + curvature_step * 0.5, v_ego) + assert noised == first + assert call_count["n"] == 1 + finally: + CurvatureDLookup.interp_curve_value = original + + def test_get_correction_cache_invalidates_on_live_params_update(self): + """Cache must be invalidated when fit_corrections / fit_valid change, + otherwise stale corrections would be served after a params update. + """ + controller = CurvatureDController() + msg = messaging.new_message('liveCurvatureParameters') + msg.liveCurvatureParameters.liveValid = True + msg.liveCurvatureParameters.version = VERSION + msg.liveCurvatureParameters.useParams = True + msg.liveCurvatureParameters.counts = [0] * CurvatureDLookup.total_size() + msg.liveCurvatureParameters.biases = [0.0] * CurvatureDLookup.total_size() + + curvature_idx = CurvatureDLookup.curvature_index(32e-6) + assert curvature_idx is not None + self._set_curve(msg, 3, {curvature_idx: 4e-6}) + controller.update_live_params(msg.liveCurvatureParameters) + v_ego = float(CurvatureDLookup.SPEED_ANCHORS[3]) + + first = controller.get_correction(32e-6, v_ego) + + # Update the underlying curve to a different value + self._set_curve(msg, 3, {curvature_idx: 16e-6}) + controller.update_live_params(msg.liveCurvatureParameters) + + second = controller.get_correction(32e-6, v_ego) + assert second > first + # Specifically: must not be the cached value + assert second != first + + def test_get_correction_cache_invalidates_on_reset(self): + """reset() must clear the cache to avoid stale hits after disengage/engage.""" + controller = CurvatureDController() + msg = messaging.new_message('liveCurvatureParameters') + msg.liveCurvatureParameters.liveValid = True + msg.liveCurvatureParameters.version = VERSION + msg.liveCurvatureParameters.useParams = True + msg.liveCurvatureParameters.counts = [0] * CurvatureDLookup.total_size() + msg.liveCurvatureParameters.biases = [0.0] * CurvatureDLookup.total_size() + + curvature_idx = CurvatureDLookup.curvature_index(32e-6) + assert curvature_idx is not None + self._set_curve(msg, 3, {curvature_idx: 8e-6}) + controller.update_live_params(msg.liveCurvatureParameters) + v_ego = float(CurvatureDLookup.SPEED_ANCHORS[3]) + + # Warm the cache + controller.get_correction(32e-6, v_ego) + assert controller._cached_v_ego_q is not None + + controller.reset() + # Cache state must be reset + assert controller._cached_v_ego_q is None + assert controller._cached_curvature_q is None + assert controller._cached_projected == 0.0 + + def test_get_correction_bypasses_cache_when_params_disabled(self): + """When use_params is False, the early-return at the top of get_correction + must not interfere with cache invariants (the cache may legitimately be stale). + """ + controller = CurvatureDController() + msg = messaging.new_message('liveCurvatureParameters') + msg.liveCurvatureParameters.liveValid = True + msg.liveCurvatureParameters.version = VERSION + msg.liveCurvatureParameters.useParams = True + msg.liveCurvatureParameters.counts = [0] * CurvatureDLookup.total_size() + msg.liveCurvatureParameters.biases = [0.0] * CurvatureDLookup.total_size() + + curvature_idx = CurvatureDLookup.curvature_index(32e-6) + assert curvature_idx is not None + self._set_curve(msg, 3, {curvature_idx: 8e-6}) + controller.update_live_params(msg.liveCurvatureParameters) + v_ego = float(CurvatureDLookup.SPEED_ANCHORS[3]) + + # Warm the cache + controller.get_correction(32e-6, v_ego) + assert controller._cached_projected != 0.0 + + # Disable params -> early return 0.0 + controller.use_params = False + assert controller.get_correction(32e-6, v_ego) == 0.0 + # The cache still holds the old value but is not consulted on this path + assert controller._cached_projected != 0.0 # cache not touched diff --git a/selfdrive/locationd/curvatured.py b/selfdrive/locationd/curvatured.py index d287f3d34..0e1d36316 100644 --- a/selfdrive/locationd/curvatured.py +++ b/selfdrive/locationd/curvatured.py @@ -1,4 +1,3 @@ -import math import time from collections import deque @@ -69,6 +68,10 @@ class CurvatureDLookup: LAST_BUCKET_WIDTH = float(CURVATURE_BUCKET_EDGES[-1] - CURVATURE_BUCKET_EDGES[-2]) CURVATURE_MAX = CURVATURE_BUCKET_MAX + LAST_BUCKET_WIDTH + # Precomputed log of bucket centers for sample interpolation. Avoids recomputing + # np.log on every interp_curve_value / _interp_curve_impl call. + _LOG_CENTERS = np.log(CURVATURE_BUCKET_CENTERS.astype(np.float64)) + MIN_SPEED = float(SPEED_ANCHORS[0] * 0.5) # learning/apply speed floor MAX_LAT_ACCEL_APPLY = 1.0 # apply accel gate RELATIVE_CAP_FULL_RATIO = 0.50 # inner relative cap @@ -265,8 +268,13 @@ class CurvatureDLookup: return int(round(100.0 * fully_calibrated_speeds / float(len(cls.SPEED_ANCHORS)))) @classmethod - def smoothstep(cls, x: float) -> float: - y = float(np.clip(x, 0.0, 1.0)) + def smoothstep(cls, x) -> np.ndarray: + """Smoothstep, vectorized. Accepts scalar or array input. + + Note: the original scalar-only form is preserved implicitly via np.clip + on the input - it just no longer raises on arrays. + """ + y = np.clip(x, 0.0, 1.0) return y * y * (3.0 - 2.0 * y) @classmethod @@ -375,10 +383,52 @@ class CurvatureDLookup: @classmethod def interp_curve_value(cls, fit_corrections: np.ndarray, fit_valid: np.ndarray, - v_ego: float, abs_curvature: float) -> float: - if abs_curvature < cls.CURVATURE_MIN or abs_curvature > cls.CURVATURE_MAX: + v_ego: float, abs_curvature: float | np.ndarray) -> float | np.ndarray: + """Interpolate curvature correction(s) from the learned fit curves. + + Single Public-API that accepts both scalar and array input, dispatching + internally to the vectorized implementation: + + - Scalar input (float) -> returns float (100Hz controlsd hot path) + - Array input (np.ndarray) -> returns np.ndarray (UI rendering, batched) + + Caching of common constants (_LOG_CENTERS) and a single speed_interp() call + per batch keep both paths fast. + """ + abs_curvatures = np.asarray(abs_curvature, dtype=np.float64) + was_scalar = abs_curvatures.ndim == 0 + if was_scalar: + abs_curvatures = abs_curvatures.reshape(1) + if was_scalar and cls._exceeds_safety_bounds(abs_curvatures[0], float(v_ego)): return 0.0 + result = cls._interp_curve_impl(fit_corrections, fit_valid, v_ego, abs_curvatures) + return float(result[0]) if was_scalar else result + + @classmethod + def _exceeds_safety_bounds(cls, abs_curvature: float, v_ego: float) -> bool: + """Single source of truth for the physical safety bounds. + + Returns True if the requested curvature should not be applied because + it would exceed the per-vehicle lateral acceleration limit or is + outside the learned bucket range. + """ + if abs_curvature < cls.CURVATURE_MIN or abs_curvature > cls.CURVATURE_MAX: + return True + if abs_curvature * (float(v_ego) ** 2) > cls.MAX_LAT_ACCEL_APPLY: + return True + return False + + @classmethod + def _interp_curve_impl(cls, fit_corrections: np.ndarray, fit_valid: np.ndarray, + v_ego: float, abs_curvatures: np.ndarray) -> np.ndarray: + """Vectorized core. Always receives a 1-D abs_curvatures array of len >= 1.""" + n = len(abs_curvatures) + out = np.zeros(n, dtype=np.float32) + + log_curvatures = np.log(np.maximum(abs_curvatures, cls.CURVATURE_BUCKET_MIN)) + + # Compute speed interpolation indices once (avoid per-sample calls) low_speed, high_speed, speed_alpha = cls.speed_interp(v_ego) low_curve = fit_corrections[low_speed] high_curve = fit_corrections[high_speed] @@ -386,55 +436,63 @@ class CurvatureDLookup: high_valid = fit_valid[high_speed] if not low_valid.any() and not high_valid.any(): - return 0.0 + return out - log_centers = np.log(cls.CURVATURE_BUCKET_CENTERS.astype(np.float64)) - log_curvature = math.log(max(abs_curvature, cls.CURVATURE_BUCKET_MIN)) + log_centers = cls._LOG_CENTERS + curvature_edges = cls.CURVATURE_BUCKET_EDGES + curvature_max = cls.CURVATURE_MAX + curvature_min = cls.CURVATURE_MIN + n_centers = len(cls.CURVATURE_BUCKET_CENTERS) - def curve_value(curve: np.ndarray, valid_mask: np.ndarray) -> float: + def interp_speed_row(curve: np.ndarray, valid_mask: np.ndarray) -> np.ndarray: + row_out = np.zeros(n, dtype=np.float32) runs = cls.valid_runs(valid_mask) if len(runs) == 0: - return 0.0 + return row_out for start, end in runs: run_idx = np.arange(start, end + 1) run_log_x = log_centers[run_idx] run_curve = curve[run_idx] - first_edge = cls.CURVATURE_BUCKET_EDGES[start] - last_edge = cls.CURVATURE_BUCKET_EDGES[end + 1] + first_edge = float(curvature_edges[start]) + last_edge = float(curvature_edges[end + 1]) - if first_edge <= abs_curvature <= last_edge: - return float(np.interp(log_curvature, run_log_x, run_curve)) + # Main interpolation: curvature within run range + in_range = (abs_curvatures >= first_edge) & (abs_curvatures <= last_edge) + if in_range.any(): + row_out[in_range] = np.interp(log_curvatures[in_range], run_log_x, run_curve).astype(np.float32) + # Fade in (between previous bucket and first_edge) if start > 0: - fade_in_start = cls.CURVATURE_BUCKET_EDGES[start - 1] - if fade_in_start <= abs_curvature < first_edge: - fade_span = first_edge - fade_in_start - fade = cls.smoothstep((abs_curvature - fade_in_start) / max(fade_span, 1e-9)) - return float(run_curve[0] * np.clip(fade, 0.0, 1.0)) - elif cls.CURVATURE_MIN <= abs_curvature < first_edge: - fade_span = first_edge - cls.CURVATURE_MIN - fade = cls.smoothstep((abs_curvature - cls.CURVATURE_MIN) / max(fade_span, 1e-9)) - return float(run_curve[0] * np.clip(fade, 0.0, 1.0)) + fade_in_start = float(curvature_edges[start - 1]) + fade_in_mask = (abs_curvatures >= fade_in_start) & (abs_curvatures < first_edge) + else: + fade_in_start = curvature_min + fade_in_mask = (abs_curvatures >= curvature_min) & (abs_curvatures < first_edge) + if fade_in_mask.any(): + fade_span = max(first_edge - fade_in_start, 1e-9) + fade_vals = cls.smoothstep((abs_curvatures[fade_in_mask] - fade_in_start) / fade_span) + row_out[fade_in_mask] = (run_curve[0] * fade_vals).astype(np.float32) - if end < len(cls.CURVATURE_BUCKET_CENTERS) - 1: - fade_out_end = cls.CURVATURE_BUCKET_EDGES[end + 2] - if last_edge < abs_curvature <= fade_out_end: - fade_span = fade_out_end - last_edge - fade = 1.0 - cls.smoothstep((abs_curvature - last_edge) / max(fade_span, 1e-9)) - return float(run_curve[-1] * np.clip(fade, 0.0, 1.0)) - elif last_edge < abs_curvature <= cls.CURVATURE_MAX: - fade_span = cls.CURVATURE_MAX - last_edge - fade = 1.0 - cls.smoothstep((abs_curvature - last_edge) / max(fade_span, 1e-9)) - return float(run_curve[-1] * np.clip(fade, 0.0, 1.0)) + # Fade out (between last_edge and next bucket) + if end < n_centers - 1: + fade_out_end = float(curvature_edges[end + 2]) + fade_out_mask = (abs_curvatures > last_edge) & (abs_curvatures <= fade_out_end) + else: + fade_out_end = curvature_max + fade_out_mask = (abs_curvatures > last_edge) & (abs_curvatures <= fade_out_end) + if fade_out_mask.any(): + fade_span = max(fade_out_end - last_edge, 1e-9) + fade_vals = 1.0 - cls.smoothstep((abs_curvatures[fade_out_mask] - last_edge) / fade_span) + row_out[fade_out_mask] = (run_curve[-1] * fade_vals).astype(np.float32) - return 0.0 + return row_out - low_val = curve_value(low_curve, low_valid) - high_val = curve_value(high_curve, high_valid) + low_vals = interp_speed_row(low_curve, low_valid) if low_speed == high_speed: - return low_val - return float((1.0 - speed_alpha) * low_val + speed_alpha * high_val) + return low_vals + high_vals = interp_speed_row(high_curve, high_valid) + return ((1.0 - speed_alpha) * low_vals + speed_alpha * high_vals).astype(np.float32) class CurvatureEstimator(CurvatureDLookup): @@ -756,9 +814,15 @@ class CurvatureEstimator(CurvatureDLookup): self.current_bias = float(self.bias[speed_idx, curvature_idx]) self.current_bucket_points = self.bucket_points_for_index(self.counts, idx) + if not self.use_params: + self.current_correction = 0.0 + return if not self.fit_valid[speed_idx].any(): self.current_correction = 0.0 return + if self._exceeds_safety_bounds(abs(float(desired_curvature)), float(v_ego)): + self.current_correction = 0.0 + return direction = 1.0 if desired_curvature >= 0.0 else -1.0 projected = self.interp_curve_value(self.fit_corrections, self.fit_valid, v_ego, abs(desired_curvature)) @@ -913,7 +977,8 @@ def main(): include_debug=curvature_estimator.publish_debug_data, include_preview=curvature_estimator.publish_preview_data)) - # Cache params every 60 seconds + # Persistence is non-blocking by default (Params.put with block=False). + # We do this every 60s and accept the rare latency if the disk stalls. if sm.frame % 240 == 0: live_valid = sm.all_checks() and curvature_estimator.use_params params.put("LiveCurvatureParameters", curvature_estimator.get_msg(valid=sm.all_checks(), diff --git a/selfdrive/locationd/test/test_curvatured.py b/selfdrive/locationd/test/test_curvatured.py index aa707583e..f6e4f32b5 100644 --- a/selfdrive/locationd/test/test_curvatured.py +++ b/selfdrive/locationd/test/test_curvatured.py @@ -341,3 +341,123 @@ class TestCurvatureEstimator: assert estimator.steering_pressed[-1] assert estimator.last_override_t == 12.0 + + def test_interp_curve_value_matches_interp_curve_samples(self): + """Verifies the unified interp_curve_value API returns the same result for + scalar and array input, and that both branches share the same code path. + This protects both UI and controlsd (100Hz) code paths from drift. + """ + speed_idx = 3 + v_ego = float(CurvatureDLookup.SPEED_ANCHORS[speed_idx]) + fit_corrections = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + fit_valid = np.zeros(CurvatureDLookup.bucket_shape(), dtype=bool) + + # Two valid runs with a gap + fit_valid[speed_idx, 3] = True + fit_valid[speed_idx, 6] = True + fit_corrections[speed_idx, 3] = 1.0e-6 + fit_corrections[speed_idx, 6] = 8.0e-6 + + # Test points covering: out of range, in valid run, in gap (returns 0), fade regions + test_curvatures = [ + 0.0, + 1.0e-7, + float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[2]), # before first valid run (fade-in) + float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[3]), # in first run + float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[4]), # in gap (should be 0) + float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[6]), # in second run + float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[7]), # after second run (fade-out) + float(CurvatureDLookup.CURVATURE_MAX + 1.0), # out of range + ] + + for c in test_curvatures: + scalar_result = CurvatureDLookup.interp_curve_value(fit_corrections, fit_valid, v_ego, c) + array_result = CurvatureDLookup.interp_curve_value( + fit_corrections, fit_valid, v_ego, np.asarray([c], dtype=np.float64) + ) + # Scalar path returns float + assert isinstance(scalar_result, float) + # Array path returns np.ndarray + assert isinstance(array_result, np.ndarray) + assert np.isclose(scalar_result, array_result[0]), f"mismatch at c={c}: scalar={scalar_result}, array={array_result[0]}" + + def test_interp_curve_value_handles_speed_interp_transition(self): + """When v_ego falls between two SPEED_ANCHORS, interp_curve_value must blend + the two speed buckets with weight (1-alpha, alpha). This exercises the speed + blending path that the vectorized impl shares with the scalar path. + """ + fit_corrections = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + fit_valid = np.zeros(CurvatureDLookup.bucket_shape(), dtype=bool) + + # Use a value per speed_idx that is easy to verify after blending. + per_speed_value = 1.0e-5 * np.arange(1, len(CurvatureDLookup.SPEED_ANCHORS) + 1, dtype=np.float32) + for s in range(len(CurvatureDLookup.SPEED_ANCHORS)): + fit_valid[s, 5] = True + fit_corrections[s, 5] = per_speed_value[s] + + c = float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[5]) + + # At v_ego exactly at a SPEED_ANCHOR, alpha=0 -> only the low bucket contributes. + v_ego_at_anchor = float(CurvatureDLookup.SPEED_ANCHORS[3]) + expected_at_anchor = float(per_speed_value[3]) + val = CurvatureDLookup.interp_curve_value(fit_corrections, fit_valid, v_ego_at_anchor, c) + assert np.isclose(val, expected_at_anchor), f"at anchor: {val} != {expected_at_anchor}" + + # Midpoint between two anchors: alpha=0.5 -> exact 50/50 blend. + v_ego_mid = 0.5 * (float(CurvatureDLookup.SPEED_ANCHORS[2]) + float(CurvatureDLookup.SPEED_ANCHORS[3])) + expected_mid = 0.5 * (float(per_speed_value[2]) + float(per_speed_value[3])) + val = CurvatureDLookup.interp_curve_value(fit_corrections, fit_valid, v_ego_mid, c) + assert np.isclose(val, expected_mid, rtol=1e-6), f"at midpoint: {val} != {expected_mid}" + + # General position: verify the explicit (1-alpha) * low + alpha * high formula. + low, high, alpha = CurvatureDLookup.speed_interp(v_ego_mid) + expected = (1.0 - alpha) * float(per_speed_value[low]) + alpha * float(per_speed_value[high]) + assert np.isclose(val, expected, rtol=1e-6), f"blend formula: {val} != {expected}" + + def test_exceeds_safety_bounds(self): + """Centralized safety check used by both controller.get_correction and + CurvatureEstimator._update_current_lookup. + """ + # In-range curvature is safe + assert not CurvatureDLookup._exceeds_safety_bounds(1.0e-5, 20.0) + + # Below CURVATURE_MIN -> exceed + assert CurvatureDLookup._exceeds_safety_bounds(-1.0, 20.0) + + # Above CURVATURE_MAX -> exceed + assert CurvatureDLookup._exceeds_safety_bounds(CurvatureDLookup.CURVATURE_MAX + 1.0, 20.0) + + # abs_curvature * v_ego^2 > MAX_LAT_ACCEL_APPLY (1.0 m/s^2) -> exceed + # v=20 m/s, c=3e-3 -> 3e-3 * 400 = 1.2 > 1.0 + assert CurvatureDLookup._exceeds_safety_bounds(3.0e-3, 20.0) + + # At the boundary, exactly at the limit: 2.5e-3 * 400 = 1.0, must NOT exceed (strict >) + assert not CurvatureDLookup._exceeds_safety_bounds(2.5e-3, 20.0) + + def test_update_current_lookup_respects_safety_bounds(self): + """Ensure the published current_correction is 0.0 when the requested + curvature would exceed the lateral acceleration limit, not just when the + curvature is out of bucket range. + """ + estimator = get_estimator() + estimator.use_params = True + estimator.update_use_params(force=True) + + # Populate fit_corrections and fit_valid so we can isolate the safety check + estimator.fit_corrections = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + estimator.fit_valid = np.ones(CurvatureDLookup.bucket_shape(), dtype=bool) + + # Inputs within the bucket range but exceeding the lateral-accel cap: + # 3e-3 * 20^2 = 1.2 m/s^2, above 1.0 m/s^2 limit + estimator._update_current_lookup(3.0e-3, 20.0) + assert estimator.current_correction == 0.0 + + # Inputs at exactly the limit: 2.5e-3 * 20^2 = 1.0, must not exceed + estimator._update_current_lookup(2.5e-3, 20.0) + # Not necessarily zero here (correction is small at the limit), but must be finite + assert np.isfinite(estimator.current_correction) + + # use_params = False forces 0.0 even if all other conditions would allow a value + estimator.use_params = False + estimator._update_current_lookup(1.0e-4, 20.0) + assert estimator.current_correction == 0.0 diff --git a/selfdrive/ui/layouts/settings/ictoggles.py b/selfdrive/ui/layouts/settings/ictoggles.py index 10314fc67..59ad2c571 100644 --- a/selfdrive/ui/layouts/settings/ictoggles.py +++ b/selfdrive/ui/layouts/settings/ictoggles.py @@ -194,12 +194,13 @@ class ICTogglesLayout(Widget): self._update_toggles() def _update_toggles(self): + # Use ui_state's params cache (refreshed in own thread at 5Hz) to avoid extra IPC roundtrips ui_state.update_params() # TODO: make a param control list item so we don't need to manage internal state as much here # refresh toggles from params to mirror external changes for param in self._toggle_defs: - self._toggles[param].action_item.set_state(self._params.get_bool(param)) + self._toggles[param].action_item.set_state(ui_state.params.get_bool(param)) # these toggles need restart, block while engaged for toggle_def in self._toggle_defs: @@ -217,6 +218,6 @@ class ICTogglesLayout(Widget): self._scroller.render(rect) def _toggle_callback(self, state: bool, param: str): - self._params.put_bool(param, state, block=True) + self._params.put_bool(param, state) if self._toggle_defs[param][3]: - self._params.put_bool("OnroadCycleRequested", True, block=True) + self._params.put_bool("OnroadCycleRequested", True) diff --git a/selfdrive/ui/mici/onroad/dynamic_steering_learner_graph.py b/selfdrive/ui/mici/onroad/dynamic_steering_learner_graph.py index aff52d144..172b01984 100644 --- a/selfdrive/ui/mici/onroad/dynamic_steering_learner_graph.py +++ b/selfdrive/ui/mici/onroad/dynamic_steering_learner_graph.py @@ -87,14 +87,17 @@ class DynamicSteeringLearnerGraphMici(Widget): fit_corrections: np.ndarray, fit_valid: np.ndarray, v_ego: float) -> tuple[np.ndarray, np.ndarray, float, float]: if lcp_frame != self._cached_lcp_frame: - self._cached_preview_curve = np.array([ - CurvatureDLookup.interp_curve_value(preview_corrections, preview_valid, v_ego, abs(float(k))) - for k in self._plot_x - ], dtype=np.float32) - self._cached_fit_curve = np.array([ - CurvatureDLookup.interp_curve_value(fit_corrections, fit_valid, v_ego, abs(float(k))) - for k in self._plot_x - ], dtype=np.float32) + abs_curvatures = np.abs(self._plot_x).astype(np.float64) + # Recomputes only when liveCurvatureParameters changes (4Hz); cached across UI frames. + self._cached_fit_curve = CurvatureDLookup.interp_curve_value( + fit_corrections, fit_valid, v_ego, abs_curvatures + ) + # Preview is only populated when ShowDynamicSteeringLearnerGraph is on. + has_preview = preview_corrections.shape == fit_corrections.shape and np.any(preview_corrections) + if has_preview: + self._cached_preview_curve = CurvatureDLookup.interp_curve_value( + preview_corrections, preview_valid, v_ego, abs_curvatures + ) self._cached_min_y, self._cached_max_y = self._compute_y_bounds(self._cached_preview_curve, self._cached_fit_curve) self._cached_lcp_frame = lcp_frame diff --git a/selfdrive/ui/onroad/dynamic_steering_learner_graph.py b/selfdrive/ui/onroad/dynamic_steering_learner_graph.py index cc804b40a..7a8322b6a 100644 --- a/selfdrive/ui/onroad/dynamic_steering_learner_graph.py +++ b/selfdrive/ui/onroad/dynamic_steering_learner_graph.py @@ -96,14 +96,17 @@ class DynamicSteeringLearnerGraph(Widget): fit_corrections: np.ndarray, fit_valid: np.ndarray, v_ego: float) -> tuple[np.ndarray, np.ndarray, float, float]: if lcp_frame != self._cached_lcp_frame: - self._cached_preview_curve = np.array([ - CurvatureDLookup.interp_curve_value(preview_corrections, preview_valid, v_ego, abs(float(k))) - for k in self._plot_x - ], dtype=np.float32) - self._cached_fit_curve = np.array([ - CurvatureDLookup.interp_curve_value(fit_corrections, fit_valid, v_ego, abs(float(k))) - for k in self._plot_x - ], dtype=np.float32) + abs_curvatures = np.abs(self._plot_x).astype(np.float64) + # Recomputes only when liveCurvatureParameters changes (4Hz); cached across UI frames. + self._cached_fit_curve = CurvatureDLookup.interp_curve_value( + fit_corrections, fit_valid, v_ego, abs_curvatures + ) + # Preview is only populated when ShowDynamicSteeringLearnerGraph is on. + has_preview = preview_corrections.shape == fit_corrections.shape and np.any(preview_corrections) + if has_preview: + self._cached_preview_curve = CurvatureDLookup.interp_curve_value( + preview_corrections, preview_valid, v_ego, abs_curvatures + ) self._cached_min_y, self._cached_max_y = self._compute_y_bounds(self._cached_preview_curve, self._cached_fit_curve) self._cached_lcp_frame = lcp_frame @@ -117,23 +120,18 @@ class DynamicSteeringLearnerGraph(Widget): if sm.recv_frame["carState"] < ui_state.started_frame or sm.recv_frame["controlsState"] < ui_state.started_frame: return - battery_line_height = int(BATTERY_CONFIG.line_height * BATTERY_CONFIG.scale_factor) - battery_panel_height = battery_line_height * 4 - battery_panel_margin = BATTERY_CONFIG.panel_margin - graph_bottom = rect.y + rect.height - CONFIG.bottom_gap_to_battery - battery_panel_height - battery_panel_margin - graph_top = rect.y + CONFIG.speed_display_bottom_y + CONFIG.top_gap_to_speed - graph_height = max(CONFIG.height, int(graph_bottom - graph_top)) - graph_width = int(graph_height * CONFIG.aspect_ratio) + # When curvatured is not running for this car, render a placeholder instead of + # doing any interpolation work. + lcp = sm["liveCurvatureParameters"] + if not bool(getattr(lcp, "useParams", False)): + graph_rect = self._build_graph_rect(rect) + rl.draw_rectangle_rounded(graph_rect, 0.08, 8, self._panel_bg) + self._draw_title_only(graph_rect) + return - graph_rect = rl.Rectangle( - rect.x + rect.width - graph_width - battery_panel_margin, - graph_bottom - graph_height, - graph_width, - graph_height, - ) + graph_rect = self._build_graph_rect(rect) rl.draw_rectangle_rounded(graph_rect, 0.08, 8, self._panel_bg) - lcp = sm["liveCurvatureParameters"] lcp_frame = sm.recv_frame["liveCurvatureParameters"] controls_state = sm["controlsState"] car_state = sm["carState"] @@ -170,6 +168,30 @@ class DynamicSteeringLearnerGraph(Widget): self._draw_overlay_info(graph_rect, lcp, float(car_state.vEgo), float(controls_state.modelDesiredCurvature), fit_corrections, fit_valid, min_y, max_y, transport_valid, payload_valid) + def _build_graph_rect(self, rect: rl.Rectangle) -> rl.Rectangle: + """Compute the on-screen rectangle for the dynamic steering learner panel.""" + battery_line_height = int(BATTERY_CONFIG.line_height * BATTERY_CONFIG.scale_factor) + battery_panel_height = battery_line_height * 4 + battery_panel_margin = BATTERY_CONFIG.panel_margin + graph_bottom = rect.y + rect.height - CONFIG.bottom_gap_to_battery - battery_panel_height - battery_panel_margin + graph_top = rect.y + CONFIG.speed_display_bottom_y + CONFIG.top_gap_to_speed + graph_height = max(CONFIG.height, int(graph_bottom - graph_top)) + graph_width = int(graph_height * CONFIG.aspect_ratio) + return rl.Rectangle( + rect.x + rect.width - graph_width - battery_panel_margin, + graph_bottom - graph_height, + graph_width, + graph_height, + ) + + def _draw_title_only(self, graph_rect: rl.Rectangle) -> None: + """Draw the panel title when no live curve data is available.""" + title_size = min(42, max(34, int(graph_rect.height * 0.095))) + text_x = float(graph_rect.x + CONFIG.padding) + title_y = float(graph_rect.y + 12) + rl.draw_text_ex(self._font_bold, "Dynamic Steering Learner", rl.Vector2(text_x, title_y), + title_size, 0, self._text_color) + def _draw_plot(self, plot_rect: rl.Rectangle, preview_curve: np.ndarray, corrections: np.ndarray, min_y: float, max_y: float,