diff --git a/release/files_common b/release/files_common index 131d61f2c0..0ec43716b0 100644 --- a/release/files_common +++ b/release/files_common @@ -365,6 +365,7 @@ selfdrive/modeld/navmodeld.py selfdrive/modeld/dmonitoringmodeld.py selfdrive/modeld/constants.py selfdrive/modeld/modeld +selfdrive/modeld/model_capabilities.py selfdrive/modeld/models/__init__.py selfdrive/modeld/models/*.pxd diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f09fb80d4c..8680e95351 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -31,6 +31,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.system.hardware import HARDWARE @@ -180,7 +181,8 @@ class Controls: self.process_not_running = False self.custom_model, self.model_gen = get_model_generation(self.params) - self.model_use_lateral_planner = self.custom_model and self.model_gen == 1 + model_capabilities = ModelCapabilities.get_by_gen(self.model_gen) + self.model_use_lateral_planner = self.custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution self.can_log_mono_time = 0 diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index bd4decbb80..6cac8a70e2 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -3,6 +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 LaneChangeState = log.LaneChangeState @@ -63,7 +64,8 @@ class DesireHelper: self.lane_change_bsm_delay = self.param_s.get_bool("AutoLaneChangeBsmDelay") self.custom_model, self.model_gen = get_model_generation(self.param_s) - self.model_use_lateral_planner = self.custom_model and self.model_gen == 1 + model_capabilities = ModelCapabilities.get_by_gen(self.model_gen) + self.model_use_lateral_planner = self.custom_model and model_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 a9e8cabca7..588e044c3f 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -8,6 +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 import cereal.messaging as messaging @@ -47,7 +48,8 @@ def plannerd_thread(): longitudinal_planner = LongitudinalPlanner(CP) custom_model, model_gen = get_model_generation(params) - model_use_lateral_planner = custom_model and model_gen == 1 + model_capabilities = ModelCapabilities.get_by_gen(model_gen) + model_use_lateral_planner = custom_model and model_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/model_capabilities.py b/selfdrive/modeld/model_capabilities.py new file mode 100644 index 0000000000..dea0143a8b --- /dev/null +++ b/selfdrive/modeld/model_capabilities.py @@ -0,0 +1,41 @@ +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 2208fa42ea..2b8d05984e 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -20,6 +20,7 @@ from openpilot.common.transformations.model import get_warp_matrix from openpilot.selfdrive 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.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 @@ -61,25 +62,27 @@ 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.frame = ModelFrame(context) self.wide_frame = ModelFrame(context) self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) + # Default model, and as of time of writing, this model uses DesiredCurvatureV2 _inputs = { 'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32), '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_gen != 0: - if self.model_gen == 1: + if self.custom_model and self.model_capabilities != ModelCapabilities.Default: + if self.model_capabilities & ModelCapabilities.LateralPlannerSolution: _inputs = { 'lat_planner_state': np.zeros(ModelConstants.LAT_PLANNER_STATE_LEN, dtype=np.float32), } - if self.model_gen == 2: + if self.model_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_gen not in (0, 4): + if self.model_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), @@ -93,7 +96,7 @@ class ModelState: 'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32), } - if self.custom_model and self.model_gen != 0: + if self.custom_model and self.model_capabilities != ModelCapabilities.Default: _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") @@ -131,9 +134,9 @@ class ModelState: self.prev_desire[:] = inputs['desire'] self.inputs['traffic_convention'][:] = inputs['traffic_convention'] - if not (self.custom_model and self.model_gen == 1): + if not (self.custom_model and self.model_capabilities & ModelCapabilities.LateralPlannerSolution): self.inputs['lateral_control_params'][:] = inputs['lateral_control_params'] - if self.custom_model and self.model_gen not in (0, 4): + if self.custom_model and self.model_capabilities & ModelCapabilities.NoO: self.inputs['nav_features'][:] = inputs['nav_features'] self.inputs['nav_instructions'][:] = inputs['nav_instructions'] @@ -150,14 +153,14 @@ 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_gen != 0: - if self.model_gen == 1: + if self.custom_model and self.model_capabilities != ModelCapabilities.Default: + if self.model_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_gen == 2: + elif self.model_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: + else: # Default model, and as of time of writing, this model uses DesiredCurvatureV2 self.inputs['prev_desired_curv'][:-ModelConstants.PREV_DESIRED_CURV_LEN] = self.inputs['prev_desired_curv'][ModelConstants.PREV_DESIRED_CURV_LEN:] self.inputs['prev_desired_curv'][-ModelConstants.PREV_DESIRED_CURV_LEN:] = outputs['desired_curvature'][0, :] return outputs @@ -202,10 +205,11 @@ def main(demo=False): params = Params() custom_model, model_gen = get_model_generation(params) + model_capabilities = ModelCapabilities.get_by_gen(model_gen) # messaging extended_svs = ["lateralPlanDEPRECATED", "lateralPlanSPDEPRECATED"] - if custom_model and model_gen not in (0, 4): + if custom_model and model_capabilities & ModelCapabilities.NoO: extended_svs += ["navModelDEPRECATED", "navInstruction"] pm = PubMaster(["modelV2", "modelV2SP", "cameraOdometry"]) sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"] + extended_svs) @@ -221,9 +225,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_gen == 1: + if custom_model and model_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_gen not in (0, 4): + if custom_model and model_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 @@ -278,10 +282,10 @@ def main(demo=False): meta_extra = meta_main sm.update(0) - desire = sm["lateralPlanDEPRECATED"].desire.raw if custom_model and model_gen == 1 else DH.desire + desire = sm["lateralPlanDEPRECATED"].desire.raw if custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution else DH.desire is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId - if not (custom_model and model_gen == 1): + if not (custom_model and model_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) @@ -299,7 +303,7 @@ def main(demo=False): timestamp_llk = 0 nav_enabled = False - if custom_model and model_gen not in (0, 4): + if custom_model and model_capabilities & ModelCapabilities.NoO: # Enable/disable nav features timestamp_llk = sm["navModelDEPRECATED"].locationMonoTime nav_valid = sm.valid["navModelDEPRECATED"] # and (nanos_since_boot() - timestamp_llk < 1e9) @@ -337,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_gen == 1: + if custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution: _inputs = { 'driving_style': driving_style } @@ -345,7 +349,7 @@ def main(demo=False): _inputs = { 'lateral_control_params': lateral_control_params } - if custom_model and model_gen not in (0, 4): + if custom_model and model_capabilities & ModelCapabilities.NoO: _inputs_2 = { 'nav_features': nav_features, 'nav_instructions': nav_instructions @@ -372,9 +376,9 @@ def main(demo=False): posenet_send = messaging.new_message('cameraOdometry') fill_model_msg(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_gen == 1, custom_model and model_gen not in (0, 4)) + custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution, custom_model and model_capabilities & ModelCapabilities.NoO) - if not (custom_model and model_gen == 1): + if not (custom_model and model_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] @@ -389,7 +393,7 @@ def main(demo=False): modelv2_sp_send = messaging.new_message('modelV2SP') modelv2_sp_send.valid = True - if not (custom_model and model_gen == 1): + 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 pm.send('modelV2SP', modelv2_sp_send)