diff --git a/openpilot/selfdrive/locationd/curvatured.py b/openpilot/selfdrive/locationd/curvatured.py index f96fb9c8e..d1d1e28a3 100644 --- a/openpilot/selfdrive/locationd/curvatured.py +++ b/openpilot/selfdrive/locationd/curvatured.py @@ -18,6 +18,7 @@ VERSION = 1 HISTORY = 5.0 MAX_YAW_RATE_STD = 1.0 MIN_ENGAGE_BUFFER = 2.0 +OVERRIDE_QUALITY_WINDOW = 10.0 ALLOWED_CARS = ['volkswagen'] STATUS_LOG_INTERVAL = 10.0 MAX_LEARN_ROLL_LATERAL_ACCEL = 0.10 @@ -39,8 +40,8 @@ PREVIEW_REFRESH_EVERY_N_UPDATES = 20 # # Safety / scope: # - Learning is gated by valid upstream pose/calibration, low roll, low yaw uncertainty, and no steering override. -# - TODO: Track 10 s override-free data quality and use it to weight sample counts/EMA updates after the hard override gate. -# - TODO: Latch override events/durations so brief overrides cannot be missed by conflated messaging. +# - Override data quality tracks the override-free share of the last 10 s and weights learning after the hard gate. +# - Override events are latched from non-conflated sockets so brief overrides cannot be missed by the learning loop. # - Corrections are bounded by a relative cap envelope over the speed-available buckets: # - up to 50% of local curvature through the last still-supported bucket # - from there, the cap fades toward 0 at the next outer bucket center @@ -529,6 +530,10 @@ class CurvatureEstimator(CurvatureDLookup): self.last_lat_inactive_t = 0.0 self.last_override_t = 0.0 + self.override_states = {"carState": False, "carStateIC": False} + self.override_intervals: deque[tuple[float, float]] = deque() + self.override_active_since: float | None = None + self.data_quality = 1.0 self.current_bucket = (-1, -1) self.current_correction = 0.0 @@ -639,17 +644,54 @@ class CurvatureEstimator(CurvatureDLookup): return None return bool(steering_pressed or steering_slightly_pressed) + @property + def override_active(self) -> bool: + return any(self.override_states.values()) + + def record_override_event(self, t: float, which: str, pressed: bool) -> None: + if which not in self.override_states: + raise ValueError(f"unsupported override service: {which}") + + was_active = self.override_active + self.override_states[which] = bool(pressed) + is_active = self.override_active + + if not was_active and is_active: + self.override_active_since = t + elif was_active and not is_active: + if self.override_active_since is not None: + self.override_intervals.append((self.override_active_since, t)) + self.override_active_since = None + + if was_active or is_active: + self.last_override_t = max(self.last_override_t, t) + + def override_data_quality_at(self, t: float) -> float: + window_start = t - OVERRIDE_QUALITY_WINDOW + while self.override_intervals and self.override_intervals[0][1] <= window_start: + self.override_intervals.popleft() + + override_duration = sum(max(0.0, min(end, t) - max(start, window_start)) + for start, end in self.override_intervals) + if self.override_active_since is not None: + override_duration += max(0.0, t - max(self.override_active_since, window_start)) + + self.data_quality = float(np.clip(1.0 - override_duration / OVERRIDE_QUALITY_WINDOW, 0.0, 1.0)) + return self.data_quality + def add_measurement(self, desired_curvature: float, actual_curvature: float, v_ego: float, - schedule_only: bool = False) -> None: + schedule_only: bool = False, data_quality: float = 1.0) -> None: curvature_idx = self.curvature_index(desired_curvature) speed_weights = self.learning_speed_weights(v_ego) - if curvature_idx is None or len(speed_weights) == 0: + data_quality = float(np.clip(data_quality, 0.0, 1.0)) + if curvature_idx is None or len(speed_weights) == 0 or data_quality <= 0.0: return error_cap = self.learning_error_cap(desired_curvature) error = float(np.clip(self.projected_error(desired_curvature, actual_curvature), -error_cap, error_cap)) - for speed_idx, weight in speed_weights: + for speed_idx, speed_weight in speed_weights: + weight = speed_weight * data_quality if weight <= 0.0: continue @@ -865,12 +907,12 @@ class CurvatureEstimator(CurvatureDLookup): self.vego.append(msg.vEgo) self.steering_pressed.append(bool(msg.steeringPressed)) if msg.steeringPressed: - self.last_override_t = t + self.last_override_t = max(self.last_override_t, t) elif which == "carStateIC": self.car_state_ic_t.append(t) self.steering_slightly_pressed.append(bool(msg.steeringSlightlyPressed)) if msg.steeringSlightlyPressed: - self.last_override_t = t + self.last_override_t = max(self.last_override_t, t) elif which == "controlsStateIC": self.controls_state_ic_t.append(t) self.model_desired_curvature.append(msg.modelDesiredCurvature) @@ -883,6 +925,7 @@ class CurvatureEstimator(CurvatureDLookup): self.lag = get_lat_delay(self.params, msg.lateralDelay) elif which == "livePose" and self.use_params: self.live_pose_update_index += 1 + self.override_data_quality_at(t) if not self._history_ready(): return if not (msg.angularVelocityDevice.valid and msg.posenetOK and msg.inputsOK and self.calibrator.calib_valid): @@ -901,7 +944,7 @@ class CurvatureEstimator(CurvatureDLookup): if any(x is None for x in (lat_active, roll_comp, v_ego, desired_curvature, steering_override)): return - if not bool(lat_active) or bool(steering_override) or float(v_ego) < self.MIN_SPEED: + if not bool(lat_active) or bool(steering_override) or self.override_active or float(v_ego) < self.MIN_SPEED: return device_pose = Pose.from_live_pose(msg) @@ -917,7 +960,8 @@ class CurvatureEstimator(CurvatureDLookup): desired_curvature = float(desired_curvature) actual_curvature = self.actual_curvature_from_yaw_rate(yaw_rate, v_ego, roll_compensation=float(roll_comp)) - self.add_measurement(desired_curvature, actual_curvature, v_ego, schedule_only=True) + self.add_measurement(desired_curvature, actual_curvature, v_ego, + schedule_only=True, data_quality=self.data_quality) self.refresh_curve_lookups(self.live_pose_update_index) def get_msg(self, valid: bool = True, live_valid: bool = True, @@ -964,6 +1008,7 @@ class CurvatureEstimator(CurvatureDLookup): f"lag={self.lag:.3f} total_points={int(round(float(self.counts.sum())))} " + f"bucket={self.current_bucket} bucket_points={self.current_bucket_points} " + f"corr={self.current_correction:.8f} cal={self.calibration_percent(self.counts)} " + + f"data_quality={self.data_quality:.3f} " + f"invalid={invalid} not_alive={not_alive}") @@ -973,6 +1018,7 @@ def main(): pm = messaging.PubMaster(['liveCurvatureParameters']) sm = messaging.SubMaster(['carControlIC', 'carControl', 'carState', 'carStateIC', 'liveCalibration', 'livePose', 'liveDelay', 'controlsStateIC'], poll='livePose') + override_socks = {which: messaging.sub_sock(which, conflate=False) for which in ('carState', 'carStateIC')} params = Params() CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) @@ -981,6 +1027,15 @@ def main(): while True: sm.update() + override_events = [] + for which, sock in override_socks.items(): + for event in messaging.drain_sock(sock): + pressed = event.carState.steeringPressed if which == 'carState' else event.carStateIC.steeringSlightlyPressed + if event.valid or pressed: + override_events.append((event.logMonoTime * 1e-9, which, bool(pressed))) + for t, which, pressed in sorted(override_events): + curvature_estimator.record_override_event(t, which, pressed) + if sm.all_checks(): for which in sm.updated.keys(): if sm.updated[which]: diff --git a/openpilot/selfdrive/locationd/test/test_curvatured.py b/openpilot/selfdrive/locationd/test/test_curvatured.py index bc40ea0bb..227492b20 100644 --- a/openpilot/selfdrive/locationd/test/test_curvatured.py +++ b/openpilot/selfdrive/locationd/test/test_curvatured.py @@ -75,6 +75,17 @@ class TestCurvatureEstimator: for speed_idx, weight in speed_weights: assert np.isclose(float(estimator.counts[speed_idx, curvature_idx]), weight) + def test_data_quality_weights_learning(self): + estimator = get_estimator() + desired_curvature = 32e-6 + v_ego = float(CurvatureDLookup.SPEED_ANCHORS[3]) + + estimator.add_measurement(desired_curvature, desired_curvature * 0.6, v_ego, data_quality=0.25) + + idx = CurvatureDLookup.indices(desired_curvature, v_ego) + assert idx is not None + assert np.isclose(float(estimator.counts[idx]), 0.25) + def test_preview_is_not_apply_capped(self): estimator = get_estimator() desired_curvature = 2.048e-3 @@ -348,6 +359,43 @@ class TestCurvatureEstimator: assert estimator._steering_override_at(12.0) assert estimator.last_override_t == 12.0 + def test_override_events_are_latched_and_quality_recovers(self): + estimator = get_estimator() + + estimator.record_override_event(1.0, "carState", True) + assert estimator.override_active + assert np.isclose(estimator.override_data_quality_at(6.0), 0.5) + + estimator.record_override_event(11.0, "carState", False) + assert not estimator.override_active + assert estimator.last_override_t == 11.0 + assert np.isclose(estimator.override_data_quality_at(13.0), 0.2) + assert np.isclose(estimator.override_data_quality_at(16.0), 0.5) + assert np.isclose(estimator.override_data_quality_at(21.0), 1.0) + + def test_brief_override_remains_latched_until_live_pose(self): + estimator = get_estimator() + + estimator.record_override_event(10.01, "carStateIC", True) + estimator.record_override_event(10.02, "carStateIC", False) + + assert not estimator.override_active + assert estimator.last_override_t == 10.02 + assert 0.99 <= estimator.override_data_quality_at(10.05) < 1.0 + assert (10.05 - estimator.last_override_t) < 2.0 + + def test_overlapping_override_sources_count_as_one_interval(self): + estimator = get_estimator() + + estimator.record_override_event(1.0, "carState", True) + estimator.record_override_event(2.0, "carStateIC", True) + estimator.record_override_event(3.0, "carState", False) + assert estimator.override_active + + estimator.record_override_event(4.0, "carStateIC", False) + assert not estimator.override_active + assert np.isclose(estimator.override_data_quality_at(4.0), 0.7) + def test_ic_values_keep_their_own_timestamps(self): estimator = get_estimator() estimator.use_params = True