mirror of
https://github.com/infiniteCable2/openpilot.git
synced 2026-08-06 08:46:08 +08:00
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
This commit is contained in:
infiniteCable2
+1
-1
@@ -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
|
||||
|
||||
+17
-1
@@ -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);
|
||||
}
|
||||
|
||||
+2
-1
@@ -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
|
||||
|
||||
@@ -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.),
|
||||
|
||||
@@ -79,6 +79,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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<std::string, ParamKeyAttributes> 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}},
|
||||
|
||||
+1
-1
Submodule msgq_repo updated: b7688b9bd7...0930be91af
@@ -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)
|
||||
|
||||
@@ -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))
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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,
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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")
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
@@ -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")
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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)
|
||||
@@ -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:
|
||||
|
||||
@@ -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)
|
||||
@@ -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))
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -59,6 +59,7 @@ class UIState(UIStateSP):
|
||||
"carOutput",
|
||||
"carControl",
|
||||
"liveParameters",
|
||||
"liveCurvatureParameters",
|
||||
"rawAudioData",
|
||||
] + self.sm_services_ext
|
||||
)
|
||||
|
||||
@@ -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),
|
||||
|
||||
Reference in New Issue
Block a user