1b4308b7-a659-4ebd-b4c6-c81c1c3890f8/700

selfdrive/modeld/modeld.py

@@ -9,15 +9,19 @@ from cereal.messaging import PubMaster, SubMaster
 from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
 from openpilot.system.swaglog import cloudlog
 from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.numpy_fast import interp
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import config_realtime_process
 from openpilot.common.transformations.model import get_warp_matrix
 from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
+from openpilot.selfdrive.modeld.constants import T_IDXS
+from openpilot.selfdrive.controls.lib.lateral_planner import TRAJECTORY_SIZE
 from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext
 from openpilot.selfdrive.modeld.models.driving_pyx import (
   PublishState, create_model_msg, create_pose_msg,
   FEATURE_LEN, HISTORY_BUFFER_LEN, DESIRE_LEN, TRAFFIC_CONVENTION_LEN, NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN,
-  OUTPUT_SIZE, NET_OUTPUT_SIZE, MODEL_FREQ)
+  LAT_PLANNER_STATE_LEN, SIM_SPEED_LEN, OUTPUT_SIZE, NET_OUTPUT_SIZE, MODEL_FREQ)
 
 MODEL_PATHS = {
   ModelRunner.THNEED: Path(__file__).parent / 'models/supercombo.thneed',
@@ -48,6 +52,8 @@ class ModelState:
     self.inputs = {
       'desire': np.zeros(DESIRE_LEN * (HISTORY_BUFFER_LEN+1), dtype=np.float32),
       'traffic_convention': np.zeros(TRAFFIC_CONVENTION_LEN, dtype=np.float32),
+      'lat_planner_state': np.zeros(LAT_PLANNER_STATE_LEN, dtype=np.float32),
+      'sim_speed': np.zeros(SIM_SPEED_LEN, dtype=np.float32),
       'nav_features': np.zeros(NAV_FEATURE_LEN, dtype=np.float32),
       'nav_instructions': np.zeros(NAV_INSTRUCTION_LEN, dtype=np.float32),
       'features_buffer': np.zeros(HISTORY_BUFFER_LEN * FEATURE_LEN, dtype=np.float32),
@@ -70,6 +76,7 @@ class ModelState:
     self.inputs['traffic_convention'][:] = inputs['traffic_convention']
     self.inputs['nav_features'][:] = inputs['nav_features']
     self.inputs['nav_instructions'][:] = inputs['nav_instructions']
+    self.inputs['sim_speed'][:] = inputs['sim_speed']
     # self.inputs['driving_style'][:] = inputs['driving_style']
 
     # if getCLBuffer is not None, frame will be None
@@ -82,7 +89,12 @@ class ModelState:
 
     self.model.execute()
     self.inputs['features_buffer'][:-FEATURE_LEN] = self.inputs['features_buffer'][FEATURE_LEN:]
+    # TODO: self.output should be cast to the modeld output struct then these 2 tricky slicings can be removed
     self.inputs['features_buffer'][-FEATURE_LEN:] = self.output[OUTPUT_SIZE:OUTPUT_SIZE+FEATURE_LEN]
+    lat_planning_output = self.output[OUTPUT_SIZE-2*LAT_PLANNER_STATE_LEN*TRAJECTORY_SIZE:OUTPUT_SIZE-LAT_PLANNER_STATE_LEN*TRAJECTORY_SIZE]
+    lat_planning_output = lat_planning_output.reshape((TRAJECTORY_SIZE, LAT_PLANNER_STATE_LEN))
+    self.inputs['lat_planner_state'][2] = interp(DT_MDL, T_IDXS, lat_planning_output[:, 2])
+    self.inputs['lat_planner_state'][3] = interp(DT_MDL, T_IDXS, lat_planning_output[:, 3])
     return self.output
 
 
@@ -120,7 +132,7 @@ def main():
 
   # messaging
   pm = PubMaster(["modelV2", "cameraOdometry"])
-  sm = SubMaster(["lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction"])
+  sm = SubMaster(["lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carState"])
 
   state = PublishState()
   params = Params()
@@ -234,6 +246,7 @@ def main():
     inputs:Dict[str, np.ndarray] = {
       'desire': vec_desire,
       'traffic_convention': traffic_convention,
+      'sim_speed': sm['carState'].vEgo,
       'driving_style': driving_style,
       'nav_features': nav_features,
       'nav_instructions': nav_instructions}

selfdrive/modeld/models/driving.h

@@ -14,6 +14,8 @@ constexpr int HISTORY_BUFFER_LEN = 99;
 constexpr int DESIRE_LEN = 8;
 constexpr int DESIRE_PRED_LEN = 4;
 constexpr int TRAFFIC_CONVENTION_LEN = 2;
+constexpr int LAT_PLANNER_STATE_LEN = 4;
+constexpr int SIM_SPEED_LEN = 1;
 constexpr int NAV_FEATURE_LEN = 256;
 constexpr int NAV_INSTRUCTION_LEN = 150;
 constexpr int DRIVING_STYLE_LEN = 12;

selfdrive/modeld/models/driving.pxd

@@ -13,6 +13,8 @@ cdef extern from "selfdrive/modeld/models/driving.h":
   cdef int HISTORY_BUFFER_LEN
   cdef int DESIRE_LEN
   cdef int TRAFFIC_CONVENTION_LEN
+  cdef int LAT_PLANNER_STATE_LEN
+  cdef int SIM_SPEED_LEN
   cdef int DRIVING_STYLE_LEN
   cdef int NAV_FEATURE_LEN
   cdef int NAV_INSTRUCTION_LEN

selfdrive/modeld/models/driving_pyx.pyx

@@ -11,6 +11,7 @@ from .commonmodel cimport mat3
 from .driving cimport FEATURE_LEN as CPP_FEATURE_LEN, HISTORY_BUFFER_LEN as CPP_HISTORY_BUFFER_LEN, DESIRE_LEN as CPP_DESIRE_LEN, \
                       TRAFFIC_CONVENTION_LEN as CPP_TRAFFIC_CONVENTION_LEN, DRIVING_STYLE_LEN as CPP_DRIVING_STYLE_LEN, \
                       NAV_FEATURE_LEN as CPP_NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN as CPP_NAV_INSTRUCTION_LEN, \
+                      LAT_PLANNER_STATE_LEN as CPP_LAT_PLANNER_STATE_LEN, SIM_SPEED_LEN as CPP_SIM_SPEED_LEN, \
                       OUTPUT_SIZE as CPP_OUTPUT_SIZE, NET_OUTPUT_SIZE as CPP_NET_OUTPUT_SIZE, MODEL_FREQ as CPP_MODEL_FREQ
 from .driving cimport MessageBuilder, PublishState as cppPublishState
 from .driving cimport fill_model_msg, fill_pose_msg
@@ -19,6 +20,8 @@ FEATURE_LEN = CPP_FEATURE_LEN
 HISTORY_BUFFER_LEN = CPP_HISTORY_BUFFER_LEN
 DESIRE_LEN = CPP_DESIRE_LEN
 TRAFFIC_CONVENTION_LEN = CPP_TRAFFIC_CONVENTION_LEN
+LAT_PLANNER_STATE_LEN = CPP_LAT_PLANNER_STATE_LEN
+SIM_SPEED_LEN = CPP_SIM_SPEED_LEN
 DRIVING_STYLE_LEN = CPP_DRIVING_STYLE_LEN
 NAV_FEATURE_LEN = CPP_NAV_FEATURE_LEN
 NAV_INSTRUCTION_LEN = CPP_NAV_INSTRUCTION_LEN

selfdrive/modeld/models/supercombo.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:e448b7d38b9e8536525de3addc62cf945277a743af39261fc19f23bd077f9404
-size 52936875
+oid sha256:4606e4ff971dcd0de771ba4e90ea4857f87508c8a26e4b252a56f9ef2839920a
+size 52939645