From a55b8d2bd0d3ee2e9a2b138db4d30101d14c4db3 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 9 Aug 2024 01:14:38 -0700 Subject: [PATCH] bunch more typing --- selfdrive/car/data_structures.py | 97 ++++++++++++++++++--- selfdrive/car/gm/interface.py | 3 +- selfdrive/car/interfaces.py | 36 ++++---- selfdrive/car/tests/test_car_interfaces.py | 29 +++--- selfdrive/car/toyota/interface.py | 21 ++--- selfdrive/car/toyota/radar_interface.py | 6 +- selfdrive/car/toyota/values.py | 4 +- selfdrive/controls/lib/latcontrol_pid.py | 6 +- selfdrive/controls/lib/latcontrol_torque.py | 2 +- 9 files changed, 140 insertions(+), 64 deletions(-) diff --git a/selfdrive/car/data_structures.py b/selfdrive/car/data_structures.py index 5373c4aab5..938c0e0d16 100644 --- a/selfdrive/car/data_structures.py +++ b/selfdrive/car/data_structures.py @@ -73,9 +73,92 @@ class CarParams: notCar: bool = auto_field() # flag for non-car robotics platforms + pcmCruise: bool = auto_field() # is openpilot's state tied to the PCM's cruise state? + enableDsu: bool = auto_field() # driving support unit + enableBsm: bool = auto_field() # blind spot monitoring + flags: int = auto_field() # flags for car specific quirks + experimentalLongitudinalAvailable: bool = auto_field() + + minEnableSpeed: float = auto_field() + minSteerSpeed: float = auto_field() + # safetyConfigs: list[SafetyConfig] = auto_field() + alternativeExperience: int = auto_field() # panda flag for features like no disengage on gas + + maxLateralAccel: float = auto_field() + autoResumeSng: bool = auto_field() # describes whether car can resume from a stop automatically + + mass: float = auto_field() # [kg] curb weight: all fluids no cargo + wheelbase: float = auto_field() # [m] distance from rear axle to front axle + centerToFront: float = auto_field() # [m] distance from center of mass to front axle + steerRatio: float = auto_field() # [] ratio of steering wheel angle to front wheel angle + steerRatioRear: float = auto_field() # [] ratio of steering wheel angle to rear wheel angle (usually 0) + + rotationalInertia: float = auto_field() # [kg*m2] body rotational inertia + tireStiffnessFactor: float = auto_field() # scaling factor used in calculating tireStiffness[Front,Rear] + tireStiffnessFront: float = auto_field() # [N/rad] front tire coeff of stiff + tireStiffnessRear: float = auto_field() # [N/rad] rear tire coeff of stiff + + longitudinalTuning: 'LongitudinalPIDTuning' = field(default_factory=lambda: CarParams.LongitudinalPIDTuning()) + # lateralParams: LateralParams = auto_field() + lateralTuning: 'CarParams.LateralPIDTuning | CarParams.LateralTorqueTuning' = field(default_factory=lambda: CarParams.LateralPIDTuning()) + + @dataclass + @apply_auto_fields + class LateralPIDTuning: + kpBP: list[float] = auto_field() + kpV: list[float] = auto_field() + kiBP: list[float] = auto_field() + kiV: list[float] = auto_field() + kf: float = auto_field() + + @dataclass + @apply_auto_fields + class LateralTorqueTuning: + useSteeringAngle: bool = auto_field() + kp: float = auto_field() + ki: float = auto_field() + friction: float = auto_field() + kf: float = auto_field() + steeringAngleDeadzoneDeg: float = auto_field() + latAccelFactor: float = auto_field() + latAccelOffset: float = auto_field() + + steerLimitAlert: bool = auto_field() + steerLimitTimer: float = auto_field() # time before steerLimitAlert is issued + + vEgoStopping: float = auto_field() # Speed at which the car goes into stopping state + vEgoStarting: float = auto_field() # Speed at which the car goes into starting state + stoppingControl: bool = auto_field() # Does the car allow full control even at lows speeds when stopping + steerControlType: 'CarParams.SteerControlType' = field(default_factory=lambda: CarParams.SteerControlType.torque) + radarUnavailable: bool = auto_field() # True when radar objects aren't visible on CAN or aren't parsed out + stopAccel: float = auto_field() # Required acceleration to keep vehicle stationary + stoppingDecelRate: float = auto_field() # m/s^2/s while trying to stop + startAccel: float = auto_field() # Required acceleration to get car moving + startingState: bool = auto_field() # Does this car make use of special starting state + + steerActuatorDelay: float = auto_field() # Steering wheel actuator delay in seconds + longitudinalActuatorDelay: float = auto_field() # Gas/Brake actuator delay in seconds + openpilotLongitudinalControl: bool = auto_field() # is openpilot doing the longitudinal control? + carVin: str = auto_field() # VIN number queried during fingerprinting + dashcamOnly: bool = auto_field() + passive: bool = auto_field() # is openpilot in control? + # transmissionType: TransmissionType = auto_field() carFw: list['CarParams.CarFw'] = auto_field() - carVin: str = auto_field() + radarTimeStep: float = auto_field() # time delta between radar updates, 20Hz is very standard + # fingerprintSource: FingerprintSource = auto_field() + # networkLocation: NetworkLocation = auto_field() # Where Panda/C2 is integrated into the car's CAN network + + wheelSpeedFactor: float = auto_field() # Multiplier on wheels speeds to computer actual speeds + + @dataclass + @apply_auto_fields + class LongitudinalPIDTuning: + kpBP: list[float] = auto_field() + kpV: list[float] = auto_field() + kiBP: list[float] = auto_field() + kiV: list[float] = auto_field() + kf: float = auto_field() class SteerControlType(StrEnum): torque = auto() @@ -126,15 +209,3 @@ class CarParams: programmedFuelInjection = auto() debug = auto() - - @dataclass - @apply_auto_fields - class LateralTorqueTuning: - useSteeringAngle: bool = auto_field() - kp: float = auto_field() - ki: float = auto_field() - friction: float = auto_field() - kf: float = auto_field() - steeringAngleDeadzoneDeg: float = auto_field() - latAccelFactor: float = auto_field() - latAccelOffset: float = auto_field() diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 31b874502b..0124a2afdf 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 import os +from typing import cast from cereal import car from math import fabs, exp from panda import Panda @@ -63,7 +64,7 @@ class CarInterface(CarInterfaceBase): # An important thing to consider is that the slope at 0 should be > 0 (ideally >1) # This has big effect on the stability about 0 (noise when going straight) # ToDo: To generalize to other GMs, explore tanh function as the nonlinear - non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint) + non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(cast(CAR, self.CP.carFingerprint)) assert non_linear_torque_params, "The params are not defined" a, b, c, _ = non_linear_torque_params steer_torque = (sig(latcontrol_inputs.lateral_acceleration * a) * b) + (latcontrol_inputs.lateral_acceleration * c) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 6660aa5729..5bab332117 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -88,7 +88,7 @@ def get_torque_params(): # generic car and radar interfaces class CarInterfaceBase(ABC): - def __init__(self, CP, CarController, CarState): + def __init__(self, CP: CarParams, CarController, CarState): self.CP = CP self.frame = 0 @@ -178,8 +178,8 @@ class CarInterfaceBase(ABC): # returns a set of default params to avoid repetition in car specific params @staticmethod - def get_std_params(candidate): - ret = car.CarParams.new_message() + def get_std_params(candidate) -> CarParams: + ret = CarParams() ret.carFingerprint = candidate # Car docs fields @@ -212,18 +212,20 @@ class CarInterfaceBase(ABC): return ret @staticmethod - def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_steering_angle=True): + def configure_torque_tune(candidate: str, ret: CarParams, steering_angle_deadzone_deg: float = 0.0, use_steering_angle: bool = True): params = get_torque_params()[candidate] - tune.init('torque') - tune.torque.useSteeringAngle = use_steering_angle - tune.torque.kp = 1.0 - tune.torque.kf = 1.0 - tune.torque.ki = 0.1 - tune.torque.friction = params['FRICTION'] - tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR'] - tune.torque.latAccelOffset = 0.0 - tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg + tune = CarParams.LateralTorqueTuning() + tune.useSteeringAngle = use_steering_angle + tune.kp = 1.0 + tune.kf = 1.0 + tune.ki = 0.1 + tune.friction = params['FRICTION'] + tune.latAccelFactor = params['LAT_ACCEL_FACTOR'] + tune.latAccelOffset = 0.0 + tune.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg + + ret.lateralTuning = tune @abstractmethod def _update(self, c: car.CarControl) -> car.CarState: @@ -343,10 +345,10 @@ class CarInterfaceBase(ABC): class RadarInterfaceBase(ABC): - def __init__(self, CP): + def __init__(self, CP: CarParams): self.CP = CP self.rcp = None - self.pts = {} + self.pts: dict[int, RadarData.RadarPoint] = {} self.delay = 0 self.radar_ts = CP.radarTimeStep self.frame = 0 @@ -359,7 +361,7 @@ class RadarInterfaceBase(ABC): class CarStateBase(ABC): - def __init__(self, CP): + def __init__(self, CP: CarParams): self.CP = CP self.car_fingerprint = CP.carFingerprint self.out = car.CarState.new_message() @@ -463,7 +465,7 @@ class CarStateBase(ABC): class CarControllerBase(ABC): - def __init__(self, dbc_name: str, CP): + def __init__(self, dbc_name: str, CP: CarParams): self.CP = CP self.frame = 0 diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 9e3c7d157e..b92501c37e 100644 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -8,6 +8,7 @@ from parameterized import parameterized from cereal import car, messaging from openpilot.selfdrive.car import DT_CTRL, gen_empty_fingerprint from openpilot.selfdrive.car.car_helpers import interfaces +from openpilot.selfdrive.car.data_structures import CarParams from openpilot.selfdrive.car.fingerprints import all_known_cars from openpilot.selfdrive.car.fw_versions import FW_VERSIONS, FW_QUERY_CONFIGS from openpilot.selfdrive.car.interfaces import get_interface_attr @@ -42,8 +43,8 @@ def get_fuzzy_car_interface_args(draw: DrawType) -> dict: }) params: dict = draw(params_strategy) - params['car_fw'] = [car.CarParams.CarFw(ecu=fw[0], address=fw[1], subAddress=fw[2] or 0, - request=draw(st.sampled_from(sorted(ALL_REQUESTS)))) + params['car_fw'] = [CarParams.CarFw(ecu=fw[0], address=fw[1], subAddress=fw[2] or 0, + request=draw(st.sampled_from(sorted(ALL_REQUESTS)))) for fw in params['car_fw']] return params @@ -62,7 +63,7 @@ class TestCarInterfaces: car_params = CarInterface.get_params(car_name, args['fingerprints'], args['car_fw'], experimental_long=args['experimental_long'], docs=False) - car_params = car_params.as_reader() + # car_params = car_params.as_reader() car_interface = CarInterface(car_params, CarController, CarState) assert car_params assert car_interface @@ -78,16 +79,16 @@ class TestCarInterfaces: assert len(car_params.longitudinalTuning.kiV) == len(car_params.longitudinalTuning.kiBP) # Lateral sanity checks - if car_params.steerControlType != car.CarParams.SteerControlType.angle: + if car_params.steerControlType != CarParams.SteerControlType.angle: tune = car_params.lateralTuning - if tune.which() == 'pid': - assert not math.isnan(tune.pid.kf) and tune.pid.kf > 0 - assert len(tune.pid.kpV) > 0 and len(tune.pid.kpV) == len(tune.pid.kpBP) - assert len(tune.pid.kiV) > 0 and len(tune.pid.kiV) == len(tune.pid.kiBP) + if isinstance(tune, CarParams.LateralPIDTuning): + assert not math.isnan(tune.kf) and tune.kf > 0 + assert len(tune.kpV) > 0 and len(tune.kpV) == len(tune.kpBP) + assert len(tune.kiV) > 0 and len(tune.kiV) == len(tune.kiBP) - elif tune.which() == 'torque': - assert not math.isnan(tune.torque.kf) and tune.torque.kf > 0 - assert not math.isnan(tune.torque.friction) and tune.torque.friction > 0 + elif isinstance(tune, CarParams.LateralTorqueTuning): + assert not math.isnan(tune.kf) and tune.kf > 0 + assert not math.isnan(tune.friction) and tune.friction > 0 cc_msg = FuzzyGenerator.get_random_msg(data.draw, car.CarControl, real_floats=True) # Run car interface @@ -109,11 +110,11 @@ class TestCarInterfaces: # TODO: wait until card refactor is merged to run controller a few times, # hypothesis also slows down significantly with just one more message draw LongControl(car_params) - if car_params.steerControlType == car.CarParams.SteerControlType.angle: + if car_params.steerControlType == CarParams.SteerControlType.angle: LatControlAngle(car_params, car_interface) - elif car_params.lateralTuning.which() == 'pid': + elif isinstance(car_params.lateralTuning, CarParams.LateralPIDTuning): LatControlPID(car_params, car_interface) - elif car_params.lateralTuning.which() == 'torque': + elif isinstance(car_params.lateralTuning, CarParams.LateralTorqueTuning): LatControlTorque(car_params, car_interface) # Test radar interface diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 5b2a075ce8..fed3476f89 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -4,6 +4,7 @@ from panda.python import uds from openpilot.selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \ MIN_ACC_SPEED, EPS_SCALE, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR from openpilot.selfdrive.car import create_button_events, get_safety_config +from openpilot.selfdrive.car.data_structures import CarParams from openpilot.selfdrive.car.disable_ecu import disable_ecu from openpilot.selfdrive.car.interfaces import CarInterfaceBase @@ -35,7 +36,7 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.18 ret.steerLimitTimer = 0.8 else: - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + CarInterfaceBase.configure_torque_tune(candidate, ret) ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerLimitTimer = 0.4 @@ -66,20 +67,20 @@ class CarInterface(CarInterfaceBase): stop_and_go = candidate != CAR.TOYOTA_AVALON elif candidate in (CAR.TOYOTA_RAV4_TSS2, CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_TSS2_2023): - ret.lateralTuning.init('pid') - ret.lateralTuning.pid.kiBP = [0.0] - ret.lateralTuning.pid.kpBP = [0.0] - ret.lateralTuning.pid.kpV = [0.6] - ret.lateralTuning.pid.kiV = [0.1] - ret.lateralTuning.pid.kf = 0.00007818594 + ret.lateralTuning = CarParams.LateralPIDTuning() + ret.lateralTuning.kiBP = [0.0] + ret.lateralTuning.kpBP = [0.0] + ret.lateralTuning.kpV = [0.6] + ret.lateralTuning.kiV = [0.1] + ret.lateralTuning.kf = 0.00007818594 # 2019+ RAV4 TSS2 uses two different steering racks and specific tuning seems to be necessary. # See https://github.com/commaai/openpilot/pull/21429#issuecomment-873652891 for fw in car_fw: if fw.ecu == "eps" and (fw.fwVersion.startswith(b'\x02') or fw.fwVersion in [b'8965B42181\x00\x00\x00\x00\x00\x00']): - ret.lateralTuning.pid.kpV = [0.15] - ret.lateralTuning.pid.kiV = [0.05] - ret.lateralTuning.pid.kf = 0.00004 + ret.lateralTuning.kpV = [0.15] + ret.lateralTuning.kiV = [0.05] + ret.lateralTuning.kf = 0.00004 break elif candidate in (CAR.TOYOTA_CHR, CAR.TOYOTA_CAMRY, CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_NX): diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index fae6eecaf6..ff07b7b84f 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 from opendbc.can.parser import CANParser -from cereal import car +from openpilot.selfdrive.car.data_structures import RadarData from openpilot.selfdrive.car.toyota.values import DBC, TSS2_CAR from openpilot.selfdrive.car.interfaces import RadarInterfaceBase @@ -54,7 +54,7 @@ class RadarInterface(RadarInterfaceBase): return rr def _update(self, updated_messages): - ret = car.RadarData.new_message() + ret = RadarData() errors = [] if not self.rcp.can_valid: errors.append("canError") @@ -77,7 +77,7 @@ class RadarInterface(RadarInterfaceBase): # radar point only valid if it's a valid measurement and score is above 50 if cpt['VALID'] or (score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0): if ii not in self.pts or cpt['NEW_TRACK']: - self.pts[ii] = car.RadarData.RadarPoint.new_message() + self.pts[ii] = RadarData.RadarPoint() self.pts[ii].trackId = self.track_id self.track_id += 1 self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index b6d4eefcfc..a22f38e3c5 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -31,8 +31,8 @@ class CarControllerParams: ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.3, 0.15]) ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.36, 0.26]) - def __init__(self, CP): - if CP.lateralTuning.which == 'torque': + def __init__(self, CP: CarParams): + if isinstance(CP.lateralTuning, CarParams.LateralTorqueTuning): self.STEER_DELTA_UP = 15 # 1.0s time to peak torque self.STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) else: diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 9e6160838b..5657d1765f 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -8,9 +8,9 @@ from openpilot.selfdrive.controls.lib.pid import PIDController class LatControlPID(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) - self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), - (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), - k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) + self.pid = PIDController((CP.lateralTuning.kpBP, CP.lateralTuning.kpV), + (CP.lateralTuning.kiBP, CP.lateralTuning.kiV), + k_f=CP.lateralTuning.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.get_steer_feedforward = CI.get_steer_feedforward_function() def reset(self): diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index decff2ec05..66b2e7aabc 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -25,7 +25,7 @@ LOW_SPEED_Y = [15, 13, 10, 5] class LatControlTorque(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) - self.torque_params = CP.lateralTuning.torque.as_builder() + self.torque_params = CP.lateralTuning self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, k_f=self.torque_params.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.torque_from_lateral_accel = CI.torque_from_lateral_accel()