Merge branch 'improved-model-cabapilities-check' into master-dev-c3

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

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
 

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")

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']
 

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

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)