From b5227c0a06dbff03bf97c196233d1bf9b4f48f8e Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 7 Jul 2024 12:21:03 -0400 Subject: [PATCH] Driving Model Selector v5 --- cereal/custom.capnp | 12 ++++ selfdrive/controls/controlsd.py | 9 ++- selfdrive/controls/lib/desire_helper.py | 9 ++- selfdrive/controls/plannerd.py | 8 +-- selfdrive/modeld/custom_model_metadata.py | 66 +++++++++++++++++++ selfdrive/modeld/fill_model_msg.py | 6 +- selfdrive/modeld/model_capabilities.py | 41 ------------ selfdrive/modeld/modeld.py | 62 +++++++++-------- selfdrive/modeld/navmodeld.py | 6 +- selfdrive/sunnypilot/__init__.py | 10 --- .../qt/offroad/sunnypilot/models_fetcher.cc | 2 +- .../offroad/sunnypilot/sunnypilot_settings.cc | 8 +-- selfdrive/ui/qt/onroad/annotated_camera.cc | 4 +- selfdrive/ui/qt/onroad/annotated_camera.h | 2 +- selfdrive/ui/qt/onroad/buttons.cc | 2 +- selfdrive/ui/qt/onroad_settings.cc | 4 +- selfdrive/ui/ui.cc | 11 +++- selfdrive/ui/ui.h | 5 +- system/manager/process_config.py | 6 +- 19 files changed, 154 insertions(+), 119 deletions(-) create mode 100644 selfdrive/modeld/custom_model_metadata.py delete mode 100644 selfdrive/modeld/model_capabilities.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 3b36532ac9..8825cf6f07 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -25,6 +25,15 @@ enum AccelerationPersonality { stock @3; } +enum ModelGeneration { + default @0; + one @1; + two @2; + three @3; + four @4; + five @5; +} + struct ControlsStateSP @0x81c2f05a394cf4af { lateralState @0 :Text; personality @8 :LongitudinalPersonalitySP; @@ -189,6 +198,9 @@ struct E2eLongStateSP @0xa5cd762cd951a455 { struct ModelDataV2SP @0xf98d843bfd7004a3 { laneChangePrev @0 :Bool; laneChangeEdgeBlock @1 :Bool; + customModel @2 :Bool; + modelGeneration @3 :ModelGeneration; + modelCapabilities @4 :UInt32; } struct CustomReserved7 @0xb86e6369214c01c8 { diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 7ba19a5b04..2af32f8a0c 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -28,8 +28,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, S from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.longcontrol import LongControl from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel -from openpilot.selfdrive.modeld.model_capabilities import ModelCapabilities -from openpilot.selfdrive.sunnypilot import get_model_generation +from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities from openpilot.system.athena.registration import is_registered_device from openpilot.system.hardware import HARDWARE @@ -182,9 +181,9 @@ class Controls: self.mads_ndlob = self.enable_mads and not self.mads_disengage_lateral_on_brake self.process_not_running = False - self.custom_model, self.model_gen = get_model_generation(self.params) - model_capabilities = ModelCapabilities.get_by_gen(self.model_gen) - self.model_use_lateral_planner = self.custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution + self.custom_model_metadata = CustomModelMetadata(params=self.params, init_only=True) + self.model_use_lateral_planner = self.custom_model_metadata.valid and \ + self.custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution self.dynamic_personality = self.params.get_bool("DynamicPersonality") diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index bf2e2cba86..f335170adb 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -3,8 +3,7 @@ from openpilot.common.conversions import Conversions as CV from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.controls.lib.drive_helpers import get_road_edge -from openpilot.selfdrive.modeld.model_capabilities import ModelCapabilities -from openpilot.selfdrive.sunnypilot import get_model_generation +from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection @@ -69,9 +68,9 @@ class DesireHelper: self.lane_change_set_timer = int(self.param_s.get("AutoLaneChangeTimer", encoding="utf8")) self.lane_change_bsm_delay = self.param_s.get_bool("AutoLaneChangeBsmDelay") - self.custom_model, self.model_gen = get_model_generation(self.param_s) - model_capabilities = ModelCapabilities.get_by_gen(self.model_gen) - self.model_use_lateral_planner = self.custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution + self.custom_model_metadata = CustomModelMetadata(params=self.param_s, init_only=True) + self.model_use_lateral_planner = self.custom_model_metadata.valid and \ + self.custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution def read_param(self): self.edge_toggle = self.param_s.get_bool("RoadEdge") diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 640251f163..0d1eaeb075 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -8,8 +8,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner from openpilot.selfdrive.controls.lib.lateral_planner import LateralPlanner -from openpilot.selfdrive.modeld.model_capabilities import ModelCapabilities -from openpilot.selfdrive.sunnypilot import get_model_generation +from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities import cereal.messaging as messaging @@ -26,9 +25,8 @@ def plannerd_thread(): longitudinal_planner = LongitudinalPlanner(CP) - custom_model, model_gen = get_model_generation(params) - model_capabilities = ModelCapabilities.get_by_gen(model_gen) - model_use_lateral_planner = custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution + custom_model_metadata = CustomModelMetadata(params=params, init_only=True) + model_use_lateral_planner = custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution lateral_planner = LateralPlanner(CP, debug=debug_mode, model_use_lateral_planner=model_use_lateral_planner) lateral_planner_svs = ['lateralPlanDEPRECATED', 'lateralPlanSPDEPRECATED'] diff --git a/selfdrive/modeld/custom_model_metadata.py b/selfdrive/modeld/custom_model_metadata.py new file mode 100644 index 0000000000..360f7046ad --- /dev/null +++ b/selfdrive/modeld/custom_model_metadata.py @@ -0,0 +1,66 @@ +from enum import IntFlag +import os + +from cereal import custom +from openpilot.common.params import Params + +SIMULATION = "SIMULATION" in os.environ + +ModelGeneration = custom.ModelGeneration + + +class ModelCapabilities(IntFlag): + """Model capabilities for different generations of models.""" + + Default = 1 + """Default capability, used for the prebuilt model.""" + + NoO = 2 + """Navigation on Openpilot capability, used for models support navigation.""" + + LateralPlannerSolution = 2 ** 2 + """LateralPlannerSolution capability, used for models that support the lateral planner solution.""" + + DesiredCurvatureV1 = 2 ** 3 + """ + DesiredCurvatureV1 capability: This capability is used for models that support the desired curvature. + In this version, 'prev_desired_curvs' is used as the input for the 'desired_curvature' output. + """ + + DesiredCurvatureV2 = 2 ** 4 + """ + DesiredCurvatureV2 capability: This capability is used for models that support the desired curvature. + In V2, 'prev_desired_curv' (no plural) is used as the input for the same 'desired_curvature' output. + """ + + +class CustomModelMetadata: + def __init__(self, params=None, init_only=False) -> None: + # TODO: Handle this with cereal + if not init_only: + raise RuntimeError("cannot be used in a loop, this should only be used on init") + + self.params: Params = params + self.generation: ModelGeneration = self.read_model_generation_param() + self.capabilities: int = self.get_model_capabilities() + self.valid: bool = self.params.get_bool("CustomDrivingModel") and not SIMULATION and \ + self.capabilities != ModelCapabilities.Default + + def read_model_generation_param(self) -> ModelGeneration: + return int(self.params.get('DrivingModelGeneration') or ModelGeneration.default) + + def get_model_capabilities(self) -> int: + """Returns the model capabilities for a given generation.""" + if self.generation == ModelGeneration.five: + return ModelCapabilities.DesiredCurvatureV2 + elif self.generation == ModelGeneration.four: + return ModelCapabilities.DesiredCurvatureV2 + elif self.generation == ModelGeneration.three: + return ModelCapabilities.DesiredCurvatureV2 | ModelCapabilities.NoO + elif self.generation == ModelGeneration.two: + return ModelCapabilities.DesiredCurvatureV1 | ModelCapabilities.NoO + elif self.generation == ModelGeneration.one: + return ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO + else: + # Default model is meant to represent the capabilities of the prebuilt model + return ModelCapabilities.Default diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index a955e7aef7..39a161ed89 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -3,6 +3,7 @@ import capnp import numpy as np from cereal import log from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta +from openpilot.selfdrive.modeld.custom_model_metadata import ModelCapabilities SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -54,7 +55,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float, timestamp_eof: int, timestamp_llk: int, model_execution_time: float, nav_enabled: bool, valid: bool, - model_use_lateral_planner: bool, model_use_nav: bool) -> None: + custom_model_valid: bool, custom_model_capabilities: ModelCapabilities) -> None: frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0 frame_drop_perc = frame_drop * 100 extended_msg.valid = valid @@ -75,6 +76,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D modelV2.frameAge = frame_age modelV2.frameDropPerc = frame_drop_perc modelV2.timestampEof = timestamp_eof + model_use_nav = custom_model_valid and custom_model_capabilities & ModelCapabilities.NoO if model_use_nav: modelV2.locationMonoTimeDEPRECATED = timestamp_llk modelV2.modelExecutionTime = model_execution_time @@ -98,7 +100,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D fill_xyz_poly(poly_path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T) # lateral planning - if model_use_lateral_planner: + if custom_model_valid and custom_model_capabilities & ModelCapabilities.LateralPlannerSolution: solution = modelV2.lateralPlannerSolutionDEPRECATED solution.x, solution.y, solution.yaw, solution.yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)] solution.xStd, solution.yStd, solution.yawStd, solution.yawRateStd = [net_output_data['lat_planner_solution_stds'][0,:,i].tolist() for i in range(4)] diff --git a/selfdrive/modeld/model_capabilities.py b/selfdrive/modeld/model_capabilities.py deleted file mode 100644 index dea0143a8b..0000000000 --- a/selfdrive/modeld/model_capabilities.py +++ /dev/null @@ -1,41 +0,0 @@ -from enum import IntFlag, auto - - -class ModelCapabilities(IntFlag): - """Model capabilities for different generations of models.""" - - Default = auto() - """Default capability, used for the prebuilt model.""" - - NoO = auto() - """Navigation on Openpilot capability, used for models support navigation.""" - - LateralPlannerSolution = auto() - """LateralPlannerSolution capability, used for models that support the lateral planner solution.""" - - DesiredCurvatureV1 = auto() - """ - DesiredCurvatureV1 capability: This capability is used for models that support the desired curvature. - In this version, 'prev_desired_curvs' is used as the input for the 'desired_curvature' output. - """ - - DesiredCurvatureV2 = auto() - """ - DesiredCurvatureV2 capability: This capability is used for models that support the desired curvature. - In V2, 'prev_desired_curv' (no plural) is used as the input for the same 'desired_curvature' output. - """ - - @staticmethod - def get_by_gen(gen): - """Returns the model capabilities for a given generation.""" - if gen == 1: - return ModelCapabilities.Default | ModelCapabilities.LateralPlannerSolution | ModelCapabilities.NoO - elif gen == 2: - return ModelCapabilities.Default | ModelCapabilities.DesiredCurvatureV1 | ModelCapabilities.NoO - elif gen == 3: - return ModelCapabilities.Default | ModelCapabilities.DesiredCurvatureV2 | ModelCapabilities.NoO - elif gen == 4: - return ModelCapabilities.Default | ModelCapabilities.DesiredCurvatureV2 - else: - # Default model is meant to represent the capabilities of the prebuilt model. - return ModelCapabilities.Default diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 730aaa1331..ff3d9ec633 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -20,13 +20,12 @@ from openpilot.common.transformations.model import get_warp_matrix from openpilot.system import sentry from openpilot.selfdrive.car.car_helpers import get_demo_car_params from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper -from openpilot.selfdrive.modeld.model_capabilities import ModelCapabilities +from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime from openpilot.selfdrive.modeld.parse_model_outputs import Parser from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext -from openpilot.selfdrive.sunnypilot import get_model_generation from openpilot.system.hardware.hw import Paths THREAD_NAME = "selfdrive.modeld.modeld" @@ -62,8 +61,7 @@ class ModelState: def __init__(self, context: CLContext): self.param_s = Params() - self.custom_model, self.model_gen = get_model_generation(self.param_s) - self.model_capabilities = ModelCapabilities.get_by_gen(self.model_gen) + self.custom_model_metadata = CustomModelMetadata(params=self.param_s, init_only=True) self.frame = ModelFrame(context) self.wide_frame = ModelFrame(context) self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) @@ -73,17 +71,17 @@ class ModelState: 'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32), } _inputs_2 = {} - if self.custom_model and self.model_capabilities != ModelCapabilities.Default: - if self.model_capabilities & ModelCapabilities.LateralPlannerSolution: + if self.custom_model_metadata.valid: + if self.custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution: _inputs = { 'lat_planner_state': np.zeros(ModelConstants.LAT_PLANNER_STATE_LEN, dtype=np.float32), } - if self.model_capabilities & ModelCapabilities.DesiredCurvatureV1: + if self.custom_model_metadata.capabilities & ModelCapabilities.DesiredCurvatureV1: _inputs = { 'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32), 'prev_desired_curvs': np.zeros(ModelConstants.PREV_DESIRED_CURVS_LEN, dtype=np.float32), } - if self.model_capabilities & ModelCapabilities.NoO: + if self.custom_model_metadata.capabilities & ModelCapabilities.NoO: _inputs_2 = { 'nav_features': np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32), 'nav_instructions': np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32), @@ -97,7 +95,7 @@ class ModelState: 'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32), } - if self.custom_model and self.model_capabilities != ModelCapabilities.Default: + if self.custom_model_metadata.valid: _model_name = self.param_s.get("DrivingModelText", encoding="utf8") _model_paths = {ModelRunner.THNEED: f"{CUSTOM_MODEL_PATH}/supercombo-{_model_name}.thneed"} _metadata_name = self.param_s.get("DrivingModelMetadataText", encoding="utf8") @@ -135,9 +133,9 @@ class ModelState: self.prev_desire[:] = inputs['desire'] self.inputs['traffic_convention'][:] = inputs['traffic_convention'] - if not (self.custom_model and self.model_capabilities & ModelCapabilities.LateralPlannerSolution): + if not (self.custom_model_metadata.valid and self.custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution): self.inputs['lateral_control_params'][:] = inputs['lateral_control_params'] - if self.custom_model and self.model_capabilities & ModelCapabilities.NoO: + if self.custom_model_metadata.valid and self.custom_model_metadata.capabilities & ModelCapabilities.NoO: self.inputs['nav_features'][:] = inputs['nav_features'] self.inputs['nav_instructions'][:] = inputs['nav_instructions'] @@ -154,11 +152,11 @@ class ModelState: self.inputs['features_buffer'][:-ModelConstants.FEATURE_LEN] = self.inputs['features_buffer'][ModelConstants.FEATURE_LEN:] self.inputs['features_buffer'][-ModelConstants.FEATURE_LEN:] = outputs['hidden_state'][0, :] - if self.custom_model and self.model_capabilities != ModelCapabilities.Default: - if self.model_capabilities & ModelCapabilities.LateralPlannerSolution: + if self.custom_model_metadata.valid: + if self.custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution: self.inputs['lat_planner_state'][2] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 2]) self.inputs['lat_planner_state'][3] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 3]) - elif self.model_capabilities & ModelCapabilities.DesiredCurvatureV1: + elif self.custom_model_metadata.capabilities & ModelCapabilities.DesiredCurvatureV1: self.inputs['prev_desired_curvs'][:-1] = self.inputs['prev_desired_curvs'][1:] self.inputs['prev_desired_curvs'][-1] = outputs['desired_curvature'][0, 0] else: # Default model, and as of time of writing, this model uses DesiredCurvatureV2 @@ -205,12 +203,11 @@ def main(demo=False): cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})") params = Params() - custom_model, model_gen = get_model_generation(params) - model_capabilities = ModelCapabilities.get_by_gen(model_gen) + custom_model_metadata = CustomModelMetadata(params=params, init_only=True) # messaging extended_svs = ["lateralPlanDEPRECATED", "lateralPlanSPDEPRECATED"] - if custom_model and model_capabilities & ModelCapabilities.NoO: + if custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.NoO: extended_svs += ["navModelDEPRECATED", "navInstruction"] pm = PubMaster(["modelV2", "modelV2SP", "drivingModelData", "cameraOdometry"]) sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"] + extended_svs) @@ -226,9 +223,9 @@ def main(demo=False): model_transform_main = np.zeros((3, 3), dtype=np.float32) model_transform_extra = np.zeros((3, 3), dtype=np.float32) live_calib_seen = False - if custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution: + if custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution: driving_style = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32) - if custom_model and model_capabilities & ModelCapabilities.NoO: + if custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.NoO: nav_features = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32) nav_instructions = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32) buf_main, buf_extra = None, None @@ -282,10 +279,13 @@ def main(demo=False): meta_extra = meta_main sm.update(0) - desire = sm["lateralPlanDEPRECATED"].desire.raw if custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution else DH.desire + if custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution: + desire = sm["lateralPlanDEPRECATED"].desire.raw + else: + desire = DH.desire is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId - if not (custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution): + if not (custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution): lateral_control_params = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32) if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) @@ -303,7 +303,7 @@ def main(demo=False): timestamp_llk = 0 nav_enabled = False - if custom_model and model_capabilities & ModelCapabilities.NoO: + if custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.NoO: # Enable/disable nav features timestamp_llk = sm["navModelDEPRECATED"].locationMonoTime nav_valid = sm.valid["navModelDEPRECATED"] # and (nanos_since_boot() - timestamp_llk < 1e9) @@ -341,7 +341,7 @@ def main(demo=False): if prepare_only: cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames") - if custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution: + if custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution: _inputs = { 'driving_style': driving_style } @@ -349,7 +349,7 @@ def main(demo=False): _inputs = { 'lateral_control_params': lateral_control_params } - if custom_model and model_capabilities & ModelCapabilities.NoO: + if custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.NoO: _inputs_2 = { 'nav_features': nav_features, 'nav_instructions': nav_instructions @@ -377,9 +377,9 @@ def main(demo=False): posenet_send = messaging.new_message('cameraOdometry') fill_model_msg(drivingdata_send, modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen, - custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution, custom_model and model_capabilities & ModelCapabilities.NoO) + custom_model_metadata.valid, custom_model_metadata.capabilities) - if not (custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution): + if not (custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution): desire_state = modelv2_send.modelV2.meta.desireState l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] r_lane_change_prob = desire_state[log.Desire.laneChangeRight] @@ -398,9 +398,13 @@ def main(demo=False): modelv2_sp_send = messaging.new_message('modelV2SP') modelv2_sp_send.valid = True - if not (custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution): - modelv2_sp_send.modelV2SP.laneChangePrev = DH.prev_lane_change - modelv2_sp_send.modelV2SP.laneChangeEdgeBlock = lat_plan_sp.laneChangeEdgeBlockDEPRECATED + modelV2SP = modelv2_sp_send.modelV2SP + if not (custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution): + modelV2SP.laneChangePrev = DH.prev_lane_change + modelV2SP.laneChangeEdgeBlock = lat_plan_sp.laneChangeEdgeBlockDEPRECATED + modelV2SP.customModel = custom_model_metadata.valid + modelV2SP.modelGeneration = custom_model_metadata.generation + modelV2SP.modelCapabilities = int(custom_model_metadata.capabilities) pm.send('modelV2SP', modelv2_sp_send) last_vipc_frame_id = meta_main.frame_id diff --git a/selfdrive/modeld/navmodeld.py b/selfdrive/modeld/navmodeld.py index 671626c57c..4eadddc713 100755 --- a/selfdrive/modeld/navmodeld.py +++ b/selfdrive/modeld/navmodeld.py @@ -13,8 +13,8 @@ from openpilot.common.swaglog import cloudlog from openpilot.common.params import Params from openpilot.common.realtime import set_realtime_priority from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime -from openpilot.selfdrive.sunnypilot import get_model_generation from openpilot.system.hardware.hw import Paths NAV_INPUT_SIZE = 256*256 @@ -50,8 +50,8 @@ class ModelState: assert ctypes.sizeof(NavModelResult) == NAV_OUTPUT_SIZE * ctypes.sizeof(ctypes.c_float) self.output = np.zeros(NAV_OUTPUT_SIZE, dtype=np.float32) self.param_s = Params() - self.custom_model, self.model_gen = get_model_generation(self.param_s) - if self.custom_model and self.model_gen not in (0, 4): + self.custom_model_metadata = CustomModelMetadata(params=self.param_s, init_only=True) + if self.custom_model_metadata.valid and self.custom_model_metadata.capabilities & ModelCapabilities.NoO: _model_name = self.param_s.get("NavModelText", encoding="utf8") _model_paths = { ModelRunner.SNPE: f"{CUSTOM_MODEL_PATH}/navmodel_q_{_model_name}.dlc"} diff --git a/selfdrive/sunnypilot/__init__.py b/selfdrive/sunnypilot/__init__.py index 96b5462601..e69de29bb2 100644 --- a/selfdrive/sunnypilot/__init__.py +++ b/selfdrive/sunnypilot/__init__.py @@ -1,10 +0,0 @@ -import os - - -SIMULATION = "SIMULATION" in os.environ - - -def get_model_generation(params): - custom_model = params.get_bool("CustomDrivingModel") and not SIMULATION - gen = int(params.get("DrivingModelGeneration", encoding="utf8")) - return custom_model, gen diff --git a/selfdrive/ui/qt/offroad/sunnypilot/models_fetcher.cc b/selfdrive/ui/qt/offroad/sunnypilot/models_fetcher.cc index 230af1fe9c..3df9c09fe3 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/models_fetcher.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot/models_fetcher.cc @@ -150,5 +150,5 @@ std::vector ModelsFetcher::getModelsFromURL(const QString&url) { } std::vector ModelsFetcher::getModelsFromURL() { - return getModelsFromURL("https://docs.sunnypilot.ai/models_v4.json"); + return getModelsFromURL("https://docs.sunnypilot.ai/models_v5.json"); } diff --git a/selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc index 0413102bb5..8db23500b4 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc @@ -437,10 +437,10 @@ void SunnypilotPanel::updateToggles() { toggles["VisionCurveLaneless"]->setEnabled(dynamic_lane_profile_param == "2"); toggles["VisionCurveLaneless"]->refresh(); - bool custom_driving_model = params.getBool("CustomDrivingModel"); - auto driving_model_gen = QString::fromStdString(params.get("DrivingModelGeneration")); - bool model_use_lateral_planner = custom_driving_model && driving_model_gen == "1"; - auto driving_model_name = custom_driving_model && driving_model_gen != "0" ? QString::fromStdString(params.get("DrivingModelName")) : CURRENT_MODEL; + bool custom_driving_model_valid = params.getBool("CustomDrivingModel"); + auto driving_model_generation = QString::fromStdString(params.get("DrivingModelGeneration")); + bool model_use_lateral_planner = custom_driving_model_valid && driving_model_generation == "1"; + auto driving_model_name = custom_driving_model_valid && driving_model_generation != "0" ? QString::fromStdString(params.get("DrivingModelName")) : CURRENT_MODEL; QString driving_model_text = QString("" + driving_model_name + ""); dlp_settings->setEnabled(model_use_lateral_planner); toggles["VisionCurveLaneless"]->setVisible(model_use_lateral_planner); diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 2e0bc73469..7e5629c4ef 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -375,7 +375,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { featureStatusToggle = s.scene.feature_status_toggle; experimental_btn->setVisible(!(showDebugUI && showVTC)); - drivingModelGen = s.scene.driving_model_gen; + drivingModelGen = s.scene.driving_model_generation; } void AnnotatedCameraWidget::drawHud(QPainter &p) { @@ -1209,7 +1209,7 @@ void AnnotatedCameraWidget::drawFeatureStatusText(QPainter &p, int x, int y) { } // Dynamic Lane Profile - if (drivingModelGen == 1) { + if (drivingModelGen == cereal::ModelGeneration::ONE) { drawFeatureStatusElement(dynamicLaneProfile, feature_text.dlp_list_text, feature_color.dlp_list_color, true, "OFF", "DLP"); } diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 7c883da80f..94bd12c8c0 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -203,7 +203,7 @@ private: bool featureStatusToggle; - int drivingModelGen; + cereal::ModelGeneration drivingModelGen; protected: void paintGL() override; diff --git a/selfdrive/ui/qt/onroad/buttons.cc b/selfdrive/ui/qt/onroad/buttons.cc index 241d413cc8..fbca42da7b 100644 --- a/selfdrive/ui/qt/onroad/buttons.cc +++ b/selfdrive/ui/qt/onroad/buttons.cc @@ -99,7 +99,7 @@ void OnroadSettingsButton::paintEvent(QPaintEvent *event) { void OnroadSettingsButton::updateState(const UIState &s) { const auto cp = (*s.sm)["carParams"].getCarParams(); - auto dlp_enabled = s.scene.driving_model_gen == 1; + auto dlp_enabled = s.scene.driving_model_generation == cereal::ModelGeneration::ONE; bool allow_btn = s.scene.onroad_settings_toggle && (dlp_enabled || hasLongitudinalControl(cp) || !cp.getPcmCruiseSpeed()); setVisible(allow_btn); diff --git a/selfdrive/ui/qt/onroad_settings.cc b/selfdrive/ui/qt/onroad_settings.cc index d5fd2d890d..00810294ba 100644 --- a/selfdrive/ui/qt/onroad_settings.cc +++ b/selfdrive/ui/qt/onroad_settings.cc @@ -119,7 +119,7 @@ OnroadSettings::OnroadSettings(bool closeable, QWidget *parent) : QFrame(parent) void OnroadSettings::changeDynamicLaneProfile() { UIScene &scene = uiState()->scene; - bool can_change = scene.driving_model_gen == 1; + bool can_change = scene.driving_model_generation == cereal::ModelGeneration::ONE; if (can_change) { scene.dynamic_lane_profile++; scene.dynamic_lane_profile = scene.dynamic_lane_profile > 2 ? 0 : scene.dynamic_lane_profile; @@ -229,7 +229,7 @@ void OnroadSettings::refresh() { // Dynamic Lane Profile dlp_widget->updateDynamicLaneProfile("DynamicLaneProfile"); - dlp_widget->setVisible(scene.driving_model_gen == 1); + dlp_widget->setVisible(scene.driving_model_generation == cereal::ModelGeneration::ONE); // Gap Adjust Cruise gac_widget->updateGapAdjustCruise("LongitudinalPersonality"); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index e6f2e0d999..126be0d6b1 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -230,6 +230,12 @@ static void update_state(UIState *s) { scene.e2eX[i] = sm["longitudinalPlanSP"].getLongitudinalPlanSP().getE2eX()[i]; } } + if (sm.updated("modelV2SP")) { + auto model_v2_sp = sm["modelV2SP"].getModelV2SP(); + scene.custom_driving_model_valid = model_v2_sp.getCustomModel(); + scene.driving_model_generation = model_v2_sp.getModelGeneration(); + scene.driving_model_capabilities = model_v2_sp.getModelCapabilities(); + } } void ui_update_params(UIState *s) { @@ -260,8 +266,6 @@ void ui_update_params(UIState *s) { s->scene.speed_limit_warning_flash = params.getBool("SpeedLimitWarningFlash"); s->scene.speed_limit_warning_type = std::atoi(params.get("SpeedLimitWarningType").c_str()); s->scene.speed_limit_warning_value_offset = std::atoi(params.get("SpeedLimitWarningValueOffset").c_str()); - s->scene.custom_driving_model = params.getBool("CustomDrivingModel"); - s->scene.driving_model_gen = std::atoi(params.get("DrivingModelGeneration").c_str()); s->scene.speed_limit_control_enabled = params.getBool("EnableSlc"); s->scene.feature_status_toggle = params.getBool("FeatureStatus"); s->scene.onroad_settings_toggle = params.getBool("OnroadSettings"); @@ -388,7 +392,8 @@ UIState::UIState(QObject *parent) : QObject(parent) { "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2", "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "clocks", "longitudinalPlanSP", "liveMapDataSP", - "carControl", "lateralPlanSPDEPRECATED", "gpsLocation", "gpsLocationExternal", "liveParameters", "liveTorqueParameters", "controlsStateSP" + "carControl", "lateralPlanSPDEPRECATED", "gpsLocation", "gpsLocationExternal", "liveParameters", "liveTorqueParameters", + "controlsStateSP", "modelV2SP" }); Params params; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 19387d625e..a9fb6f0671 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -201,8 +201,9 @@ typedef struct UIScene { int speed_limit_warning_type; int speed_limit_warning_value_offset; - bool custom_driving_model; - int driving_model_gen; + bool custom_driving_model_valid; + cereal::ModelGeneration driving_model_generation; + uint32_t driving_model_capabilities; bool feature_status_toggle; bool onroad_settings_toggle; diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 8a8cce69e0..8818d3b851 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -3,7 +3,7 @@ import os from cereal import car from openpilot.common.params import Params from openpilot.system.hardware import PC, TICI -from openpilot.selfdrive.sunnypilot import get_model_generation +from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities from openpilot.system.manager.process import PythonProcess, NativeProcess, DaemonProcess from openpilot.system.mapd_manager import MAPD_PATH, COMMON_DIR from openpilot.system.manager.sunnylink import sunnylink_need_register, sunnylink_ready @@ -45,8 +45,8 @@ def only_offroad(started, params, CP: car.CarParams) -> bool: return not started def model_use_nav(started, params, CP: car.CarParams) -> bool: - custom_model, model_gen = get_model_generation(params) - return started and custom_model and model_gen not in (0, 4) + custom_model_metadata = CustomModelMetadata(params=params, init_only=True) + return started and custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.NoO def sunnylink_ready_shim(started, params, CP: car.CarParams) -> bool: """Shim for sunnylink_ready to match the process manager signature."""