From 9231b7b26e7c75efa2873cb66c5770a30565a76b Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Sun, 10 May 2026 20:35:20 +0200 Subject: [PATCH] Dynamic Steering Learner (#9) Dynamic Steering Learner This adds a learned curvature correction pipeline with live preview/apply visualization and separates debug vs runtime data. "Curvatured" as a standalone service. Service start can be toggled while offroad --- .gitmodules | 2 +- cereal/custom.capnp | 18 +- cereal/log.capnp | 3 +- cereal/services.py | 3 + common/params_keys.h | 6 +- msgq_repo | 2 +- selfdrive/controls/controlsd.py | 20 +- selfdrive/controls/lib/curvatured.py | 49 + selfdrive/controls/tests/test_curvatured.py | 157 +++ selfdrive/debug/dump_curvatured_log.py | 238 +++++ selfdrive/locationd/curvatured.py | 928 ++++++++++++++++++ selfdrive/locationd/test/test_curvatured.py | 343 +++++++ selfdrive/locationd/torqued.py | 2 +- selfdrive/selfdrived/selfdrived.py | 4 +- .../test/process_replay/process_replay.py | 18 +- selfdrive/test/test_onroad.py | 1 + selfdrive/ui/layouts/settings/device.py | 1 + selfdrive/ui/layouts/settings/ictoggles.py | 39 +- selfdrive/ui/mici/layouts/settings/device.py | 1 + .../ui/mici/layouts/settings/ictoggles.py | 9 + .../onroad/dynamic_steering_learner_graph.py | 196 ++++ selfdrive/ui/mici/onroad/hud_renderer.py | 3 + .../onroad/dynamic_steering_learner_graph.py | 270 +++++ selfdrive/ui/onroad/hud_renderer.py | 3 + .../ui/sunnypilot/layouts/settings/models.py | 1 + selfdrive/ui/ui_state.py | 1 + system/manager/process_config.py | 4 + 27 files changed, 2305 insertions(+), 17 deletions(-) create mode 100644 selfdrive/controls/lib/curvatured.py create mode 100644 selfdrive/controls/tests/test_curvatured.py create mode 100644 selfdrive/debug/dump_curvatured_log.py create mode 100644 selfdrive/locationd/curvatured.py create mode 100644 selfdrive/locationd/test/test_curvatured.py create mode 100644 selfdrive/ui/mici/onroad/dynamic_steering_learner_graph.py create mode 100644 selfdrive/ui/onroad/dynamic_steering_learner_graph.py diff --git a/.gitmodules b/.gitmodules index 5c3183513..aae49654d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -6,7 +6,7 @@ url = https://github.com/infiniteCable2/opendbc.git [submodule "msgq"] path = msgq_repo - url = https://github.com/commaai/msgq.git + url = https://github.com/infiniteCable2/msgq.git [submodule "rednose_repo"] path = rednose_repo url = https://github.com/commaai/rednose.git diff --git a/cereal/custom.capnp b/cereal/custom.capnp index fe3ed9196..3491d6986 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -483,5 +483,21 @@ struct CustomReserved17 @0xa30662f84033036c { struct CustomReserved18 @0xc86a3d38d13eb3ef { } -struct CustomReserved19 @0xa4f1eb3323f5f582 { +struct LiveCurvatureParameters @0xa4f1eb3323f5f582 { + liveValid @0 :Bool; + version @1 :Int32; + useParams @2 :Bool; + currentCorrection @3 :Float32; + currentBias @4 :Float32; + currentBucketPoints @5 :UInt16; + totalBucketPoints @6 :UInt16; + calPerc @7 :Int8; + bucketSpeed @8 :Int8; + corrections @9 :List(Float32); + counts @10 :List(UInt16); + biases @11 :List(Float32); + bucketCurvature @12 :Int8; + fitValid @13 :List(Bool); + previewCorrections @14 :List(Float32); + previewValid @15 :List(Bool); } diff --git a/cereal/log.capnp b/cereal/log.capnp index 9d4ca5e80..5d7d88597 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -808,6 +808,7 @@ struct ControlsState @0x97ff69c53601abf1 { uiAccelCmd @5 :Float32; ufAccelCmd @33 :Float32; curvature @37 :Float32; # path curvature from vehicle model + modelDesiredCurvature @67 :Float32; # raw desired curvature from modelV2 before smoothing/adaptation desiredCurvature @61 :Float32; # lag adjusted curvatures used by lateral controllers forceDecel @51 :Bool; @@ -2508,7 +2509,7 @@ struct Event { customReserved16 @142 :Custom.CustomReserved16; customReserved17 @143 :Custom.CustomReserved17; customReserved18 @144 :Custom.CustomReserved18; - customReserved19 @145 :Custom.CustomReserved19; + liveCurvatureParameters @145 :Custom.LiveCurvatureParameters; # *********** legacy + deprecated *********** model @9 :Deprecated.ModelData; # TODO: rename modelV2 and mark this as deprecated diff --git a/cereal/services.py b/cereal/services.py index 9d64f67f2..76216f305 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -103,6 +103,9 @@ _services: dict[str, tuple] = { "modelDataV2SP": (True, 20., None, QueueSize.BIG), "liveLocationKalman": (True, 20.), + # infiniteCable + "liveCurvatureParameters": (True, 4., 1), + # debug "uiDebug": (True, 0., 1), "testJoystick": (True, 0.), diff --git a/common/params_keys.h b/common/params_keys.h index a51b8ce3a..e37c6c969 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -79,6 +79,7 @@ inline static std::unordered_map keys = { {"LiveDelay", {PERSISTENT | BACKUP, BYTES}}, {"LiveParameters", {PERSISTENT, JSON}}, {"LiveParametersV2", {PERSISTENT, BYTES}}, + {"LiveCurvatureParameters", {PERSISTENT | DONT_LOG, BYTES}}, {"LiveTorqueParameters", {PERSISTENT | DONT_LOG, BYTES}}, {"LocationFilterInitialState", {PERSISTENT, BYTES}}, {"LateralManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, @@ -137,13 +138,16 @@ inline static std::unordered_map keys = { {"DisableScreenTimer", {PERSISTENT, BOOL}}, {"DarkMode", {PERSISTENT, BOOL}}, {"EnableCurvatureController", {PERSISTENT, BOOL, "1"}}, + {"EnableCurvatureD", {PERSISTENT, BOOL, "0"}}, + {"CurvatureDDebugData", {PERSISTENT, BOOL, "0"}}, {"EnableLongComfortMode", {PERSISTENT, BOOL}}, {"EnableSmoothSteer", {PERSISTENT, BOOL}}, {"EnableSpeedLimitControl", {PERSISTENT, BOOL}}, {"EnableSpeedLimitPredicative", {PERSISTENT, BOOL}}, {"EnableSLPredReactToSL", {PERSISTENT, BOOL}}, - {"EnableSLPredReactToCurves", {PERSISTENT, BOOL}}, + {"EnableSLPredReactToCurves", {PERSISTENT, BOOL}}, {"BatteryDetails", {PERSISTENT, BOOL}}, + {"ShowDynamicSteeringLearnerGraph", {PERSISTENT, BOOL}}, {"ForceRHDForBSM", {PERSISTENT, BOOL}}, {"DisableCarSteerAlerts", {PERSISTENT, BOOL}}, {"ShowAccelBar", {PERSISTENT, BOOL}}, diff --git a/msgq_repo b/msgq_repo index b7688b9bd..0930be91a 160000 --- a/msgq_repo +++ b/msgq_repo @@ -1 +1 @@ -Subproject commit b7688b9bd731dea4520adf248bf1eb49b6dde776 +Subproject commit 0930be91afe090b7009d184543e6b00856312364 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index ee68a895a..04daa6181 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -11,6 +11,7 @@ from openpilot.common.swaglog import cloudlog from opendbc.car.car_helpers import interfaces from opendbc.car.vehicle_model import VehicleModel +from openpilot.selfdrive.controls.lib.curvatured import CurvatureDController from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID @@ -45,18 +46,20 @@ class Controls(ControlsExt): self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP) - self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', + self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'liveCurvatureParameters', 'modelV2', 'selfdriveState', 'liveCalibration', 'livePose', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput', - 'driverMonitoringState', 'onroadEvents', 'driverAssistance', 'liveDelay'] + self.sm_services_ext, + 'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + self.sm_services_ext, poll='selfdriveState') self.pm = messaging.PubMaster(['carControl', 'controlsState'] + self.pm_services_ext) self.steer_limited_by_safety = False self.curvature = 0.0 self.roll_compensation = 0.0 + self.model_desired_curvature = 0.0 self.desired_curvature = 0.0 self.enable_curvature_controller = self.params.get_bool("EnableCurvatureController") + self.enable_curvatured = self.params.get_bool("EnableCurvatureD") self.enable_speed_limit_control = self.params.get_bool("EnableSpeedLimitControl") self.enable_speed_limit_predicative = self.params.get_bool("EnableSpeedLimitPredicative") self.enable_pred_react_to_speed_limits = self.params.get_bool("EnableSLPredReactToSL") @@ -72,6 +75,7 @@ class Controls(ControlsExt): self.LoC = LongControl(self.CP, self.CP_SP) self.VM = VehicleModel(self.CP) + self.curvatured = CurvatureDController() if self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED else None self.LaC: LatControl if (self.CP.steerControlType == car.CarParams.SteerControlType.angle or self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED): @@ -95,6 +99,7 @@ class Controls(ControlsExt): if self.param_counter >= 100: self.param_counter = 0 self.enable_curvature_controller = self.params.get_bool("EnableCurvatureController") + self.enable_curvatured = self.params.get_bool("EnableCurvatureD") self.enable_smooth_steer = self.params.get_bool("EnableSmoothSteer") self.enable_speed_limit_control = self.params.get_bool("EnableSpeedLimitControl") self.enable_speed_limit_predicative = self.params.get_bool("EnableSpeedLimitPredicative") @@ -130,6 +135,13 @@ class Controls(ControlsExt): self.LaC.extension.update_lateral_lag(self.lat_delay) + if self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: + curvature_params = self.sm['liveCurvatureParameters'] + if self.sm.all_checks(['liveCurvatureParameters']) and curvature_params.useParams: + self.curvatured.update_live_params(curvature_params) + else: + self.curvatured.reset() + long_plan = self.sm['longitudinalPlan'] model_v2 = self.sm['modelV2'] @@ -172,8 +184,11 @@ class Controls(ControlsExt): new_desired_curvature = self.sm['lateralManeuverPlan'].desiredCurvature if CC.latActive else self.curvature else: new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature + self.model_desired_curvature = float(model_v2.action.desiredCurvature) if self.enable_smooth_steer: new_desired_curvature = self.smooth_steer.update(new_desired_curvature) + if CC.latActive and self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED and self.enable_curvatured: + new_desired_curvature = self.curvatured.apply(new_desired_curvature, CS.vEgo) self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll) lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS @@ -257,6 +272,7 @@ class Controls(ControlsExt): cs.curvature = self.curvature cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2'] + cs.modelDesiredCurvature = self.model_desired_curvature cs.desiredCurvature = self.desired_curvature cs.longControlState = self.LoC.long_control_state cs.upAccelCmd = float(self.LoC.pid.p) diff --git a/selfdrive/controls/lib/curvatured.py b/selfdrive/controls/lib/curvatured.py new file mode 100644 index 000000000..a2be5f068 --- /dev/null +++ b/selfdrive/controls/lib/curvatured.py @@ -0,0 +1,49 @@ +import numpy as np + +from openpilot.selfdrive.locationd.curvatured import CurvatureDLookup, VERSION + + +class CurvatureDController(CurvatureDLookup): + def __init__(self) -> None: + self.reset() + + def reset(self) -> None: + self.use_params = False + 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) + + def update_live_params(self, msg) -> None: + expected_size = self.total_size() + if msg.version != VERSION or len(msg.corrections) != 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) + else: + self.fit_valid = np.abs(self.fit_corrections) > 0.0 + + if not self.live_valid: + self.reset() + + 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: + return 0.0 + + projected = self.interp_curve_value(self.fit_corrections, self.fit_valid, v_ego, abs_curvature) + 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 new file mode 100644 index 000000000..ae739434e --- /dev/null +++ b/selfdrive/controls/tests/test_curvatured.py @@ -0,0 +1,157 @@ +import cereal.messaging as messaging + +from openpilot.selfdrive.controls.lib.curvatured import CurvatureDController +from openpilot.selfdrive.locationd.curvatured import CurvatureDLookup, VERSION + + +class TestCurvatureDController: + @staticmethod + def _set_curve(msg, speed_idx: int, values: dict[int, float]): + corrections = list(msg.liveCurvatureParameters.corrections) + fit_valid = list(msg.liveCurvatureParameters.fitValid) + if len(corrections) != CurvatureDLookup.total_size(): + corrections = [0.0] * CurvatureDLookup.total_size() + if len(fit_valid) != CurvatureDLookup.total_size(): + fit_valid = [False] * CurvatureDLookup.total_size() + + width = len(CurvatureDLookup.CURVATURE_BUCKET_CENTERS) + for curvature_idx, value in values.items(): + flat_idx = speed_idx * width + curvature_idx + corrections[flat_idx] = value + fit_valid[flat_idx] = True + + msg.liveCurvatureParameters.corrections = corrections + msg.liveCurvatureParameters.fitValid = fit_valid + + def test_apply_interpolates_between_neighbor_speed_curves(self): + 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, 2, {curvature_idx: 4e-6}) + self._set_curve(msg, 3, {curvature_idx: 12e-6}) + controller.update_live_params(msg.liveCurvatureParameters) + + low_speed = float(CurvatureDLookup.SPEED_ANCHORS[2]) + high_speed = float(CurvatureDLookup.SPEED_ANCHORS[3]) + mid_speed = 0.5 * (low_speed + high_speed) + + low = controller.get_correction(32e-6, low_speed) + mid = controller.get_correction(32e-6, mid_speed) + high = controller.get_correction(32e-6, high_speed) + + assert low > 0.0 + assert high > low + assert low < mid < high + + def test_negative_curvature_uses_same_curve_with_negative_sign(self): + 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]) + pos = controller.get_correction(32e-6, v_ego) + neg = controller.get_correction(-32e-6, v_ego) + + assert pos > 0.0 + assert neg < 0.0 + assert abs(pos + neg) < 1e-12 + + def test_invalid_message_disables_corrections(self): + controller = CurvatureDController() + msg = messaging.new_message('liveCurvatureParameters') + msg.liveCurvatureParameters.liveValid = False + msg.liveCurvatureParameters.version = VERSION + 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() + + controller.update_live_params(msg.liveCurvatureParameters) + + assert controller.apply(32e-6, 20.0) == 32e-6 + + def test_correction_fades_outside_supported_curvature_range(self): + 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() + + self._set_curve(msg, 3, { + 4: 4e-6, + 5: 8e-6, + 6: 6e-6, + }) + controller.update_live_params(msg.liveCurvatureParameters) + + v_ego = float(CurvatureDLookup.SPEED_ANCHORS[3]) + inside = controller.get_correction(5.0e-5, v_ego) + lower_fade = controller.get_correction(1.0e-5, v_ego) + upper_fade = controller.get_correction(2.0e-4, v_ego) + + assert inside > 0.0 + assert 0.0 <= lower_fade < inside + assert 0.0 <= upper_fade < inside + + def test_outer_bucket_range_is_supported(self): + 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() + + outer_idx = CurvatureDLookup.curvature_index(1.5e-3) + assert outer_idx is not None + self._set_curve(msg, 3, {outer_idx: 8.0e-5}) + controller.update_live_params(msg.liveCurvatureParameters) + + v_ego = float(CurvatureDLookup.SPEED_ANCHORS[3]) + outer = controller.get_correction(1.5e-3, v_ego) + + assert outer > 0.0 + + def test_outer_range_fades_to_zero_past_last_bucket_edge(self): + 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() + + outer_idx = len(CurvatureDLookup.CURVATURE_BUCKET_CENTERS) - 1 + self._set_curve(msg, 3, {outer_idx: 8.0e-5}) + controller.update_live_params(msg.liveCurvatureParameters) + + v_ego = float(CurvatureDLookup.SPEED_ANCHORS[3]) + last_edge = float(CurvatureDLookup.CURVATURE_BUCKET_MAX) + fade_mid = 0.5 * (last_edge + float(CurvatureDLookup.CURVATURE_MAX)) + + at_last_edge = controller.get_correction(last_edge, v_ego) + in_fade = controller.get_correction(fade_mid, v_ego) + at_max = controller.get_correction(float(CurvatureDLookup.CURVATURE_MAX), v_ego) + + assert at_last_edge > 0.0 + assert 0.0 < in_fade < at_last_edge + assert at_max == 0.0 diff --git a/selfdrive/debug/dump_curvatured_log.py b/selfdrive/debug/dump_curvatured_log.py new file mode 100644 index 000000000..aa80ef267 --- /dev/null +++ b/selfdrive/debug/dump_curvatured_log.py @@ -0,0 +1,238 @@ +#!/usr/bin/env python3 +import argparse +from collections.abc import Iterable +from pathlib import Path + +import numpy as np + +from cereal import log +from openpilot.selfdrive.locationd.curvatured import CurvatureDLookup +from openpilot.tools.lib.logreader import LogReader, ReadMode + + +def speed_label(speed_idx: int) -> str: + speed_kph = CurvatureDLookup.SPEED_ANCHORS[speed_idx] * 3.6 + return f"{speed_kph:.0f} km/h" + + +def curvature_bucket_label(curvature_idx: int) -> str: + low = CurvatureDLookup.CURVATURE_BUCKET_EDGES[curvature_idx] + high = CurvatureDLookup.CURVATURE_BUCKET_EDGES[curvature_idx + 1] + return f"{low:.2e} .. {high:.2e}" + + +def iter_param_entries(init_data) -> Iterable: + params = getattr(init_data, "params", None) + if params is None: + return [] + + entries = getattr(params, "entries", None) + if entries is None: + return [] + + return entries + + +def decode_cached_param(init_data): + for entry in iter_param_entries(init_data): + if getattr(entry, "key", None) != "LiveCurvatureParameters": + continue + + raw_value = bytes(entry.value) + result = { + "byte_len": len(raw_value), + "decoded": None, + "error": None, + "redacted": False, + } + + if len(raw_value) == 0: + result["redacted"] = True + return result + + try: + with log.Event.from_bytes(raw_value) as evt: + result["decoded"] = evt.liveCurvatureParameters + except Exception as e: + result["error"] = repr(e) + + return result + + return None + + +def message_summary(msg) -> dict: + payload = msg.liveCurvatureParameters + return { + "valid": bool(msg.valid), + "live_valid": bool(payload.liveValid), + "version": int(payload.version), + "use_params": bool(payload.useParams), + "cal_perc": int(payload.calPerc), + "total_points": int(payload.totalBucketPoints), + "bucket_speed": int(payload.bucketSpeed), + "bucket_curvature": int(payload.bucketCurvature), + "bucket_points": int(payload.currentBucketPoints), + "current_bias": float(payload.currentBias), + "current_correction": float(payload.currentCorrection), + } + + +def print_bucket_details(counts: np.ndarray, biases: np.ndarray, corrections: np.ndarray, + fit_valid: np.ndarray, speed_idx: int, focus_idx: int | None = None) -> None: + print(" buckets:") + for curvature_idx in range(len(CurvatureDLookup.CURVATURE_BUCKET_CENTERS)): + marker = "*" if focus_idx is not None and curvature_idx == focus_idx else " " + bucket_range = curvature_bucket_label(curvature_idx) + bucket_points = int(round(float(counts[speed_idx, curvature_idx]))) + bucket_bias = float(biases[speed_idx, curvature_idx]) + bucket_corr = float(corrections[speed_idx, curvature_idx]) + bucket_valid = bool(fit_valid[speed_idx, curvature_idx]) + print( + f" {marker} idx={curvature_idx} range={bucket_range} " + f"points={bucket_points} fitValid={bucket_valid} " + f"bias={bucket_bias:.8f} corr={bucket_corr:.8f}" + ) + + +def print_message_summary(title: str, payload) -> None: + has_debug_arrays = len(list(payload.counts)) == CurvatureDLookup.total_size() and len(list(payload.biases)) == CurvatureDLookup.total_size() + counts = CurvatureDLookup.unflatten_bucket(list(payload.counts)) if has_debug_arrays else np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + biases = CurvatureDLookup.unflatten_bucket(list(payload.biases)) if has_debug_arrays else np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + corrections = CurvatureDLookup.unflatten_bucket(list(payload.corrections)) + fit_valid = CurvatureDLookup.unflatten_bucket(list(payload.fitValid), dtype=bool) + + print(title) + print(f" version: {int(payload.version)}") + print(f" liveValid: {bool(payload.liveValid)}") + print(f" useParams: {bool(payload.useParams)}") + print(f" calPerc: {int(payload.calPerc)}") + print(f" totalBucketPoints: {int(payload.totalBucketPoints)}") + print(f" currentCorrection: {float(payload.currentCorrection):.8f}") + print(f" currentBias: {float(payload.currentBias):.8f}") + print(f" currentBucket: ({int(payload.bucketSpeed)}, {int(payload.bucketCurvature)})") + if not has_debug_arrays: + print(" debugArrays: omitted") + + speed_entries = [] + for speed_idx in range(len(CurvatureDLookup.SPEED_ANCHORS)): + speed_counts = counts[speed_idx] + speed_valid = CurvatureDLookup.speed_curve_valid(counts, speed_idx) if has_debug_arrays else bool(np.any(fit_valid[speed_idx])) + valid_bucket_count = int(np.count_nonzero(speed_counts >= CurvatureDLookup.MIN_BUCKET_POINTS)) if has_debug_arrays else int(np.count_nonzero(fit_valid[speed_idx])) + total_points = int(round(float(speed_counts.sum()))) if has_debug_arrays else int(np.count_nonzero(fit_valid[speed_idx])) + if total_points == 0 and not speed_valid: + continue + + best_idx = int(np.argmax(speed_counts)) if total_points > 0 else 0 + best_points = int(round(float(speed_counts[best_idx]))) + best_bias = float(biases[speed_idx, best_idx]) + best_corr = float(corrections[speed_idx, best_idx]) + best_bucket = curvature_bucket_label(best_idx) + fit_points = int(np.count_nonzero(fit_valid[speed_idx])) + focus_idx = best_idx + if int(payload.bucketSpeed) == speed_idx and int(payload.bucketCurvature) >= 0: + focus_idx = int(payload.bucketCurvature) + speed_entries.append((speed_idx, focus_idx, + f" {speed_label(speed_idx)}: valid={speed_valid} total={total_points} " + f"validBuckets={valid_bucket_count} fitPoints={fit_points} " + f"topBucket={best_idx} ({best_bucket}) points={best_points} " + f"bias={best_bias:.8f} corr={best_corr:.8f}") + ) + + if speed_entries: + print(" speed anchors:") + for speed_idx, focus_idx, line in speed_entries: + print(line) + if has_debug_arrays: + print_bucket_details(counts, biases, corrections, fit_valid, speed_idx, focus_idx) + + +def main() -> None: + parser = argparse.ArgumentParser( + description="Dump liveCurvatureParameters from a route, segment, local log file, or URL using the local schema." + ) + parser.add_argument( + "route_or_segment_name", + help="Route id, segment range, local rlog/qlog path, or comma URL accepted by LogReader", + ) + parser.add_argument( + "--segment", + help="Optional segment selector to append to a bare route id, e.g. 0, -1, or 2:6", + ) + parser.add_argument( + "--mode", + choices=[m.value for m in ReadMode], + default=ReadMode.AUTO.value, + help="LogReader mode: r=rlog, q=qlog, a=auto, i=auto interactive", + ) + parser.add_argument("--limit", type=int, default=5, help="How many liveCurvatureParameters events to sample") + args = parser.parse_args() + + target = args.route_or_segment_name.strip() + is_local_path = Path(target).exists() + if args.segment and not is_local_path and "://" not in target: + selector = args.segment.strip().lstrip("/") + if "/q" not in target and "/r" not in target and "/a" not in target and "/i" not in target: + target = f"{target.rstrip('/')}/{selector}" + + lr = LogReader(target, default_mode=ReadMode(args.mode)) + + init_data = None + curvature_msgs = [] + valid_count = 0 + live_valid_count = 0 + + for msg in lr: + which = msg.which() + if which == "initData" and init_data is None: + init_data = msg.initData + elif which == "liveCurvatureParameters": + curvature_msgs.append(msg) + valid_count += int(bool(msg.valid)) + live_valid_count += int(bool(msg.liveCurvatureParameters.liveValid)) + + print(f"liveCurvatureParameters events: {len(curvature_msgs)}") + if curvature_msgs: + print(f"transport valid count: {valid_count}") + print(f"payload liveValid count: {live_valid_count}") + + first = message_summary(curvature_msgs[0]) + last = message_summary(curvature_msgs[-1]) + print("first event:") + for key, value in first.items(): + print(f" {key}: {value}") + print("last event:") + for key, value in last.items(): + print(f" {key}: {value}") + + sample_indices = np.linspace(0, len(curvature_msgs) - 1, min(args.limit, len(curvature_msgs)), dtype=int) + printed = set() + for idx in sample_indices: + idx = int(idx) + if idx in printed: + continue + printed.add(idx) + print_message_summary(f"sample event #{idx}", curvature_msgs[idx].liveCurvatureParameters) + else: + print("No liveCurvatureParameters events found.") + + if init_data is None: + print("No initData found.") + return + + cached = decode_cached_param(init_data) + if cached is None: + print("initData: no LiveCurvatureParameters param entry found.") + return + + print(f"initData cache bytes: {cached['byte_len']}") + if cached["redacted"]: + print("initData cache: redacted (DONT_LOG)") + elif cached["decoded"] is not None: + print_message_summary("decoded initData cache", cached["decoded"]) + else: + print(f"initData cache decode error: {cached['error']}") + + +if __name__ == "__main__": + main() diff --git a/selfdrive/locationd/curvatured.py b/selfdrive/locationd/curvatured.py new file mode 100644 index 000000000..e63be9335 --- /dev/null +++ b/selfdrive/locationd/curvatured.py @@ -0,0 +1,928 @@ +import math +import time +from collections import deque + +import numpy as np + +import cereal.messaging as messaging +from cereal import car, log +from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY +from openpilot.common.params import Params +from openpilot.common.realtime import config_realtime_process, DT_MDL +from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose +from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD +from openpilot.sunnypilot.livedelay.helpers import get_lat_delay + +VERSION = 1 +HISTORY = 5.0 +MAX_YAW_RATE_STD = 1.0 +MIN_ENGAGE_BUFFER = 2.0 +ALLOWED_CARS = ['volkswagen'] +STATUS_LOG_INTERVAL = 10.0 +MAX_LEARN_ROLL_LATERAL_ACCEL = 0.10 +FIT_REFRESH_EVERY_N_UPDATES = 4 +PREVIEW_REFRESH_EVERY_N_UPDATES = 20 + +# CurvatureD learns a small, center-focused curvature correction on top of the model/controller target. +# The goal is not to replace the steering model, but to reduce subtle dynamic-steering mismatch that can +# show up as light ping-pong or center softness around straight driving and shallow bends. +# +# Important magnitude intuition: +# - The corrected range mainly targets small steering wheel angles around center, not large cornering input. +# - Rough real-world feel for this vehicle family: +# - regular straight / gentle highway lane-keeping tends to stay below ~5e-4 +# - ~1e-3 is still only around a few degrees at the steering wheel (~3 deg order of magnitude) +# - ~5e-3 is already a clearly visible steering input (~16 deg order of magnitude) +# - In other words, CurvatureD mainly works from near-center out into modest bends, while larger low-speed cornering +# curvature is only part of the outer fade range and not the primary target. +# +# Safety / scope: +# - Learning is gated by valid upstream pose/calibration, low roll, low yaw uncertainty, and no steering override. +# - 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 +# - Apply magnitude is limited by the relative cap envelope and the lateral-accel apply gate. + + +class CurvatureDLookup: + SPEED_ANCHORS = np.array([20.0, 40.0, 60.0, 80.0, 100.0, 120.0, 140.0], dtype=np.float32) / 3.6 + CURVATURE_BUCKET_EDGES = np.array([ + 1.0e-6, + 2.0e-6, + 4.0e-6, + 8.0e-6, + 1.6e-5, + 3.2e-5, + 6.4e-5, + 1.28e-4, + 2.56e-4, + 5.12e-4, + 1.024e-3, + 2.048e-3, + 4.096e-3, + ], dtype=np.float32) + CURVATURE_BUCKET_CENTERS = np.sqrt(CURVATURE_BUCKET_EDGES[:-1] * CURVATURE_BUCKET_EDGES[1:]).astype(np.float32) + CURVATURE_BUCKET_MIN = float(CURVATURE_BUCKET_EDGES[0]) + CURVATURE_MIN = 0.0 + CURVATURE_BUCKET_MAX = float(CURVATURE_BUCKET_EDGES[-1]) + LAST_BUCKET_WIDTH = float(CURVATURE_BUCKET_EDGES[-1] - CURVATURE_BUCKET_EDGES[-2]) + CURVATURE_MAX = CURVATURE_BUCKET_MAX + LAST_BUCKET_WIDTH + + 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 + + MAX_SAMPLES = 600 # per-bucket saturation + MEAN_WINDOW = 180.0 # bias EMA horizon + MIN_REQUIRED_SUPPORT_BUCKETS = 4 # support floor per speed + SUPPORT_REFERENCE_LAT_ACCEL = 0.05 # maps speed to support width + MIN_BUCKET_POINTS = np.array([20, 20, 18, 16, 14, 12, 10, 8, 6, 6, 4, 4], dtype=np.float32) # bucket fit-valid threshold + FULL_BUCKET_STRENGTH_SAMPLES = MIN_BUCKET_POINTS + MIN_BUCKET_POINTS # local_strength == 1 + + @classmethod + def bucket_shape(cls) -> tuple[int, int]: + return len(cls.SPEED_ANCHORS), len(cls.CURVATURE_BUCKET_CENTERS) + + @classmethod + def total_size(cls) -> int: + a, b = cls.bucket_shape() + return a * b + + @classmethod + def flatten(cls, arr: np.ndarray) -> list: + return arr.reshape(-1).tolist() + + @classmethod + def unflatten_bucket(cls, values, dtype=np.float32) -> np.ndarray: + return np.asarray(values, dtype=dtype).reshape(cls.bucket_shape()) + + @classmethod + def curvature_index(cls, curvature: float) -> int | None: + abs_curvature = abs(float(curvature)) + if abs_curvature < cls.CURVATURE_BUCKET_MIN or abs_curvature > cls.CURVATURE_BUCKET_MAX: + return None + + idx = int(np.searchsorted(cls.CURVATURE_BUCKET_EDGES, abs_curvature, side='right') - 1) + return min(max(idx, 0), len(cls.CURVATURE_BUCKET_CENTERS) - 1) + + @classmethod + def speed_index(cls, v_ego: float) -> int | None: + v = float(v_ego) + if v < cls.MIN_SPEED: + return None + return int(np.argmin(np.abs(cls.SPEED_ANCHORS - v))) + + @classmethod + def learning_speed_weights(cls, v_ego: float) -> list[tuple[int, float]]: + v = float(v_ego) + if v < cls.MIN_SPEED: + return [] + + low, high, alpha = cls.speed_interp(v) + if low == high: + return [(low, 1.0)] + return [(low, 1.0 - alpha), (high, alpha)] + + @classmethod + def indices(cls, curvature: float, v_ego: float) -> tuple[int, int] | None: + speed_idx = cls.speed_index(v_ego) + curvature_idx = cls.curvature_index(curvature) + if speed_idx is None or curvature_idx is None: + return None + return speed_idx, curvature_idx + + @classmethod + def speed_interp(cls, v_ego: float) -> tuple[int, int, float]: + v = float(v_ego) + if v <= cls.SPEED_ANCHORS[0]: + return 0, 0, 0.0 + if v >= cls.SPEED_ANCHORS[-1]: + last = len(cls.SPEED_ANCHORS) - 1 + return last, last, 0.0 + + high = int(np.searchsorted(cls.SPEED_ANCHORS, v, side='right')) + low = high - 1 + span = float(cls.SPEED_ANCHORS[high] - cls.SPEED_ANCHORS[low]) + alpha = (v - float(cls.SPEED_ANCHORS[low])) / max(span, 1e-6) + return low, high, float(np.clip(alpha, 0.0, 1.0)) + + @classmethod + def fit_local_strength(cls, bucket_counts: np.ndarray, valid_idx: np.ndarray) -> np.ndarray: + bucket_conf_start = cls.MIN_BUCKET_POINTS[valid_idx] + bucket_conf_full = np.asarray(cls.FULL_BUCKET_STRENGTH_SAMPLES[valid_idx], dtype=np.float64) + bucket_conf_span = bucket_conf_full - bucket_conf_start + return np.clip( + (bucket_counts[valid_idx] - bucket_conf_start) / np.maximum(bucket_conf_span, 1.0), + 0.0, 1.0 + ).astype(np.float64) + + @classmethod + def preview_local_strength(cls, bucket_counts: np.ndarray, valid_idx: np.ndarray) -> np.ndarray: + return np.ones(len(valid_idx), dtype=np.float64) + + @classmethod + def _build_curve_corrections(cls, bias: np.ndarray, counts: np.ndarray, + valid_mask_fn, + min_valid_buckets_fn, + local_strength_fn, + speed_strength_fn, + apply_cap: bool = True, + zero_invalid_buckets: bool = False) -> tuple[np.ndarray, np.ndarray]: + corrections = np.zeros(cls.bucket_shape(), dtype=np.float32) + valid = np.zeros(cls.bucket_shape(), dtype=bool) + + for speed_idx in range(len(cls.SPEED_ANCHORS)): + curve_valid = np.asarray(valid_mask_fn(counts[speed_idx]), dtype=bool) + if apply_cap: + curve_valid &= cls.apply_bucket_mask(speed_idx) + if int(np.count_nonzero(curve_valid)) < int(min_valid_buckets_fn(speed_idx)): + continue + + valid_idx = np.flatnonzero(curve_valid) + if apply_cap: + bucket_caps = np.asarray([cls.correction_cap(float(curvature), float(cls.SPEED_ANCHORS[speed_idx])) + for curvature in cls.CURVATURE_BUCKET_CENTERS], dtype=np.float64) + else: + bucket_caps = np.full(len(cls.CURVATURE_BUCKET_CENTERS), np.inf, dtype=np.float64) + local_strength = local_strength_fn(counts[speed_idx], valid_idx) + speed_strength = float(speed_strength_fn(counts[speed_idx], speed_idx, valid_idx, local_strength)) + row = np.zeros(len(cls.CURVATURE_BUCKET_CENTERS), dtype=np.float32) + + for start, end in cls.valid_runs(curve_valid): + run_idx = np.arange(start, end + 1) + run_curve = np.clip(bias[speed_idx, run_idx], -bucket_caps[run_idx], bucket_caps[run_idx]).astype(np.float32) + run_strength = local_strength_fn(counts[speed_idx], run_idx).astype(np.float32) + + if len(run_curve) >= 3: + smoothed_run = run_curve.copy() + smoothed_run[1:-1] = 0.25 * run_curve[:-2] + 0.5 * run_curve[1:-1] + 0.25 * run_curve[2:] + else: + smoothed_run = run_curve + + run_values = speed_strength * run_strength * smoothed_run + row[run_idx] = np.clip(run_values, -bucket_caps[run_idx], bucket_caps[run_idx]) if apply_cap else run_values + + if zero_invalid_buckets: + row = np.where(curve_valid, row, 0.0) + + corrections[speed_idx] = row.astype(np.float32) + valid[speed_idx] = curve_valid + + return corrections, valid + + @classmethod + def bucket_points_for_index(cls, counts: np.ndarray, idx: tuple[int, int] | None) -> int: + if idx is None: + return 0 + return int(round(float(counts[idx]))) + + @classmethod + def actual_curvature_from_yaw_rate(cls, yaw_rate: float, v_ego: float, roll_compensation: float = 0.0) -> float: + return float(yaw_rate / max(float(v_ego), 0.1) - float(roll_compensation)) + + @classmethod + def apply_bucket_mask(cls, speed_idx: int) -> np.ndarray: + mask = np.zeros(len(cls.CURVATURE_BUCKET_CENTERS), dtype=bool) + max_bucket_idx = cls.max_supported_bucket_index(float(cls.SPEED_ANCHORS[speed_idx])) + if max_bucket_idx is None: + return mask + mask[:max_bucket_idx + 1] = True + return mask + + @classmethod + def speed_curve_valid(cls, counts: np.ndarray, speed_idx: int) -> bool: + return cls.speed_curve_strength(counts[speed_idx], speed_idx) > 0.0 + + @classmethod + def speed_curve_strength(cls, speed_counts: np.ndarray, speed_idx: int) -> float: + valid_mask = np.asarray(speed_counts >= cls.MIN_BUCKET_POINTS, dtype=bool) & cls.apply_bucket_mask(speed_idx) + valid_idx = np.flatnonzero(valid_mask) + if len(valid_idx) == 0: + return 0.0 + + local_strength = cls.fit_local_strength(speed_counts, valid_idx) + required_bucket_count = cls.required_support_bucket_count(speed_idx) + top_strengths = np.sort(local_strength)[-required_bucket_count:] + return float(np.sum(top_strengths) / float(required_bucket_count)) + + @classmethod + def speed_curve_fully_calibrated(cls, counts: np.ndarray, speed_idx: int) -> bool: + fully_calibrated_mask = np.asarray(counts[speed_idx] >= cls.FULL_BUCKET_STRENGTH_SAMPLES, dtype=bool) & cls.apply_bucket_mask(speed_idx) + required_bucket_count = cls.required_support_bucket_count(speed_idx) + return int(np.count_nonzero(fully_calibrated_mask)) >= required_bucket_count + + @classmethod + def required_support_bucket_count(cls, speed_idx: int) -> int: + v_ego = float(cls.SPEED_ANCHORS[speed_idx]) + typical_curvature = cls.SUPPORT_REFERENCE_LAT_ACCEL / max(v_ego ** 2, 1e-6) + bucket_count = int(np.searchsorted(cls.CURVATURE_BUCKET_CENTERS, typical_curvature, side='right')) + return int(np.clip(bucket_count, cls.MIN_REQUIRED_SUPPORT_BUCKETS, len(cls.CURVATURE_BUCKET_CENTERS))) + + @classmethod + def calibration_percent(cls, counts: np.ndarray) -> int: + fully_calibrated_speeds = sum(cls.speed_curve_fully_calibrated(counts, speed_idx) for speed_idx in range(len(cls.SPEED_ANCHORS))) + 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)) + return y * y * (3.0 - 2.0 * y) + + @classmethod + def max_supported_bucket_index(cls, v_ego: float) -> int | None: + max_curvature = min(cls.MAX_LAT_ACCEL_APPLY / max(float(v_ego) ** 2, 1e-6), cls.CURVATURE_BUCKET_MAX) + return cls.curvature_index(max_curvature) + + @classmethod + def cap_zero_curvature(cls, v_ego: float) -> float: + max_bucket_idx = cls.max_supported_bucket_index(v_ego) + if max_bucket_idx is None: + return cls.CURVATURE_BUCKET_MIN + + next_idx = max_bucket_idx + 1 + if next_idx < len(cls.CURVATURE_BUCKET_CENTERS): + return float(cls.CURVATURE_BUCKET_CENTERS[next_idx]) + return cls.CURVATURE_MAX + + @classmethod + def correction_cap_ratio(cls, curvature: float, v_ego: float) -> float: + abs_curvature = abs(float(curvature)) + if abs_curvature <= cls.CURVATURE_MIN: + return 0.0 + + if abs_curvature < cls.CURVATURE_BUCKET_MIN: + inner_alpha = cls.smoothstep((abs_curvature - cls.CURVATURE_MIN) / + max(cls.CURVATURE_BUCKET_MIN - cls.CURVATURE_MIN, 1e-9)) + return float(inner_alpha * cls.RELATIVE_CAP_FULL_RATIO) + + bucket_idx = cls.curvature_index(abs_curvature) + if bucket_idx is None: + return 0.0 + + max_bucket_idx = cls.max_supported_bucket_index(v_ego) + if max_bucket_idx is None: + return 0.0 + + if bucket_idx <= max_bucket_idx: + return cls.RELATIVE_CAP_FULL_RATIO + + fade_start = float(cls.CURVATURE_BUCKET_CENTERS[max_bucket_idx]) + fade_end = cls.cap_zero_curvature(v_ego) + if abs_curvature >= fade_end: + return 0.0 + + alpha = cls.smoothstep((abs_curvature - fade_start) / max(fade_end - fade_start, 1e-9)) + return float((1.0 - alpha) * cls.RELATIVE_CAP_FULL_RATIO) + + @classmethod + def correction_cap(cls, curvature: float, v_ego: float) -> float: + abs_curvature = abs(float(curvature)) + return float(cls.correction_cap_ratio(abs_curvature, v_ego) * abs_curvature) + + @classmethod + def learning_error_cap(cls, curvature: float) -> float: + return float(cls.RELATIVE_CAP_FULL_RATIO * abs(float(curvature))) + + @classmethod + def projected_error(cls, desired_curvature: float, actual_curvature: float) -> float: + direction = 1.0 if desired_curvature >= 0.0 else -1.0 + return float(direction * (desired_curvature - actual_curvature)) + + @classmethod + def build_fit_corrections(cls, bias: np.ndarray, counts: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + return cls._build_curve_corrections( + bias, + counts, + lambda speed_counts: speed_counts >= cls.MIN_BUCKET_POINTS, + lambda _speed_idx: 1, + cls.fit_local_strength, + lambda speed_counts, speed_idx, _valid_idx, _local_strength: cls.speed_curve_strength(speed_counts, speed_idx), + apply_cap=True, + zero_invalid_buckets=True, + ) + + @classmethod + def build_preview_corrections(cls, bias: np.ndarray, counts: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + return cls._build_curve_corrections( + bias, + counts, + lambda speed_counts: speed_counts > 0.0, + lambda _speed_idx: 1, + cls.preview_local_strength, + lambda _all_counts, _speed_idx, _valid_idx, _local_strength: 1.0, + apply_cap=False, + ) + + @classmethod + def valid_runs(cls, valid_mask: np.ndarray) -> list[tuple[int, int]]: + idx = np.flatnonzero(valid_mask) + if len(idx) == 0: + return [] + + runs: list[tuple[int, int]] = [] + start = int(idx[0]) + end = start + for current in idx[1:]: + current = int(current) + if current == end + 1: + end = current + else: + runs.append((start, end)) + start = end = current + runs.append((start, end)) + return runs + + @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: + return 0.0 + + low_speed, high_speed, speed_alpha = cls.speed_interp(v_ego) + low_curve = fit_corrections[low_speed] + high_curve = fit_corrections[high_speed] + low_valid = fit_valid[low_speed] + high_valid = fit_valid[high_speed] + + if not low_valid.any() and not high_valid.any(): + return 0.0 + + log_centers = np.log(cls.CURVATURE_BUCKET_CENTERS.astype(np.float64)) + log_curvature = math.log(max(abs_curvature, cls.CURVATURE_BUCKET_MIN)) + + def curve_value(curve: np.ndarray, valid_mask: np.ndarray) -> float: + runs = cls.valid_runs(valid_mask) + if len(runs) == 0: + return 0.0 + + 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] + + if first_edge <= abs_curvature <= last_edge: + return float(np.interp(log_curvature, run_log_x, run_curve)) + + 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)) + + 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)) + + return 0.0 + + low_val = curve_value(low_curve, low_valid) + high_val = curve_value(high_curve, high_valid) + if low_speed == high_speed: + return low_val + return float((1.0 - speed_alpha) * low_val + speed_alpha * high_val) + + +class CurvatureEstimator(CurvatureDLookup): + def __init__(self, CP: car.CarParams): + self.CP = CP + self.params = Params() + self.frame = -1 + self.lag = 0.0 + self.hist_len = int(HISTORY / DT_MDL) + self.calibrator = PoseCalibrator() + + self.bias = np.zeros(self.bucket_shape(), dtype=np.float32) + self.counts = np.zeros(self.bucket_shape(), dtype=np.float32) + self.fit_corrections = np.zeros(self.bucket_shape(), dtype=np.float32) + self.fit_valid = np.zeros(self.bucket_shape(), dtype=bool) + self.preview_corrections = np.zeros(self.bucket_shape(), dtype=np.float32) + self.preview_valid = np.zeros(self.bucket_shape(), dtype=bool) + self.fit_speed_strength = np.zeros(len(self.SPEED_ANCHORS), dtype=np.float32) + + self.car_control_t = deque(maxlen=self.hist_len) + self.lat_active = deque(maxlen=self.hist_len) + self.roll_compensation = deque(maxlen=self.hist_len) + self.car_state_t = deque(maxlen=self.hist_len) + self.vego = deque(maxlen=self.hist_len) + self.steering_pressed = deque(maxlen=self.hist_len) + self.controls_state_t = deque(maxlen=self.hist_len) + self.model_desired_curvature = deque(maxlen=self.hist_len) + + self.last_lat_inactive_t = 0.0 + self.last_override_t = 0.0 + + self.current_bucket = (-1, -1) + self.current_correction = 0.0 + self.current_bias = 0.0 + self.current_bucket_points = 0 + + self.use_params = False + self.enable_curvatured = False + self.publish_debug_data = False + self.publish_preview_data = False + self.prev_use_params = None + self.last_status_log_t = 0.0 + self.fit_refresh_pending_rows: dict[int, set[int]] = {} + self.preview_refresh_pending_rows: dict[int, set[int]] = {} + self.live_pose_update_index = 0 + self.last_fit_refresh_update = -FIT_REFRESH_EVERY_N_UPDATES + self.last_preview_refresh_update = -PREVIEW_REFRESH_EVERY_N_UPDATES + + self._restore_cached_params() + self.update_use_params(force=True) + + cloudlog.info(f"curvatured init brand={self.CP.brand} fingerprint={self.CP.carFingerprint} " + f"steerControlType={self.CP.steerControlType} history={HISTORY:.2f}s") + + @staticmethod + def get_restore_key(CP: car.CarParams, version: int): + return (CP.carFingerprint, CP.brand, CP.steerControlType.raw, version) + + def _restore_cached_params(self) -> None: + params_cache = self.params.get("CarParamsPrevRoute") + curvature_cache = self.params.get("LiveCurvatureParameters") + if params_cache is None or curvature_cache is None: + return + + try: + with log.Event.from_bytes(curvature_cache) as log_evt: + cache_lcp = log_evt.liveCurvatureParameters + with car.CarParams.from_bytes(params_cache) as msg: + cache_CP = msg + + if self.get_restore_key(cache_CP, cache_lcp.version) != self.get_restore_key(self.CP, VERSION): + return + + biases = list(cache_lcp.biases) + counts = list(cache_lcp.counts) + if len(biases) != self.total_size() or len(counts) != self.total_size(): + raise ValueError("invalid curvature cache shape") + + self.bias = self.unflatten_bucket(biases).astype(np.float32) + self.counts = self.unflatten_bucket(counts).astype(np.float32) + self.fit_corrections, self.fit_valid = self.build_fit_corrections(self.bias, self.counts) + self.preview_corrections, self.preview_valid = self.build_preview_corrections(self.bias, self.counts) + self.fit_speed_strength = np.asarray([self.speed_curve_strength(self.counts[speed_idx], speed_idx) + for speed_idx in range(len(self.SPEED_ANCHORS))], dtype=np.float32) + cloudlog.info("restored curvature params from cache") + except Exception: + cloudlog.exception("failed to restore cached curvature params") + self.params.remove("LiveCurvatureParameters") + + def update_use_params(self, force: bool = False): + if force or self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: + self.enable_curvatured = self.params.get_bool("EnableCurvatureD") + self.publish_debug_data = self.params.get_bool("CurvatureDDebugData") + self.publish_preview_data = self.publish_debug_data or self.params.get_bool("ShowDynamicSteeringLearnerGraph") + self.use_params = self.enable_curvatured and self.CP.brand in ALLOWED_CARS and \ + self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED + if self.prev_use_params != self.use_params: + cloudlog.info(f"curvatured use_params={self.use_params} toggle={self.enable_curvatured} " + f"brand={self.CP.brand} allowed={self.CP.brand in ALLOWED_CARS} " + f"steerControlType={self.CP.steerControlType}") + self.prev_use_params = self.use_params + if not self.use_params: + self.current_bucket = (-1, -1) + self.current_correction = 0.0 + self.current_bias = 0.0 + self.current_bucket_points = 0 + if self.prev_use_params: + for d in [self.car_control_t, self.lat_active, self.roll_compensation, + self.car_state_t, self.vego, self.steering_pressed, + self.controls_state_t, self.model_desired_curvature]: + d.clear() + self.last_lat_inactive_t = 0.0 + self.last_override_t = 0.0 + self.frame += 1 + + def _history_ready(self) -> bool: + return min(len(self.car_control_t), len(self.car_state_t), len(self.controls_state_t)) == self.hist_len + + @staticmethod + def _sample_at_or_before(target_t: float, ts: deque, values: deque): + if len(ts) == 0: + return None + if target_t < ts[0]: + return None + + for i in range(len(ts) - 1, -1, -1): + if ts[i] <= target_t: + return values[i] + return None + + def add_measurement(self, desired_curvature: float, actual_curvature: float, v_ego: float, + schedule_only: bool = False) -> 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: + 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: + if weight <= 0.0: + continue + + prev_count = float(self.counts[speed_idx, curvature_idx]) + sample_count = min(prev_count + float(weight), self.MAX_SAMPLES) + delta = sample_count - prev_count + if delta <= 0.0: + continue + + self.counts[speed_idx, curvature_idx] = sample_count + alpha = delta / min(sample_count, self.MEAN_WINDOW) + prev_bias = float(self.bias[speed_idx, curvature_idx]) + self.bias[speed_idx, curvature_idx] = prev_bias + alpha * (error - prev_bias) + self._mark_curve_refresh_pending(speed_idx, curvature_idx) + + if not schedule_only: + self.refresh_curve_lookups(self.live_pose_update_index, force_fit=True, force_preview=True) + + def _mark_curve_refresh_pending(self, speed_idx: int, curvature_idx: int) -> None: + self.fit_refresh_pending_rows.setdefault(speed_idx, set()).add(curvature_idx) + self.preview_refresh_pending_rows.setdefault(speed_idx, set()).add(curvature_idx) + + def _row_bucket_caps(self, speed_idx: int, apply_cap: bool) -> np.ndarray: + if not apply_cap: + return np.full(len(self.CURVATURE_BUCKET_CENTERS), np.inf, dtype=np.float32) + return np.asarray([self.correction_cap(float(curvature), float(self.SPEED_ANCHORS[speed_idx])) + for curvature in self.CURVATURE_BUCKET_CENTERS], dtype=np.float32) + + def _row_curve_valid(self, speed_idx: int, valid_mask_fn, min_valid_buckets_fn, apply_cap: bool) -> np.ndarray: + curve_valid = np.asarray(valid_mask_fn(self.counts[speed_idx]), dtype=bool) + if apply_cap: + curve_valid &= self.apply_bucket_mask(speed_idx) + if int(np.count_nonzero(curve_valid)) < int(min_valid_buckets_fn(speed_idx)): + return np.zeros(len(self.CURVATURE_BUCKET_CENTERS), dtype=bool) + return curve_valid + + @staticmethod + def _merge_bounds(bounds: list[tuple[int, int]]) -> list[tuple[int, int]]: + if len(bounds) == 0: + return [] + bounds = sorted(bounds) + merged = [bounds[0]] + for start, end in bounds[1:]: + prev_start, prev_end = merged[-1] + if start <= prev_end + 1: + merged[-1] = (prev_start, max(prev_end, end)) + else: + merged.append((start, end)) + return merged + + def _affected_run_bounds(self, previous_valid: np.ndarray, curve_valid: np.ndarray, + changed_indices: set[int]) -> list[tuple[int, int]]: + bounds: list[tuple[int, int]] = [] + for idx in changed_indices: + bounds.append((max(0, idx - 1), min(len(self.CURVATURE_BUCKET_CENTERS) - 1, idx + 1))) + for mask in (previous_valid, curve_valid): + for start, end in self.valid_runs(mask): + if any(start <= idx <= end for idx in changed_indices): + bounds.append((start, end)) + return self._merge_bounds(bounds) + + def _run_values(self, speed_idx: int, run_idx: np.ndarray, speed_strength: float, + local_strength_fn, bucket_caps: np.ndarray, apply_cap: bool) -> np.ndarray: + run_curve = np.clip(self.bias[speed_idx, run_idx], -bucket_caps[run_idx], bucket_caps[run_idx]).astype(np.float32) + run_strength = local_strength_fn(self.counts[speed_idx], run_idx).astype(np.float32) + + if len(run_curve) >= 3: + smoothed_run = run_curve.copy() + smoothed_run[1:-1] = 0.25 * run_curve[:-2] + 0.5 * run_curve[1:-1] + 0.25 * run_curve[2:] + else: + smoothed_run = run_curve + + run_values = speed_strength * run_strength * smoothed_run + return np.clip(run_values, -bucket_caps[run_idx], bucket_caps[run_idx]) if apply_cap else run_values + + def _refresh_row(self, speed_idx: int, changed_indices: set[int], + valid_mask_fn, + min_valid_buckets_fn, + local_strength_fn, + speed_strength_fn, + apply_cap: bool, + zero_invalid_buckets: bool, + previous_row: np.ndarray, + previous_valid: np.ndarray, + previous_speed_strength: float) -> tuple[np.ndarray, np.ndarray, float]: + curve_valid = self._row_curve_valid(speed_idx, valid_mask_fn, min_valid_buckets_fn, apply_cap) + bucket_caps = self._row_bucket_caps(speed_idx, apply_cap) + + if not curve_valid.any(): + return np.zeros(len(self.CURVATURE_BUCKET_CENTERS), dtype=np.float32), curve_valid, 0.0 + + valid_idx = np.flatnonzero(curve_valid) + local_strength = local_strength_fn(self.counts[speed_idx], valid_idx) + speed_strength = float(speed_strength_fn(self.counts[speed_idx], speed_idx, valid_idx, local_strength)) + force_full = not np.isclose(speed_strength, previous_speed_strength) + + if force_full: + row = np.zeros(len(self.CURVATURE_BUCKET_CENTERS), dtype=np.float32) + rebuild_bounds = self.valid_runs(curve_valid) + else: + row = previous_row.copy() + rebuild_bounds = self._affected_run_bounds(previous_valid, curve_valid, changed_indices) + + for start, end in rebuild_bounds: + row[start:end + 1] = 0.0 + + current_runs = self.valid_runs(curve_valid) + for start, end in current_runs: + if not force_full and all(end < bound_start or start > bound_end for bound_start, bound_end in rebuild_bounds): + continue + run_idx = np.arange(start, end + 1) + row[run_idx] = self._run_values(speed_idx, run_idx, speed_strength, local_strength_fn, bucket_caps, apply_cap) + + if zero_invalid_buckets: + row = np.where(curve_valid, row, 0.0) + + return row.astype(np.float32), curve_valid, speed_strength + + def refresh_curve_lookups(self, update_index: int, force_fit: bool = False, force_preview: bool = False) -> None: + fit_due = force_fit or ((update_index - self.last_fit_refresh_update) >= FIT_REFRESH_EVERY_N_UPDATES) + preview_due = force_preview or ((update_index - self.last_preview_refresh_update) >= PREVIEW_REFRESH_EVERY_N_UPDATES) + + if fit_due and self.fit_refresh_pending_rows: + for speed_idx, changed_indices in list(self.fit_refresh_pending_rows.items()): + row, valid, speed_strength = self._refresh_row( + speed_idx, + changed_indices, + lambda speed_counts: speed_counts >= self.MIN_BUCKET_POINTS, + lambda _speed_idx: 1, + self.fit_local_strength, + lambda speed_counts, row_idx, _valid_idx, _local_strength: self.speed_curve_strength(speed_counts, row_idx), + True, + True, + self.fit_corrections[speed_idx], + self.fit_valid[speed_idx], + float(self.fit_speed_strength[speed_idx]), + ) + self.fit_corrections[speed_idx] = row + self.fit_valid[speed_idx] = valid + self.fit_speed_strength[speed_idx] = speed_strength + self.fit_refresh_pending_rows.clear() + self.last_fit_refresh_update = update_index + + if preview_due: + if (self.publish_preview_data or force_preview) and self.preview_refresh_pending_rows: + for speed_idx, changed_indices in list(self.preview_refresh_pending_rows.items()): + row, valid, _ = self._refresh_row( + speed_idx, + changed_indices, + lambda speed_counts: speed_counts > 0.0, + lambda _speed_idx: 1, + self.preview_local_strength, + lambda _all_counts, _speed_idx, _valid_idx, _local_strength: 1.0, + False, + False, + self.preview_corrections[speed_idx], + self.preview_valid[speed_idx], + 1.0, + ) + self.preview_corrections[speed_idx] = row + self.preview_valid[speed_idx] = valid + # Always clear pending rows even if not publishing, to prevent unbounded growth + self.preview_refresh_pending_rows.clear() + self.last_preview_refresh_update = update_index + + + def _update_current_lookup(self, desired_curvature: float, v_ego: float) -> None: + idx = self.indices(desired_curvature, v_ego) + if idx is None: + self.current_bucket = (-1, -1) + self.current_correction = 0.0 + self.current_bias = 0.0 + self.current_bucket_points = 0 + return + + speed_idx, curvature_idx = idx + self.current_bucket = idx + 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.fit_valid[speed_idx].any(): + 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)) + self.current_correction = float(direction * projected) + + def handle_log(self, t: float, which: str, msg) -> None: + if not self.use_params: + if which == "liveCalibration": + self.calibrator.feed_live_calib(msg) + elif which == "liveDelay": + self.lag = get_lat_delay(self.params, msg.lateralDelay) + return + + if which == "carControl": + self.car_control_t.append(t) + self.lat_active.append(msg.latActive) + self.roll_compensation.append(msg.rollCompensation) + if not msg.latActive: + self.last_lat_inactive_t = t + elif which == "carState": + steering_override = bool(msg.steeringPressed or msg.steeringSlightlyPressed) + self.car_state_t.append(t) + self.vego.append(msg.vEgo) + self.steering_pressed.append(steering_override) + if steering_override: + self.last_override_t = t + elif which == "controlsState": + self.controls_state_t.append(t) + self.model_desired_curvature.append(msg.modelDesiredCurvature) + if self.car_state_t: + self._update_current_lookup(self.model_desired_curvature[-1], self.vego[-1]) + elif which == "liveCalibration": + self.calibrator.feed_live_calib(msg) + elif which == "liveDelay": + self.lag = get_lat_delay(self.params, msg.lateralDelay) + elif which == "livePose" and self.use_params: + self.live_pose_update_index += 1 + if not self._history_ready(): + return + if not (msg.angularVelocityDevice.valid and msg.posenetOK and msg.inputsOK and self.calibrator.calib_valid): + return + if (t - self.last_lat_inactive_t) < MIN_ENGAGE_BUFFER or (t - self.last_override_t) < MIN_ENGAGE_BUFFER: + return + + target_t = t - self.lag + lat_active = self._sample_at_or_before(target_t, self.car_control_t, self.lat_active) + roll_comp = self._sample_at_or_before(target_t, self.car_control_t, self.roll_compensation) + steering_pressed = self._sample_at_or_before(target_t, self.car_state_t, self.steering_pressed) + v_ego = self._sample_at_or_before(target_t, self.car_state_t, self.vego) + desired_curvature = self._sample_at_or_before(target_t, self.controls_state_t, self.model_desired_curvature) + + if any(x is None for x in (lat_active, roll_comp, steering_pressed, v_ego, desired_curvature)): + return + + if not bool(lat_active) or bool(steering_pressed) or float(v_ego) < self.MIN_SPEED: + return + + device_pose = Pose.from_live_pose(msg) + if not self.roll_learning_allowed(device_pose.orientation.roll): + return + calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) + yaw_rate = calibrated_pose.angular_velocity.yaw + yaw_rate_std = calibrated_pose.angular_velocity.yaw_std + if yaw_rate_std >= MAX_YAW_RATE_STD: + return + + v_ego = float(v_ego) + 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.refresh_curve_lookups(self.live_pose_update_index) + + def get_msg(self, valid: bool = True, live_valid: bool = True, + include_debug: bool = False, include_preview: bool = False): + msg = messaging.new_message('liveCurvatureParameters') + msg.valid = valid + + curvature_params = msg.liveCurvatureParameters + curvature_params.liveValid = bool(live_valid) and bool(np.isfinite(self.bias).all()) and bool(np.isfinite(self.fit_corrections).all()) + curvature_params.version = VERSION + curvature_params.useParams = self.use_params + curvature_params.currentCorrection = self.current_correction if self.use_params else 0.0 + curvature_params.currentBias = self.current_bias if self.use_params else 0.0 + curvature_params.currentBucketPoints = self.current_bucket_points if self.use_params else 0 + curvature_params.totalBucketPoints = int(round(float(self.counts.sum()))) + curvature_params.calPerc = self.calibration_percent(self.counts) + curvature_params.bucketSpeed = int(self.current_bucket[0]) if self.use_params else -1 + curvature_params.bucketCurvature = int(self.current_bucket[1]) if self.use_params else -1 + curvature_params.corrections = self.flatten(self.fit_corrections) + curvature_params.fitValid = self.flatten(self.fit_valid) + if include_debug: + curvature_params.counts = self.flatten(np.rint(self.counts).astype(np.uint16)) + curvature_params.biases = self.flatten(self.bias) + if include_preview: + curvature_params.previewCorrections = self.flatten(self.preview_corrections) + curvature_params.previewValid = self.flatten(self.preview_valid) + return msg + + @staticmethod + def roll_learning_allowed(roll: float) -> bool: + return abs(np.sin(float(roll)) * ACCELERATION_DUE_TO_GRAVITY) <= MAX_LEARN_ROLL_LATERAL_ACCEL + + def maybe_log_status(self, t: float, sm, services: list[str] | None = None, valid: bool | None = None) -> None: + if t < self.last_status_log_t + STATUS_LOG_INTERVAL: + return + + tracked_services = list(sm.valid.keys()) if services is None else services + invalid = [s for s in tracked_services if not sm.valid[s]] + not_alive = [s for s in tracked_services if not sm.alive[s]] + self.last_status_log_t = t + + checks = sm.all_checks(tracked_services) if valid is None else valid + cloudlog.info(f"curvatured status use_params={self.use_params} checks={checks} " + 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"invalid={invalid} not_alive={not_alive}") + + +def main(): + config_realtime_process([0, 1, 2, 3], 5) + + pm = messaging.PubMaster(['liveCurvatureParameters']) + sm = messaging.SubMaster(['carControl', 'carState', 'liveCalibration', 'livePose', 'liveDelay', 'controlsState'], poll='livePose') + + params = Params() + CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) + curvature_estimator = CurvatureEstimator(CP) + + while True: + sm.update() + + if sm.all_checks(): + for which in sm.updated.keys(): + if sm.updated[which]: + t = sm.logMonoTime[which] * 1e-9 + try: + curvature_estimator.handle_log(t, which, sm[which]) + except Exception: + cloudlog.exception(f"curvatured handle_log failed service={which}") + + curvature_estimator.update_use_params() + + # 4Hz driven by livePose + if sm.frame % 5 == 0: + live_valid = sm.all_checks() and curvature_estimator.use_params + curvature_estimator.maybe_log_status(time.monotonic(), sm) + pm.send('liveCurvatureParameters', + curvature_estimator.get_msg(valid=sm.all_checks(), + live_valid=live_valid, + include_debug=curvature_estimator.publish_debug_data, + include_preview=curvature_estimator.publish_preview_data)) + + # Cache params every 60 seconds + if sm.frame % 240 == 0: + live_valid = sm.all_checks() and curvature_estimator.use_params + params.put_nonblocking("LiveCurvatureParameters", + curvature_estimator.get_msg(valid=sm.all_checks(), + live_valid=live_valid, + include_debug=True, + include_preview=False).to_bytes()) + + +if __name__ == "__main__": + main() + diff --git a/selfdrive/locationd/test/test_curvatured.py b/selfdrive/locationd/test/test_curvatured.py new file mode 100644 index 000000000..aa707583e --- /dev/null +++ b/selfdrive/locationd/test/test_curvatured.py @@ -0,0 +1,343 @@ +import numpy as np + +from cereal import car + +from opendbc.car.volkswagen.values import CAR +from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY +from openpilot.selfdrive.locationd.curvatured import CurvatureEstimator, CurvatureDLookup, MAX_LEARN_ROLL_LATERAL_ACCEL + + +def get_estimator(): + CP = car.CarParams.new_message() + CP.carFingerprint = CAR.CUPRA_BORN_MK1 + CP.brand = "volkswagen" + CP.steerControlType = car.CarParams.SteerControlType.curvatureDEPRECATED + return CurvatureEstimator(CP) + + +class TestCurvatureEstimator: + @staticmethod + def _train_speed_curve(estimator, v_ego: float): + for desired_curvature in CurvatureDLookup.CURVATURE_BUCKET_CENTERS: + for sign in (-1.0, 1.0): + for _ in range(int(CurvatureDLookup.MIN_BUCKET_POINTS[CurvatureDLookup.curvature_index(float(desired_curvature))]) + 2): + desired = sign * float(desired_curvature) + estimator.add_measurement(desired, desired * 0.6, v_ego) + + @staticmethod + def _train_speed_curve_full(estimator, v_ego: float): + for desired_curvature in CurvatureDLookup.CURVATURE_BUCKET_CENTERS: + for sign in (-1.0, 1.0): + curvature_idx = CurvatureDLookup.curvature_index(float(desired_curvature)) + assert curvature_idx is not None + for _ in range(int(CurvatureDLookup.FULL_BUCKET_STRENGTH_SAMPLES[curvature_idx]) + 2): + desired = sign * float(desired_curvature) + estimator.add_measurement(desired, desired * 0.6, v_ego) + + def test_left_and_right_feed_the_same_bucket_curve(self): + estimator = get_estimator() + desired_curvature = 32e-6 + v_ego = 22.0 + + for _ in range(40): + estimator.add_measurement(desired_curvature, desired_curvature * 0.7, v_ego) + estimator.add_measurement(-desired_curvature, -desired_curvature * 0.7, v_ego) + + curvature_idx = CurvatureDLookup.curvature_index(desired_curvature) + assert curvature_idx is not None + speed_weights = CurvatureDLookup.learning_speed_weights(v_ego) + assert len(speed_weights) == 2 + + total = 0.0 + for speed_idx, weight in speed_weights: + bucket_count = float(estimator.counts[speed_idx, curvature_idx]) + total += bucket_count + assert np.isclose(bucket_count, 80.0 * weight) + assert estimator.bias[speed_idx, curvature_idx] > 0.0 + + assert np.isclose(total, 80.0) + + def test_learning_is_weighted_between_neighbor_speed_anchors(self): + estimator = get_estimator() + desired_curvature = 32e-6 + low_speed = float(CurvatureDLookup.SPEED_ANCHORS[2]) + high_speed = float(CurvatureDLookup.SPEED_ANCHORS[3]) + v_ego = 0.25 * low_speed + 0.75 * high_speed + + estimator.add_measurement(desired_curvature, desired_curvature * 0.6, v_ego) + + curvature_idx = CurvatureDLookup.curvature_index(desired_curvature) + assert curvature_idx is not None + speed_weights = CurvatureDLookup.learning_speed_weights(v_ego) + assert len(speed_weights) == 2 + + for speed_idx, weight in speed_weights: + assert np.isclose(float(estimator.counts[speed_idx, curvature_idx]), weight) + + def test_preview_is_not_apply_capped(self): + estimator = get_estimator() + desired_curvature = 2.048e-3 + actual_curvature = 0.0 + v_ego = float(CurvatureDLookup.SPEED_ANCHORS[-1]) + + for _ in range(80): + estimator.add_measurement(desired_curvature, actual_curvature, v_ego) + + idx = CurvatureDLookup.indices(desired_curvature, v_ego) + assert idx is not None + speed_idx, curvature_idx = idx + assert estimator.bias[idx] > CurvatureDLookup.correction_cap(desired_curvature, v_ego) + assert estimator.preview_valid[speed_idx, curvature_idx] + assert estimator.preview_corrections[speed_idx, curvature_idx] > estimator.fit_corrections[speed_idx, curvature_idx] + + def test_learning_error_is_capped_to_full_ratio(self): + estimator = get_estimator() + desired_curvature = 2.048e-3 + actual_curvature = -5.0e-3 + v_ego = float(CurvatureDLookup.SPEED_ANCHORS[-1]) + + for _ in range(CurvatureDLookup.MAX_SAMPLES): + estimator.add_measurement(desired_curvature, actual_curvature, v_ego) + + idx = CurvatureDLookup.indices(desired_curvature, v_ego) + assert idx is not None + assert estimator.bias[idx] <= CurvatureDLookup.learning_error_cap(desired_curvature) + 1e-9 + + def test_schedule_only_learning_refreshes_on_flush(self): + estimator = get_estimator() + desired_curvature = 32e-6 + v_ego = 22.0 + + estimator.add_measurement(desired_curvature, desired_curvature * 0.6, v_ego, schedule_only=True) + + idx = CurvatureDLookup.indices(desired_curvature, v_ego) + assert idx is not None + assert estimator.counts[idx] > 0.0 + assert estimator.fit_corrections[idx] == 0.0 + assert not estimator.preview_valid[idx] + + estimator.refresh_curve_lookups(1, force_fit=True, force_preview=True) + + assert estimator.preview_valid[idx] + assert estimator.preview_corrections[idx] > 0.0 + + def test_relative_correction_cap_envelope_fades_after_last_supported_bucket(self): + v_ego = float(CurvatureDLookup.SPEED_ANCHORS[5]) + max_bucket_idx = CurvatureDLookup.max_supported_bucket_index(v_ego) + assert max_bucket_idx is not None + assert max_bucket_idx < len(CurvatureDLookup.CURVATURE_BUCKET_CENTERS) + + inner_idx = 0 + supported_curvature = float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[max_bucket_idx]) + fade_end_curvature = float(CurvatureDLookup.cap_zero_curvature(v_ego)) + beyond_curvature = min(fade_end_curvature * 1.05, CurvatureDLookup.CURVATURE_MAX) + + inner_curvature = float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[inner_idx]) + + assert np.isclose(CurvatureDLookup.correction_cap_ratio(inner_curvature, v_ego), CurvatureDLookup.RELATIVE_CAP_FULL_RATIO) + assert np.isclose(CurvatureDLookup.correction_cap_ratio(supported_curvature, v_ego), CurvatureDLookup.RELATIVE_CAP_FULL_RATIO) + assert CurvatureDLookup.correction_cap_ratio(0.5 * (supported_curvature + fade_end_curvature), v_ego) < CurvatureDLookup.RELATIVE_CAP_FULL_RATIO + assert CurvatureDLookup.correction_cap_ratio(beyond_curvature, v_ego) == 0.0 + + def test_calibration_percent_tracks_valid_speed_curves(self): + estimator = get_estimator() + assert estimator.get_msg().liveCurvatureParameters.calPerc == 0 + + for v_ego in CurvatureDLookup.SPEED_ANCHORS: + self._train_speed_curve_full(estimator, float(v_ego)) + + assert estimator.get_msg().liveCurvatureParameters.calPerc == 100 + + def test_required_support_bucket_count_decreases_with_speed(self): + low = CurvatureDLookup.required_support_bucket_count(0) + mid = CurvatureDLookup.required_support_bucket_count(3) + high = CurvatureDLookup.required_support_bucket_count(6) + + assert low == len(CurvatureDLookup.CURVATURE_BUCKET_CENTERS) + assert low >= mid >= high >= CurvatureDLookup.MIN_REQUIRED_SUPPORT_BUCKETS + + def test_fit_valid_no_longer_requires_global_total_samples(self): + speed_idx = 3 + bucket_idx = 5 + counts = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + bias = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + + counts[speed_idx, bucket_idx] = float(CurvatureDLookup.MIN_BUCKET_POINTS[bucket_idx] + 1.0) + bias[speed_idx, bucket_idx] = float(0.5 * CurvatureDLookup.correction_cap( + float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[bucket_idx]), + float(CurvatureDLookup.SPEED_ANCHORS[speed_idx]), + )) + + filler_idx = np.arange(CurvatureDLookup.required_support_bucket_count(speed_idx), dtype=int) + filler_idx = filler_idx[filler_idx != bucket_idx] + counts[speed_idx, filler_idx] = CurvatureDLookup.MIN_BUCKET_POINTS[filler_idx] + 1.0 + + fit_corrections, fit_valid = CurvatureDLookup.build_fit_corrections(bias, counts) + + assert fit_valid[speed_idx, bucket_idx] + assert float(fit_corrections[speed_idx, bucket_idx]) > 0.0 + + def test_calibration_percent_requires_full_local_strength(self): + speed_idx = 3 + counts = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + required = CurvatureDLookup.required_support_bucket_count(speed_idx) + selected = np.arange(required, dtype=int) + + counts[speed_idx, selected] = CurvatureDLookup.MIN_BUCKET_POINTS[selected] + assert np.all(counts[speed_idx, selected] >= CurvatureDLookup.MIN_BUCKET_POINTS[selected]) + assert not CurvatureDLookup.speed_curve_valid(counts, speed_idx) + assert not CurvatureDLookup.speed_curve_fully_calibrated(counts, speed_idx) + assert CurvatureDLookup.calibration_percent(counts) == 0 + + counts[speed_idx, selected] = CurvatureDLookup.FULL_BUCKET_STRENGTH_SAMPLES[selected] + assert CurvatureDLookup.speed_curve_fully_calibrated(counts, speed_idx) + assert CurvatureDLookup.calibration_percent(counts) > 0 + + def test_speed_curve_strength_grows_smoothly_from_bucket_strengths(self): + speed_idx = 4 + counts = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + required = CurvatureDLookup.required_support_bucket_count(speed_idx) + selected = np.arange(required, dtype=int) + + counts[speed_idx, selected] = CurvatureDLookup.MIN_BUCKET_POINTS[selected] + assert np.isclose(CurvatureDLookup.speed_curve_strength(counts[speed_idx], speed_idx), 0.0) + + counts[speed_idx, selected] = CurvatureDLookup.MIN_BUCKET_POINTS[selected] + 0.5 * ( + CurvatureDLookup.FULL_BUCKET_STRENGTH_SAMPLES[selected] - CurvatureDLookup.MIN_BUCKET_POINTS[selected] + ) + assert np.isclose(CurvatureDLookup.speed_curve_strength(counts[speed_idx], speed_idx), 0.5) + + def test_message_contains_symmetric_fit_curve(self): + estimator = get_estimator() + desired_curvature = 32e-6 + v_ego = 22.0 + + self._train_speed_curve(estimator, v_ego) + estimator._update_current_lookup(desired_curvature, v_ego) + msg = estimator.get_msg(include_debug=True, include_preview=True) + idx = CurvatureDLookup.indices(desired_curvature, v_ego) + + assert idx is not None + assert msg.liveCurvatureParameters.bucketSpeed == idx[0] + assert msg.liveCurvatureParameters.bucketCurvature == idx[1] + assert msg.liveCurvatureParameters.currentCorrection > 0.0 + assert len(msg.liveCurvatureParameters.corrections) == CurvatureDLookup.total_size() + assert len(msg.liveCurvatureParameters.counts) == CurvatureDLookup.total_size() + assert len(msg.liveCurvatureParameters.biases) == CurvatureDLookup.total_size() + assert len(msg.liveCurvatureParameters.fitValid) == CurvatureDLookup.total_size() + assert len(msg.liveCurvatureParameters.previewCorrections) == CurvatureDLookup.total_size() + assert len(msg.liveCurvatureParameters.previewValid) == CurvatureDLookup.total_size() + + def test_fit_valid_allows_noncontiguous_supported_buckets(self): + estimator = get_estimator() + speed_idx = len(CurvatureDLookup.SPEED_ANCHORS) - 1 + v_ego = float(CurvatureDLookup.SPEED_ANCHORS[speed_idx]) + required = CurvatureDLookup.required_support_bucket_count(speed_idx) + selected_indices = list(range(0, required - 1)) + [required] + + for bucket_idx in selected_indices: + desired_curvature = float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[bucket_idx]) + for _ in range(int(CurvatureDLookup.MIN_BUCKET_POINTS[bucket_idx]) + 120): + estimator.add_measurement(desired_curvature, desired_curvature * 0.6, v_ego) + + msg = estimator.get_msg().liveCurvatureParameters + fit_valid = CurvatureDLookup.unflatten_bucket(list(msg.fitValid), dtype=bool) + + assert fit_valid[speed_idx, selected_indices].all() + assert not fit_valid[speed_idx, required - 1] + + def test_fit_corrections_are_zero_outside_fit_valid(self): + speed_idx = 3 + counts = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + bias = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + selected = np.array([5, 6, 7, 8], dtype=int) + + counts[speed_idx, selected] = CurvatureDLookup.MIN_BUCKET_POINTS[selected] + 40.0 + bias[speed_idx, selected] = np.array([2.0e-6, 6.0e-6, 1.2e-5, 2.0e-5], dtype=np.float32) + + fit_corrections, fit_valid = CurvatureDLookup.build_fit_corrections(bias, counts) + + assert fit_valid[speed_idx, selected].all() + assert np.allclose(fit_corrections[speed_idx, ~fit_valid[speed_idx]], 0.0) + + def test_outer_learned_buckets_stay_invalid_for_apply(self): + speed_idx = len(CurvatureDLookup.SPEED_ANCHORS) - 1 + v_ego = float(CurvatureDLookup.SPEED_ANCHORS[speed_idx]) + outer_idx = len(CurvatureDLookup.CURVATURE_BUCKET_CENTERS) - 1 + counts = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + bias = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + + counts[speed_idx, outer_idx] = CurvatureDLookup.MIN_BUCKET_POINTS[outer_idx] + 40.0 + bias[speed_idx, outer_idx] = 8.0e-5 + + fit_corrections, fit_valid = CurvatureDLookup.build_fit_corrections(bias, counts) + preview_corrections, preview_valid = CurvatureDLookup.build_preview_corrections(bias, counts) + + assert not fit_valid[speed_idx, outer_idx] + assert fit_corrections[speed_idx, outer_idx] == 0.0 + assert preview_valid[speed_idx, outer_idx] + assert preview_corrections[speed_idx, outer_idx] > 0.0 + + def test_interp_curve_value_does_not_bridge_invalid_gap(self): + 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) + + 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 + + gap_curvature = float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[4]) + valid_curvature = float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[3]) + + assert CurvatureDLookup.interp_curve_value(fit_corrections, fit_valid, v_ego, gap_curvature) == 0.0 + assert CurvatureDLookup.interp_curve_value(fit_corrections, fit_valid, v_ego, valid_curvature) > 0.0 + + def test_preview_build_keeps_separate_runs_independent(self): + speed_idx = 3 + counts = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + bias = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + left_idx = 3 + right_idx = 6 + + counts[speed_idx, left_idx] = 1.0 + counts[speed_idx, right_idx] = 1.0 + bias[speed_idx, left_idx] = 1.0e-6 + bias[speed_idx, right_idx] = 8.0e-6 + + preview_corrections, preview_valid = CurvatureDLookup.build_preview_corrections(bias, counts) + + assert preview_valid[speed_idx, left_idx] + assert preview_valid[speed_idx, right_idx] + assert np.isclose(preview_corrections[speed_idx, left_idx], bias[speed_idx, left_idx]) + assert np.isclose(preview_corrections[speed_idx, right_idx], bias[speed_idx, right_idx]) + + def test_learning_is_blocked_for_larger_roll(self): + estimator = get_estimator() + + small_roll = np.arcsin(0.5 * MAX_LEARN_ROLL_LATERAL_ACCEL / ACCELERATION_DUE_TO_GRAVITY) + large_roll = np.arcsin(1.5 * MAX_LEARN_ROLL_LATERAL_ACCEL / ACCELERATION_DUE_TO_GRAVITY) + + assert estimator.roll_learning_allowed(float(small_roll)) + assert not estimator.roll_learning_allowed(float(large_roll)) + + def test_actual_curvature_subtracts_roll_compensation(self): + yaw_rate = 0.03 + v_ego = 20.0 + roll_comp = 4.0e-4 + + raw_curvature = yaw_rate / v_ego + corrected_curvature = CurvatureDLookup.actual_curvature_from_yaw_rate(yaw_rate, v_ego, roll_comp) + + assert np.isclose(corrected_curvature, raw_curvature - roll_comp) + + def test_slight_steering_press_blocks_learning_like_override(self): + estimator = get_estimator() + estimator.use_params = True + + estimator.handle_log(12.0, "carState", car.CarState(vEgo=20.0, steeringPressed=False, steeringSlightlyPressed=True)) + + assert estimator.steering_pressed[-1] + assert estimator.last_override_t == 12.0 diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 28731d2ad..c391bf7f5 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -287,4 +287,4 @@ if __name__ == "__main__": parser = argparse.ArgumentParser(description='Process the --demo argument.') parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.') args = parser.parse_args() - main(demo=args.demo) + main(demo=args.demo) \ No newline at end of file diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index dcee71b77..566674ec1 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -91,6 +91,8 @@ class SelfdriveD(CruiseHelper): self.car_state_sock = messaging.sub_sock('carState', timeout=20) ignore = self.sensor_packets + self.gps_packets + ['alertDebug', 'lateralManeuverPlan'] + ['modelDataV2SP'] + if not Params().get_bool("EnableCurvatureD"): + ignore += ['liveCurvatureParameters'] if SIMULATION: ignore += ['driverCameraState', 'managerState'] if REPLAY: @@ -98,7 +100,7 @@ class SelfdriveD(CruiseHelper): ignore += ['roadCameraState', 'wideRoadCameraState'] self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay', - 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', + 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'liveCurvatureParameters', 'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback', 'lateralManeuverPlan', 'modelDataV2SP', 'longitudinalPlanSP'] + \ self.camera_packets + self.sensor_packets + self.gps_packets, diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 65587d44b..b2c3f4501 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -440,6 +440,7 @@ CONFIGS = [ "carState", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState", "longitudinalPlan", "livePose", "liveDelay", "liveParameters", "radarState", "modelV2", "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState", "liveTorqueParameters", + "liveCurvatureParameters", "accelerometer", "gyroscope", "carOutput", "gpsLocationExternal", "gpsLocation", "controlsState", "carControl", "driverAssistance", "alertDebug", "audioFeedback", ], @@ -453,7 +454,7 @@ CONFIGS = [ ), ProcessConfig( proc_name="controlsd", - pubs=["liveParameters", "liveTorqueParameters", "modelV2", "selfdriveState", + pubs=["liveParameters", "liveTorqueParameters", "liveCurvatureParameters", "modelV2", "selfdriveState", "liveCalibration", "livePose", "longitudinalPlan", "carState", "carOutput", "driverMonitoringState", "onroadEvents", "driverAssistance"], subs=["carControl", "controlsState"], @@ -552,6 +553,15 @@ CONFIGS = [ should_recv_callback=MessageBasedRcvCallback("livePose", True), tolerance=NUMPY_TOLERANCE, ), + ProcessConfig( + proc_name="curvatured", + pubs=["livePose", "liveCalibration", "liveDelay", "carState", "carControl", "controlsState"], + subs=["liveCurvatureParameters"], + ignore=["logMonoTime"], + init_callback=get_car_params_callback, + should_recv_callback=MessageBasedRcvCallback("livePose", True), + tolerance=NUMPY_TOLERANCE, + ), ProcessConfig( proc_name="modeld", pubs=["deviceState", "roadCameraState", "wideRoadCameraState", "liveCalibration", "liveDelay", "driverMonitoringState", "carState", "carControl"], @@ -591,13 +601,15 @@ def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") -> """ Use this to get custom params dict based on provided logs. Useful when replaying following processes: calibrationd, paramsd, torqued - The params may be based on first or last message of given type (carParams, liveCalibration, liveParameters, liveTorqueParameters) in the logs. + The params may be based on first or last message of given type + (carParams, liveCalibration, liveParameters, liveTorqueParameters, liveCurvatureParameters) in the logs. """ car_params = [m for m in lr if m.which() == "carParams"] live_calibration = [m for m in lr if m.which() == "liveCalibration"] live_parameters = [m for m in lr if m.which() == "liveParameters"] live_torque_parameters = [m for m in lr if m.which() == "liveTorqueParameters"] + live_curvature_parameters = [m for m in lr if m.which() == "liveCurvatureParameters"] assert initial_state in ["first", "last"] msg_index = 0 if initial_state == "first" else -1 @@ -615,6 +627,8 @@ def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") -> custom_params["LiveParametersV2"] = live_parameters[msg_index].as_builder().to_bytes() if len(live_torque_parameters) > 0: custom_params["LiveTorqueParameters"] = live_torque_parameters[msg_index].as_builder().to_bytes() + if len(live_curvature_parameters) > 0: + custom_params["LiveCurvatureParameters"] = live_curvature_parameters[msg_index].as_builder().to_bytes() return custom_params diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 1129a1a2f..62eb1782c 100644 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -50,6 +50,7 @@ PROCS = { "system.hardware.hardwared": 4.0, "selfdrive.locationd.calibrationd": 2.0, "selfdrive.locationd.torqued": 5.0, + "selfdrive.locationd.curvatured": 5.0, "selfdrive.locationd.locationd": 25.0, "selfdrive.locationd.paramsd": 9.0, "selfdrive.locationd.lagd": 11.0, diff --git a/selfdrive/ui/layouts/settings/device.py b/selfdrive/ui/layouts/settings/device.py index 126ad22a3..93ebe7d89 100644 --- a/selfdrive/ui/layouts/settings/device.py +++ b/selfdrive/ui/layouts/settings/device.py @@ -104,6 +104,7 @@ class DeviceLayout(Widget): return self._params.remove("CalibrationParams") + self._params.remove("LiveCurvatureParameters") self._params.remove("LiveTorqueParameters") self._params.remove("LiveParameters") self._params.remove("LiveParametersV2") diff --git a/selfdrive/ui/layouts/settings/ictoggles.py b/selfdrive/ui/layouts/settings/ictoggles.py index a0eef21d1..a4f1ccdac 100644 --- a/selfdrive/ui/layouts/settings/ictoggles.py +++ b/selfdrive/ui/layouts/settings/ictoggles.py @@ -15,6 +15,12 @@ DESCRIPTIONS = { "EnableCurvatureController": tr_noop( "Enables curvature PID post-processing additionally to QFK curvature offset" ), + "EnableCurvatureD": tr_noop( + "Learns speed- and curvature-dependent steering corrections around center for dynamic steering behavior. Experimental and only used on curvature-based steering paths." + ), + "ShowDynamicSteeringLearnerGraph": tr_noop( + "Display the current dynamic steering learner fit, marker, and status information in the onroad UI." + ), "EnableLongComfortMode": tr_noop( "Enables longitudinal jerk and accel deviation limit control for safe and comfortable driving" ), @@ -94,12 +100,6 @@ class ICTogglesLayout(Widget): "speed_limit.png", False, ), - "BatteryDetails": ( - lambda: tr("VW MEB: Display Battery Details"), - DESCRIPTIONS["BatteryDetails"], - "capslock-fill.png", - False, - ), "ForceRHDForBSM": ( lambda: tr("VW: Force RHD for BSM"), DESCRIPTIONS["ForceRHDForBSM"], @@ -130,10 +130,29 @@ class ICTogglesLayout(Widget): "eye_closed.png", False, ), + "BatteryDetails": ( + lambda: tr("VW MEB: Display Battery Details"), + DESCRIPTIONS["BatteryDetails"], + "capslock-fill.png", + False, + ), + "EnableCurvatureD": ( + lambda: tr("Enable Dynamic Steering Learner"), + DESCRIPTIONS["EnableCurvatureD"], + "chffr_wheel.png", + False, + ), + "ShowDynamicSteeringLearnerGraph": ( + lambda: tr("Show Dynamic Steering Learner Graph"), + DESCRIPTIONS["ShowDynamicSteeringLearnerGraph"], + "chffr_wheel.png", + False, + ), } self._toggles = {} self._locked_toggles = set() + self._offroad_only_toggles = {"EnableCurvatureD"} for param, (title, desc, icon, needs_restart) in self._toggle_defs.items(): toggle = toggle_item( title, @@ -164,6 +183,7 @@ class ICTogglesLayout(Widget): self._scroller = Scroller(list(self._toggles.values()), line_separator=True, spacing=0) ui_state.add_engaged_transition_callback(self._update_toggles) + ui_state.add_offroad_transition_callback(self._update_toggles) def _update_state(self): return @@ -186,6 +206,13 @@ class ICTogglesLayout(Widget): if self._toggle_defs[toggle_def][3] and toggle_def not in self._locked_toggles: self._toggles[toggle_def].action_item.set_enabled(not ui_state.engaged) + for toggle_def in self._offroad_only_toggles: + if toggle_def not in self._locked_toggles: + self._toggles[toggle_def].action_item.set_enabled(ui_state.is_offroad()) + + if "EnableCurvatureD" not in self._locked_toggles: + self._toggles["EnableCurvatureD"].action_item.set_enabled(ui_state.is_offroad()) + def _render(self, rect): self._scroller.render(rect) diff --git a/selfdrive/ui/mici/layouts/settings/device.py b/selfdrive/ui/mici/layouts/settings/device.py index 9bf04ae7c..53e05beb2 100644 --- a/selfdrive/ui/mici/layouts/settings/device.py +++ b/selfdrive/ui/mici/layouts/settings/device.py @@ -484,6 +484,7 @@ class DeviceLayoutMici(NavScroller): def reset_calibration_callback(): params = ui_state.params params.remove("CalibrationParams") + params.remove("LiveCurvatureParameters") params.remove("LiveTorqueParameters") params.remove("LiveParameters") params.remove("LiveParametersV2") diff --git a/selfdrive/ui/mici/layouts/settings/ictoggles.py b/selfdrive/ui/mici/layouts/settings/ictoggles.py index 9e4b62e2f..832a8bbf0 100644 --- a/selfdrive/ui/mici/layouts/settings/ictoggles.py +++ b/selfdrive/ui/mici/layouts/settings/ictoggles.py @@ -23,6 +23,8 @@ class ICTogglesLayoutMici(NavScroller): enable_dark_mode = BigParamControl("Dark Mode", "DarkMode") enable_onroad_screen_timer = BigParamControl("Onroad Screen Timeout", "DisableScreenTimer") enable_accel_bar = BigParamControl("Enable Accel Bar", "ShowAccelBar") + enable_curvatured = BigParamControl("Enable Dynamic Steering Learner", "EnableCurvatureD") + show_curvatured_graph = BigParamControl("Show Dynamic Steering Learner Graph", "ShowDynamicSteeringLearnerGraph") self._scroller.add_widgets([ enable_curvature_correction, @@ -37,6 +39,8 @@ class ICTogglesLayoutMici(NavScroller): enable_dark_mode, enable_onroad_screen_timer, enable_accel_bar, + enable_curvatured, + show_curvatured_graph, ]) # Toggle lists @@ -53,13 +57,18 @@ class ICTogglesLayoutMici(NavScroller): ("DarkMode", enable_dark_mode), ("DisableScreenTimer", enable_onroad_screen_timer), ("ShowAccelBar", enable_accel_bar), + ("EnableCurvatureD", enable_curvatured), + ("ShowDynamicSteeringLearnerGraph", show_curvatured_graph), ) + enable_curvatured.set_enabled(lambda: ui_state.is_offroad()) + if ui_state.params.get_bool("ShowDebugInfo"): gui_app.set_show_touches(True) gui_app.set_show_fps(True) ui_state.add_engaged_transition_callback(self._update_toggles) + ui_state.add_offroad_transition_callback(self._update_toggles) def _update_state(self): super()._update_state() diff --git a/selfdrive/ui/mici/onroad/dynamic_steering_learner_graph.py b/selfdrive/ui/mici/onroad/dynamic_steering_learner_graph.py new file mode 100644 index 000000000..aff52d144 --- /dev/null +++ b/selfdrive/ui/mici/onroad/dynamic_steering_learner_graph.py @@ -0,0 +1,196 @@ +import time +from dataclasses import dataclass + +import numpy as np +import pyray as rl + +from openpilot.common.params import Params +from openpilot.selfdrive.locationd.curvatured import CurvatureDLookup +from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus +from openpilot.system.ui.widgets import Widget + + +@dataclass(frozen=True) +class DynamicSteeringLearnerGraphMiciConfig: + width: int = 144 + height: int = 72 + right_margin: int = 77 + zero_line_screen_y_frac: float = 0.68 + plot_padding_left: int = 0 + plot_padding_right: int = 0 + plot_padding_top: int = 0 + plot_padding_bottom: int = 0 + sample_points: int = 81 + + +CONFIG = DynamicSteeringLearnerGraphMiciConfig() + + +class DynamicSteeringLearnerGraphMici(Widget): + def __init__(self) -> None: + super().__init__() + self._params = Params() + self._display_enabled = False + self._param_update_time = 0.0 + + self._preview_glow_color = rl.Color(255, 255, 255, 42) + self._preview_curve_color = rl.Color(250, 250, 250, 168) + self._curve_glow_color = rl.Color(0, 255, 64, 56) + self._curve_color = rl.Color(0, 255, 64, 188) + self._curve_invalid_glow_color = rl.Color(255, 170, 70, 44) + self._curve_invalid_color = rl.Color(235, 185, 95, 166) + self._marker_glow_color = rl.Color(255, 80, 80, 72) + self._marker_color = rl.Color(255, 90, 90, 240) + self._plot_x = np.linspace(-CurvatureDLookup.CURVATURE_MAX, CurvatureDLookup.CURVATURE_MAX, CONFIG.sample_points) + self._cached_lcp_frame = -1 + self._cached_preview_curve = np.zeros(CONFIG.sample_points, dtype=np.float32) + self._cached_fit_curve = np.zeros(CONFIG.sample_points, dtype=np.float32) + self._cached_min_y = 0.0 + self._cached_max_y = 2e-5 + + self._update_params() + + @staticmethod + def _compute_y_bounds(preview_curve: np.ndarray, corrections: np.ndarray) -> tuple[float, float]: + min_val = float(min(np.min(preview_curve), np.min(corrections))) + max_val = float(max(np.max(preview_curve), np.max(corrections))) + min_span = 2e-5 + + if min_val >= 0.0: + return 0.0, max(min_span, max_val * 1.2) + if max_val <= 0.0: + return min(-min_span, min_val * 1.2), 0.0 + + low = min_val * 1.2 + high = max_val * 1.2 + if (high - low) < min_span: + center = 0.5 * (high + low) + half = 0.5 * min_span + return center - half, center + half + return low, high + + @staticmethod + def _map_y(plot_rect: rl.Rectangle, value: float, min_y: float, max_y: float) -> float: + frac = (value - min_y) / max(max_y - min_y, 1e-9) + return float(plot_rect.y + plot_rect.height * (1.0 - frac)) + + def _update_params(self) -> None: + self._param_update_time = time.monotonic() + self._display_enabled = self._params.get_bool("ShowDynamicSteeringLearnerGraph") + + def _update_state(self) -> None: + if time.monotonic() - self._param_update_time > 2.0: + self._update_params() + + def _get_curve_samples(self, lcp_frame: int, + preview_corrections: np.ndarray, preview_valid: np.ndarray, + 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) + 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 + + return self._cached_preview_curve, self._cached_fit_curve, self._cached_min_y, self._cached_max_y + + def _render(self, rect: rl.Rectangle) -> None: + if not self._display_enabled: + return + if ui_state.status in (UIStatus.DISENGAGED, UIStatus.LONG_ONLY): + return + + sm = ui_state.sm + if sm.recv_frame["carState"] < ui_state.started_frame or sm.recv_frame["controlsState"] < ui_state.started_frame: + return + + zero_line_y = rect.y + rect.height * CONFIG.zero_line_screen_y_frac + graph_rect = rl.Rectangle( + rect.x + rect.width - CONFIG.right_margin - CONFIG.width, + zero_line_y - CONFIG.height * 0.5, + CONFIG.width, + CONFIG.height, + ) + + lcp = sm["liveCurvatureParameters"] + lcp_frame = sm.recv_frame["liveCurvatureParameters"] + car_state = sm["carState"] + controls_state = sm["controlsState"] + + fit_corrections = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + fit_valid = np.zeros(CurvatureDLookup.bucket_shape(), dtype=bool) + preview_corrections = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + preview_valid = np.zeros(CurvatureDLookup.bucket_shape(), dtype=bool) + payload_valid = bool(getattr(lcp, "liveValid", False)) + + expected_size = CurvatureDLookup.total_size() + if len(getattr(lcp, "corrections", [])) == expected_size: + fit_corrections = CurvatureDLookup.unflatten_bucket(lcp.corrections, dtype=np.float32) + if len(getattr(lcp, "fitValid", [])) == expected_size: + fit_valid = CurvatureDLookup.unflatten_bucket(lcp.fitValid, dtype=bool) + if len(getattr(lcp, "previewCorrections", [])) == expected_size: + preview_corrections = CurvatureDLookup.unflatten_bucket(lcp.previewCorrections, dtype=np.float32) + if len(getattr(lcp, "previewValid", [])) == expected_size: + preview_valid = CurvatureDLookup.unflatten_bucket(lcp.previewValid, dtype=bool) + + plot_rect = rl.Rectangle( + graph_rect.x + CONFIG.plot_padding_left, + graph_rect.y + CONFIG.plot_padding_top, + graph_rect.width - CONFIG.plot_padding_left - CONFIG.plot_padding_right, + graph_rect.height - CONFIG.plot_padding_top - CONFIG.plot_padding_bottom, + ) + preview_curve, corrections, min_y, max_y = self._get_curve_samples( + lcp_frame, preview_corrections, preview_valid, fit_corrections, fit_valid, float(car_state.vEgo) + ) + + self._draw_plot( + plot_rect, + preview_curve, + corrections, + min_y, + max_y, + float(controls_state.modelDesiredCurvature), + payload_valid, + ) + + def _draw_plot(self, plot_rect: rl.Rectangle, + preview_curve: np.ndarray, corrections: np.ndarray, + min_y: float, max_y: float, + desired_curvature: float, curve_valid: bool) -> None: + + preview_points = [] + actual_points = [] + for curvature, preview_correction, correction in zip(self._plot_x, preview_curve, corrections, strict=True): + x = plot_rect.x + ((float(curvature) + CurvatureDLookup.CURVATURE_MAX) / (2.0 * CurvatureDLookup.CURVATURE_MAX)) * plot_rect.width + preview_y = self._map_y(plot_rect, float(preview_correction), min_y, max_y) + actual_y = self._map_y(plot_rect, float(correction), min_y, max_y) + preview_points.append(rl.Vector2(float(x), float(preview_y))) + actual_points.append(rl.Vector2(float(x), float(actual_y))) + + for p0, p1 in zip(preview_points[:-1], preview_points[1:], strict=True): + rl.draw_line_ex(p0, p1, 4.2, self._preview_glow_color) + for p0, p1 in zip(preview_points[:-1], preview_points[1:], strict=True): + rl.draw_line_ex(p0, p1, 2.0, self._preview_curve_color) + + curve_glow_color = self._curve_glow_color if curve_valid else self._curve_invalid_glow_color + curve_color = self._curve_color if curve_valid else self._curve_invalid_color + for p0, p1 in zip(actual_points[:-1], actual_points[1:], strict=True): + rl.draw_line_ex(p0, p1, 6.4, curve_glow_color) + for p0, p1 in zip(actual_points[:-1], actual_points[1:], strict=True): + rl.draw_line_ex(p0, p1, 3.4, curve_color) + + marker_alpha = float(np.clip( + (desired_curvature + CurvatureDLookup.CURVATURE_MAX) / (2.0 * CurvatureDLookup.CURVATURE_MAX), + 0.0, 1.0, + )) + marker_x = plot_rect.x + marker_alpha * plot_rect.width + marker_correction = float(np.interp(abs(desired_curvature), np.abs(self._plot_x), self._cached_fit_curve)) + marker_y = self._map_y(plot_rect, marker_correction, min_y, max_y) + rl.draw_circle(int(marker_x), int(marker_y), 5, self._marker_glow_color) + rl.draw_circle(int(marker_x), int(marker_y), 3, self._marker_color) diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py index e93341a8e..ebd682298 100644 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ b/selfdrive/ui/mici/onroad/hud_renderer.py @@ -1,6 +1,7 @@ import pyray as rl from dataclasses import dataclass from openpilot.common.constants import CV +from openpilot.selfdrive.ui.mici.onroad.dynamic_steering_learner_graph import DynamicSteeringLearnerGraphMici from openpilot.selfdrive.ui.mici.onroad.torque_bar import TorqueBar from openpilot.selfdrive.ui.mici.onroad.long_accel_bar import LongitudinalAccelBar from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus @@ -119,6 +120,7 @@ class HudRenderer(Widget): self._turn_intent = TurnIntent() self._torque_bar = TorqueBar() self._long_accel_bar = LongitudinalAccelBar() + self._dynamic_steering_learner_graph = DynamicSteeringLearnerGraphMici() self._txt_wheel: rl.Texture = gui_app.texture('icons_mici/wheel.png', 50, 50) self._txt_wheel_critical: rl.Texture = gui_app.texture('icons_mici/wheel_critical.png', 50, 50) @@ -177,6 +179,7 @@ class HudRenderer(Widget): if ui_state.enable_accel_bar: self._long_accel_bar.render(rect) + self._dynamic_steering_learner_graph.render(rect) self._torque_bar.render(rect) if self.is_cruise_set: diff --git a/selfdrive/ui/onroad/dynamic_steering_learner_graph.py b/selfdrive/ui/onroad/dynamic_steering_learner_graph.py new file mode 100644 index 000000000..cc804b40a --- /dev/null +++ b/selfdrive/ui/onroad/dynamic_steering_learner_graph.py @@ -0,0 +1,270 @@ +import time +from dataclasses import dataclass + +import numpy as np +import pyray as rl + +from openpilot.common.params import Params +from openpilot.selfdrive.locationd.curvatured import CurvatureDLookup +from openpilot.selfdrive.ui.onroad.battery_details import CONFIG as BATTERY_CONFIG +from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.system.ui.lib.application import gui_app, FontWeight +from openpilot.system.ui.widgets import Widget + + +@dataclass(frozen=True) +class DynamicSteeringLearnerGraphConfig: + width: int = 896 + height: int = 392 + right_margin: int = 30 + bottom_gap_to_battery: int = 20 + top_gap_to_speed: int = 20 + speed_display_bottom_y: int = 360 + aspect_ratio: float = 896 / 392 + padding: int = 25 + plot_padding_left: int = 73 + plot_padding_right: int = 25 + plot_padding_top: int = 59 + plot_padding_bottom: int = 72 + sample_points: int = 121 + + +CONFIG = DynamicSteeringLearnerGraphConfig() + + +class DynamicSteeringLearnerGraph(Widget): + def __init__(self) -> None: + super().__init__() + self._params = Params() + self._display_enabled = False + self._param_update_time = 0.0 + + self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM) + self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD) + self._panel_bg = rl.Color(0, 0, 0, 128) + self._axis_color = rl.Color(255, 255, 255, 90) + self._grid_color = rl.Color(255, 255, 255, 45) + self._preview_curve_color = rl.Color(240, 240, 240, 185) + self._curve_color = rl.Color(120, 220, 170, 255) + self._curve_invalid_color = rl.Color(220, 180, 90, 220) + self._marker_color = rl.Color(255, 80, 80, 255) + self._text_color = rl.Color(255, 255, 255, 245) + self._muted_text_color = rl.Color(200, 200, 200, 220) + self._plot_x = np.linspace(-CurvatureDLookup.CURVATURE_MAX, CurvatureDLookup.CURVATURE_MAX, CONFIG.sample_points) + self._cached_lcp_frame = -1 + self._cached_preview_curve = np.zeros(CONFIG.sample_points, dtype=np.float32) + self._cached_fit_curve = np.zeros(CONFIG.sample_points, dtype=np.float32) + self._cached_min_y = 0.0 + self._cached_max_y = 2e-5 + + self._update_params() + + @staticmethod + def _compute_y_bounds(preview_curve: np.ndarray, corrections: np.ndarray) -> tuple[float, float]: + min_val = float(min(np.min(preview_curve), np.min(corrections))) + max_val = float(max(np.max(preview_curve), np.max(corrections))) + min_span = 2e-5 + + if min_val >= 0.0: + return 0.0, max(min_span, max_val * 1.2) + if max_val <= 0.0: + return min(-min_span, min_val * 1.2), 0.0 + + low = min_val * 1.2 + high = max_val * 1.2 + if (high - low) < min_span: + center = 0.5 * (high + low) + half = 0.5 * min_span + return center - half, center + half + return low, high + + @staticmethod + def _map_y(plot_rect: rl.Rectangle, value: float, min_y: float, max_y: float) -> float: + frac = (value - min_y) / max(max_y - min_y, 1e-9) + return float(plot_rect.y + plot_rect.height * (1.0 - frac)) + + def _update_params(self) -> None: + self._param_update_time = time.monotonic() + self._display_enabled = self._params.get_bool("ShowDynamicSteeringLearnerGraph") + + def _update_state(self) -> None: + if time.monotonic() - self._param_update_time > 2.0: + self._update_params() + + def _get_curve_samples(self, lcp_frame: int, + preview_corrections: np.ndarray, preview_valid: np.ndarray, + 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) + 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 + + return self._cached_preview_curve, self._cached_fit_curve, self._cached_min_y, self._cached_max_y + + def _render(self, rect: rl.Rectangle) -> None: + if not self._display_enabled: + return + + sm = ui_state.sm + 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) + + graph_rect = rl.Rectangle( + rect.x + rect.width - graph_width - battery_panel_margin, + graph_bottom - graph_height, + graph_width, + graph_height, + ) + 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"] + + fit_corrections = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + fit_valid = np.zeros(CurvatureDLookup.bucket_shape(), dtype=bool) + preview_corrections = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32) + preview_valid = np.zeros(CurvatureDLookup.bucket_shape(), dtype=bool) + transport_valid = bool(sm.valid["liveCurvatureParameters"]) + payload_valid = bool(getattr(lcp, "liveValid", False)) + + expected_size = CurvatureDLookup.total_size() + if len(getattr(lcp, "corrections", [])) == expected_size: + fit_corrections = CurvatureDLookup.unflatten_bucket(lcp.corrections, dtype=np.float32) + if len(getattr(lcp, "fitValid", [])) == expected_size: + fit_valid = CurvatureDLookup.unflatten_bucket(lcp.fitValid, dtype=bool) + if len(getattr(lcp, "previewCorrections", [])) == expected_size: + preview_corrections = CurvatureDLookup.unflatten_bucket(lcp.previewCorrections, dtype=np.float32) + if len(getattr(lcp, "previewValid", [])) == expected_size: + preview_valid = CurvatureDLookup.unflatten_bucket(lcp.previewValid, dtype=bool) + + plot_rect = rl.Rectangle( + graph_rect.x + CONFIG.plot_padding_left, + graph_rect.y + CONFIG.plot_padding_top, + graph_rect.width - CONFIG.plot_padding_left - CONFIG.plot_padding_right, + graph_rect.height - CONFIG.plot_padding_top - CONFIG.plot_padding_bottom, + ) + preview_curve, corrections, min_y, max_y = self._get_curve_samples( + lcp_frame, preview_corrections, preview_valid, fit_corrections, fit_valid, float(car_state.vEgo) + ) + _, _, min_y, max_y = self._draw_plot( + plot_rect, preview_curve, corrections, min_y, max_y, transport_valid and payload_valid + ) + 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 _draw_plot(self, plot_rect: rl.Rectangle, + preview_curve: np.ndarray, corrections: np.ndarray, + min_y: float, max_y: float, + curve_valid: bool) -> tuple[np.ndarray, np.ndarray, float, float]: + rl.draw_rectangle_lines_ex(plot_rect, 1.0, self._grid_color) + + zero_x = plot_rect.x + plot_rect.width / 2 + zero_y = plot_rect.y + plot_rect.height / 2 + for frac in (0.25, 0.5, 0.75): + x = plot_rect.x + plot_rect.width * frac + rl.draw_line_ex(rl.Vector2(float(x), float(plot_rect.y)), + rl.Vector2(float(x), float(plot_rect.y + plot_rect.height)), 1.0, self._grid_color) + + zero_y = self._map_y(plot_rect, 0.0, min_y, max_y) + + rl.draw_line_ex(rl.Vector2(float(plot_rect.x), float(zero_y)), + rl.Vector2(float(plot_rect.x + plot_rect.width), float(zero_y)), 2.0, self._axis_color) + rl.draw_line_ex(rl.Vector2(float(zero_x), float(plot_rect.y)), + rl.Vector2(float(zero_x), float(plot_rect.y + plot_rect.height)), 2.0, self._axis_color) + + for frac in (0.25, 0.75): + y = plot_rect.y + plot_rect.height * frac + rl.draw_line_ex(rl.Vector2(float(plot_rect.x), float(y)), + rl.Vector2(float(plot_rect.x + plot_rect.width), float(y)), 1.0, self._grid_color) + + preview_points = [] + actual_points = [] + for curvature, preview_correction, correction in zip(self._plot_x, preview_curve, corrections, strict=True): + x = plot_rect.x + ((float(curvature) + CurvatureDLookup.CURVATURE_MAX) / (2.0 * CurvatureDLookup.CURVATURE_MAX)) * plot_rect.width + preview_y = self._map_y(plot_rect, float(preview_correction), min_y, max_y) + actual_y = self._map_y(plot_rect, float(correction), min_y, max_y) + preview_points.append(rl.Vector2(float(x), float(preview_y))) + actual_points.append(rl.Vector2(float(x), float(actual_y))) + + for p0, p1 in zip(preview_points[:-1], preview_points[1:], strict=True): + rl.draw_line_ex(p0, p1, 1.5, self._preview_curve_color) + curve_color = self._curve_color if curve_valid else self._curve_invalid_color + for p0, p1 in zip(actual_points[:-1], actual_points[1:], strict=True): + rl.draw_line_ex(p0, p1, 4.0, curve_color) + return preview_curve, corrections, min_y, max_y + + def _draw_overlay_info(self, graph_rect: rl.Rectangle, lcp, v_ego: float, desired_curvature: float, + fit_corrections: np.ndarray, fit_valid: np.ndarray, min_y: float, max_y: float, + transport_valid: bool, payload_valid: bool) -> None: + low_idx, high_idx, alpha = CurvatureDLookup.speed_interp(v_ego) + current_correction = 0.0 + display_correction = 0.0 + if transport_valid and payload_valid: + current_correction = CurvatureDLookup.interp_curve_value( + fit_corrections, fit_valid, v_ego, abs(desired_curvature) + ) + display_correction = current_correction * (1.0 if desired_curvature >= 0.0 else -1.0) + + marker_alpha = float(np.clip( + (desired_curvature + CurvatureDLookup.CURVATURE_MAX) / (2.0 * CurvatureDLookup.CURVATURE_MAX), + 0.0, 1.0, + )) + marker_x = graph_rect.x + CONFIG.plot_padding_left + marker_alpha * ( + graph_rect.width - CONFIG.plot_padding_left - CONFIG.plot_padding_right + ) + plot_height = graph_rect.height - CONFIG.plot_padding_top - CONFIG.plot_padding_bottom + plot_rect = rl.Rectangle( + graph_rect.x + CONFIG.plot_padding_left, + graph_rect.y + CONFIG.plot_padding_top, + graph_rect.width - CONFIG.plot_padding_left - CONFIG.plot_padding_right, + plot_height, + ) + marker_y = self._map_y(plot_rect, float(current_correction), min_y, max_y) + rl.draw_circle(int(marker_x), int(marker_y), 6, self._marker_color) + + title_size = min(42, max(34, int(graph_rect.height * 0.095))) + status_size = min(30, max(24, int(graph_rect.height * 0.068))) + footer_size = min(27, max(22, int(graph_rect.height * 0.058))) + text_x = float(graph_rect.x + CONFIG.padding) + title_y = float(graph_rect.y + 12) + status_y = title_y + title_size + 8 + footer_y2 = float(graph_rect.y + graph_rect.height - footer_size - 10) + footer_y1 = float(footer_y2 - footer_size - 8) + + title = "Dynamic Steering Learner" + rl.draw_text_ex(self._font_bold, title, rl.Vector2(text_x, title_y), title_size, 0, self._text_color) + + status_text = ( + f"live={payload_valid} transport={transport_valid} cal={int(getattr(lcp, 'calPerc', 0))}% " + f"points={int(getattr(lcp, 'totalBucketPoints', 0))}" + ) + rl.draw_text_ex(self._font_medium, status_text, rl.Vector2(text_x, status_y), status_size, 0, self._muted_text_color) + + speed_mix = ( + f"v={v_ego * 3.6:.0f} km/h mix={CurvatureDLookup.SPEED_ANCHORS[low_idx] * 3.6:.0f}/" + f"{CurvatureDLookup.SPEED_ANCHORS[high_idx] * 3.6:.0f} alpha={alpha:.2f}" + ) + marker_info = ( + f"k={desired_curvature:.2e} corr={display_correction:.2e} " + f"bucket=({int(getattr(lcp, 'bucketSpeed', -1))}, {int(getattr(lcp, 'bucketCurvature', -1))})" + ) + rl.draw_text_ex(self._font_medium, speed_mix, rl.Vector2(text_x, footer_y1), footer_size, 0, self._muted_text_color) + rl.draw_text_ex(self._font_medium, marker_info, rl.Vector2(text_x, footer_y2), footer_size, 0, self._muted_text_color) diff --git a/selfdrive/ui/onroad/hud_renderer.py b/selfdrive/ui/onroad/hud_renderer.py index c0fd3059b..b3c82d238 100644 --- a/selfdrive/ui/onroad/hud_renderer.py +++ b/selfdrive/ui/onroad/hud_renderer.py @@ -3,6 +3,7 @@ from dataclasses import dataclass from openpilot.common.constants import CV from openpilot.selfdrive.ui.onroad.exp_button import ExpButton from openpilot.selfdrive.ui.onroad.battery_details import BatteryDetails +from openpilot.selfdrive.ui.onroad.dynamic_steering_learner_graph import DynamicSteeringLearnerGraph from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus from openpilot.system.ui.lib.application import gui_app, FontWeight from openpilot.system.ui.lib.multilang import tr @@ -73,6 +74,7 @@ class HudRenderer(Widget): self._exp_button: ExpButton = ExpButton(UI_CONFIG.button_size, UI_CONFIG.wheel_icon_size) self._battery_details = BatteryDetails() + self._dynamic_steering_learner_graph = DynamicSteeringLearnerGraph() def _update_state(self) -> None: """Update HUD state based on car state and controls state.""" @@ -121,6 +123,7 @@ class HudRenderer(Widget): button_x = rect.x + rect.width - UI_CONFIG.border_size - UI_CONFIG.button_size button_y = rect.y + UI_CONFIG.border_size + self._dynamic_steering_learner_graph.render(rect) self._battery_details.render(rect) self._exp_button.render(rl.Rectangle(button_x, button_y, UI_CONFIG.button_size, UI_CONFIG.button_size)) diff --git a/selfdrive/ui/sunnypilot/layouts/settings/models.py b/selfdrive/ui/sunnypilot/layouts/settings/models.py index adbd99f1f..5f9fcc476 100644 --- a/selfdrive/ui/sunnypilot/layouts/settings/models.py +++ b/selfdrive/ui/sunnypilot/layouts/settings/models.py @@ -179,6 +179,7 @@ class ModelsLayout(Widget): def _callback(response): if response == DialogResult.CONFIRM: ui_state.params.remove("CalibrationParams") + ui_state.params.remove("LiveCurvatureParameters") ui_state.params.remove("LiveTorqueParameters") msg = tr("Model download has started in the background. We suggest resetting calibration. Would you like to do that now?") dialog = ConfirmDialog(msg, tr("Reset Calibration"), callback=_callback) diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py index e7cb58cd3..8084f5132 100644 --- a/selfdrive/ui/ui_state.py +++ b/selfdrive/ui/ui_state.py @@ -59,6 +59,7 @@ class UIState(UIStateSP): "carOutput", "carControl", "liveParameters", + "liveCurvatureParameters", "rawAudioData", ] + self.sm_services_ext ) diff --git a/system/manager/process_config.py b/system/manager/process_config.py index a9ecf302f..eb56dd366 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -64,6 +64,9 @@ def only_onroad(started: bool, params: Params, CP: car.CarParams) -> bool: def only_offroad(started: bool, params: Params, CP: car.CarParams) -> bool: return not started +def curvatured_enabled(started: bool, params: Params, CP: car.CarParams) -> bool: + return only_onroad(started, params, CP) and params.get_bool("EnableCurvatureD") + def use_github_runner(started, params, CP: car.CarParams) -> bool: return not PC and params.get_bool("EnableGithubRunner") and ( not params.get_bool("NetworkMetered") and not params.get_bool("GithubRunnerSufficientVoltage")) @@ -131,6 +134,7 @@ procs = [ NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False), PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad), PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad), + PythonProcess("curvatured", "selfdrive.locationd.curvatured", curvatured_enabled), PythonProcess("controlsd", "selfdrive.controls.controlsd", and_(not_joystick, iscar)), PythonProcess("joystickd", "tools.joystick.joystickd", or_(joystick, notcar)), PythonProcess("selfdrived", "selfdrive.selfdrived.selfdrived", only_onroad),