From e2a2cb184ac250fa4a3b307e18adb6379bdd49c5 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 16 Mar 2025 02:25:29 -0400 Subject: [PATCH] more init --- cereal/custom.capnp | 3 + common/params_keys.h | 3 + sunnypilot/selfdrive/car/interfaces.py | 25 +++++ .../selfdrive/controls/lib/nnlc/__init__.py | 0 .../selfdrive/controls/lib/nnlc/flux_model.py | 100 ++++++++++++++++++ .../selfdrive/controls/lib/nnlc/helpers.py | 74 +++++++++++++ 6 files changed, 205 insertions(+) create mode 100644 sunnypilot/selfdrive/controls/lib/nnlc/__init__.py create mode 100644 sunnypilot/selfdrive/controls/lib/nnlc/flux_model.py create mode 100644 sunnypilot/selfdrive/controls/lib/nnlc/helpers.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 38e019338f..84a1e64200 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -143,6 +143,9 @@ struct CarParamsSP @0x80ae746ee2596b11 { struct NeuralNetworkLateralControl { enabled @0 :Bool; + modelPath @1 :Text; + modelName @2 :Text; + fuzzyFingerprint @3 :Bool; } } diff --git a/common/params_keys.h b/common/params_keys.h index 307f7ada46..7c613e7077 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -141,6 +141,9 @@ inline static std::unordered_map keys = { {"ModelManager_LastSyncTime", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"ModelManager_ModelsCache", PERSISTENT | BACKUP}, + // Neural Network Lateral Control + {"NeuralNetworkLateralControl", PERSISTENT | BACKUP}, + // sunnylink params {"EnableSunnylinkUploader", PERSISTENT | BACKUP}, {"LastSunnylinkPingTime", CLEAR_ON_MANAGER_START}, diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py index 375ce7fa7f..0ad991f915 100644 --- a/sunnypilot/selfdrive/car/interfaces.py +++ b/sunnypilot/selfdrive/car/interfaces.py @@ -5,12 +5,17 @@ This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ +import os + from opendbc.car import Bus, structs from opendbc.car.can_definitions import CanRecvCallable, CanSendCallable from opendbc.car.car_helpers import can_fingerprint +from opendbc.car.interfaces import CarInterfaceBase from opendbc.car.hyundai.radar_interface import RADAR_START_ADDR from opendbc.car.hyundai.values import HyundaiFlags, DBC as HYUNDAI_DBC from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP +from openpilot.common.swaglog import cloudlog +from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import get_nn_model_path import openpilot.system.sentry as sentry @@ -22,6 +27,24 @@ def log_fingerprint(CP: structs.CarParams) -> None: sentry.capture_fingerprint(CP.carFingerprint, CP.brand) +def initialize_neural_network_lateral_control(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params) -> None: + nnlc_model_path, nnlc_model_name, fuzzy_fingerprint = get_nn_model_path(CP) + + if nnlc_model_path is None: + cloudlog.error({"nnlc event": "car doesn't match any Neural Network model"}) + nnlc_model_path = "MOCK" + + if nnlc_model_path != "MOCK" and CP.steerControlType != structs.CarParams.SteerControlType.angle: + CP_SP.neuralNetworkLateralControl.enabled = params.get_bool("NeuralNetworkLateralControl") + + if CP_SP.neuralNetworkLateralControl.enabled: + CarInterfaceBase.configure_torque_tune(CP.carFingerprint, CP.lateralTuning) + + CP_SP.neuralNetworkLateralControl.modelPath = os.path.splitext(os.path.basename(nnlc_model_path))[0] + CP_SP.neuralNetworkLateralControl.modelName = nnlc_model_name + CP_SP.neuralNetworkLateralControl.fuzzyFingerprint = fuzzy_fingerprint + + def setup_car_interface_sp(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params): if CP.brand == 'hyundai': if CP.flags & HyundaiFlags.MANDO_RADAR and CP.radarUnavailable: @@ -32,6 +55,8 @@ def setup_car_interface_sp(CP: structs.CarParams, CP_SP: structs.CarParamsSP, pa if params.get_bool("HyundaiRadarTracks"): CP.radarUnavailable = False + initialize_neural_network_lateral_control(CP, CP_SP, params) + def initialize_car_interface_sp(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params, can_recv: CanRecvCallable, can_send: CanSendCallable): diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/__init__.py b/sunnypilot/selfdrive/controls/lib/nnlc/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/flux_model.py b/sunnypilot/selfdrive/controls/lib/nnlc/flux_model.py new file mode 100644 index 0000000000..24ccf5e483 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/nnlc/flux_model.py @@ -0,0 +1,100 @@ +""" +The MIT License + +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + +Last updated: July 29, 2024 +""" + +import numpy as np +from json import load + +from openpilot.sunnypilot.selfdrive.car.nnlc.helpers import ACTIVATION_FUNCTION_NAMES + + +class FluxModel: + def __init__(self, params_file, zero_bias=False): + with open(params_file, "r") as f: + params = load(f) + + self.input_size = params["input_size"] + self.output_size = params["output_size"] + self.input_mean = np.array(params["input_mean"], dtype=np.float32).T + self.input_std = np.array(params["input_std"], dtype=np.float32).T + self.layers = [] + self.friction_override = False + + for layer_params in params["layers"]: + W = np.array(layer_params[next(key for key in layer_params.keys() if key.endswith('_W'))], dtype=np.float32).T + b = np.array(layer_params[next(key for key in layer_params.keys() if key.endswith('_b'))], dtype=np.float32).T + if zero_bias: + b = np.zeros_like(b) + activation = layer_params["activation"] + for k, v in ACTIVATION_FUNCTION_NAMES.items(): + activation = activation.replace(k, v) + self.layers.append((W, b, activation)) + + self.validate_layers() + self.check_for_friction_override() + + # Begin activation functions. + # These are called by name using the keys in the model json file + @staticmethod + def sigmoid(x): + return 1 / (1 + np.exp(-x)) + + @staticmethod + def identity(x): + return x + # End activation functions + + def forward(self, x): + for W, b, activation in self.layers: + x = getattr(self, activation)(x.dot(W) + b) + return x + + def evaluate(self, input_array): + in_len = len(input_array) + if in_len != self.input_size: + # If the input is length 2-4, then it's a simplified evaluation. + # In that case, need to add on zeros to fill out the input array to match the correct length. + if 2 <= in_len: + input_array = input_array + [0] * (self.input_size - in_len) + else: + raise ValueError(f"Input array length {len(input_array)} must be length 2 or greater") + + input_array = np.array(input_array, dtype=np.float32) + + # Rescale the input array using the input_mean and input_std + input_array = (input_array - self.input_mean) / self.input_std + + output_array = self.forward(input_array) + + return float(output_array[0, 0]) + + def validate_layers(self): + for W, b, activation in self.layers: + if not hasattr(self, activation): + raise ValueError(f"Unknown activation: {activation}") + + def check_for_friction_override(self): + y = self.evaluate([10.0, 0.0, 0.2]) + self.friction_override = (y < 0.1) diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/helpers.py b/sunnypilot/selfdrive/controls/lib/nnlc/helpers.py new file mode 100644 index 0000000000..4e6b65cb9b --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/nnlc/helpers.py @@ -0,0 +1,74 @@ +""" +The MIT License + +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + +Last updated: July 29, 2024 +""" +import os +from difflib import SequenceMatcher + +from opendbc.car import structs +from openpilot.common.basedir import BASEDIR + +# dict used to rename activation functions whose names aren't valid python identifiers +ACTIVATION_FUNCTION_NAMES = {'σ': 'sigmoid'} + +TORQUE_NN_MODEL_PATH = os.path.join(BASEDIR, 'lat_models') + + +def similarity(s1: str, s2: str) -> float: + return SequenceMatcher(None, s1, s2).ratio() + + +def get_nn_model_path(CP: structs.CarParams) -> tuple[str | None, str, bool]: + _car = CP.carFingerprint + _eps_fw = str(next((fw.fwVersion for fw in CP.carFw if fw.ecu == "eps"), "")) + _model_name = "" + + def check_nn_path(_check_model): + _model_path = None + _max_similarity = -1.0 + for f in os.listdir(TORQUE_NN_MODEL_PATH): + if f.endswith(".json"): + model = f.replace(".json", "").replace(f"{TORQUE_NN_MODEL_PATH}/", "") + similarity_score = similarity(model, _check_model) + if similarity_score > _max_similarity: + _max_similarity = similarity_score + _model_path = os.path.join(TORQUE_NN_MODEL_PATH, f) + return _model_path, _max_similarity + + if len(_eps_fw) > 3: + _eps_fw = _eps_fw.replace("\\", "") + check_model = f"{_car} {_eps_fw}" + else: + check_model = _car + model_path, max_similarity = check_nn_path(check_model) + if 0.0 <= max_similarity < 0.9: + check_model = _car + model_path, max_similarity = check_nn_path(check_model) + if 0.0 <= max_similarity < 0.9: + model_path = None + + _model_name = os.path.splitext(os.path.basename(model_path))[0] if model_path else "MOCK" + + fuzzy_fingerprint = max_similarity < 0.99 + return model_path, _model_name, fuzzy_fingerprint