diff --git a/selfdrive/car/data_structures.py b/selfdrive/car/data_structures.py index 1b1dc129bd..ea4fa44dc7 100644 --- a/selfdrive/car/data_structures.py +++ b/selfdrive/car/data_structures.py @@ -1,5 +1,14 @@ -from dataclasses import dataclass -from enum import StrEnum as _StrEnum, auto +from dataclasses import dataclass, fields, is_dataclass +from enum import Enum, StrEnum as _StrEnum, auto +# from typing import Type, TypeVar +from typing import Type, TypeVar, TYPE_CHECKING, Any, get_type_hints, get_origin + +if TYPE_CHECKING: + from _typeshed import DataclassInstance +# +# DataclassT = TypeVar("DataclassT", bound="DataclassInstance") +# +# T = TypeVar('T', bound='Struct') class StrEnum(_StrEnum): @@ -9,18 +18,99 @@ class StrEnum(_StrEnum): return name +# class Struct: +# @classmethod +# def new_message(cls, **kwargs): +# init_values = {} +# for f in fields(cls): +# init_values[f.name] = kwargs.get(f.name, f.type()) +# +# return cls(**init_values) + +T = TypeVar('T', bound='DataclassInstance') + + +class Struct: + @classmethod + def new_message(cls: Type[T], **kwargs: Any) -> T: + if not is_dataclass(cls): + raise TypeError(f"{cls.__name__} is not a dataclass") + + init_values = {} + type_hints = get_type_hints(cls) + print(type_hints) + for f in fields(cls): + field_type = type_hints[f.name] + print(f.name, f.type, field_type) + print(issubclass(field_type, Enum)) + if issubclass(field_type, Enum): + init_values[f.name] = kwargs.get(f.name, list(field_type)[0]) + # TODO: fix this + # assert issubclass(init_values[f.name], type(field_type)), f"Expected {field_type} for {f.name}, got {type(init_values[f.name])}" + else: + # FIXME: typing check hack since mypy doesn't catch anything + init_values[f.name] = kwargs.get(f.name, field_type()) + print('field_type', field_type, f.type) + # TODO: this is so bad + assert isinstance(init_values[f.name], get_origin(f.type) or f.type), f"Expected {field_type} for {f.name}, got {type(init_values[f.name])}" + + return cls(**init_values) + + @dataclass -class CarParams: +class RadarData(Struct): + errors: list['Error'] + points: list['RadarPoint'] + + class Error(StrEnum): + canError = auto() + fault = auto() + wrongConfig = auto() + + @dataclass + class RadarPoint(Struct): + trackId: int # no trackId reuse + + # these 3 are the minimum required + dRel: float # m from the front bumper of the car + yRel: float # m + vRel: float # m/s + + # these are optional and valid if they are not NaN + aRel: float # m/s^2 + yvRel: float # m/s + + # some radars flag measurements VS estimates + measured: bool + + +@dataclass +class CarParams(Struct): carName: str carFingerprint: str fuzzyFingerprint: bool notCar: bool # flag for non-car robotics platforms + carFw: list['CarFw'] + class SteerControlType(StrEnum): torque = auto() angle = auto() + @dataclass + class CarFw(Struct): + ecu: 'CarParams.Ecu' + fwVersion: bytes + address: int + subAddress: int + responseAddress: int + request: list[bytes] + brand: str + bus: int + logging: bool + obdMultiplexing: bool + class Ecu(StrEnum): eps = auto() abs = auto() @@ -52,3 +142,12 @@ class CarParams: programmedFuelInjection = auto() debug = auto() + + +# CP: CarParams = CarParams.new_message(carName='toyota', fuzzyFingerprint=123) +# CP: CarParams = CarParams(carName='toyota', fuzzyFingerprint=123) + +import ast + + +# test = ast.literal_eval('CarParams.CarFw') diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 8f7a7ed9dd..e166682a1a 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,6 +1,7 @@ from cereal import car from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_tester_present_msg from openpilot.selfdrive.car.can_definitions import CanData +from openpilot.selfdrive.car.data_structures import CarParams from openpilot.selfdrive.car.helpers import clip from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.toyota import toyotacan @@ -9,7 +10,7 @@ from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_ UNSUPPORTED_DSU_CAR from opendbc.can.packer import CANPacker -SteerControlType = car.CarParams.SteerControlType +SteerControlType = CarParams.SteerControlType VisualAlert = car.CarControl.HUDControl.VisualAlert # LKA limits