drivingModelData: new model packet for qlogs (#32821)

* Mini model v2 prototype

* Apply suggestions

* Add meta

* Support for new packet in model

* Support in process replay

* Meta

* Add it to services

* Update model replay

* Add drivingModelData in model_replay

* Update ref

* MAX_FRAMES*3

* No modelv2, but drivingModelData at 2hz

* Polypath

* Dont use keyword

* Update model ref commit

* xyz coeff

* Fix field name

* Update ref commit

* Min qlog size 0.6

---------

Co-authored-by: Comma Device <device@comma.ai>

cereal/log.capnp

@@ -868,6 +868,36 @@ struct ControlsState @0x97ff69c53601abf1 {
   vPidDEPRECATED @2 :Float32;
 }
 
+struct DrivingModelData {
+  frameId @0 :UInt32;
+  frameIdExtra @1 :UInt32;
+
+  action @2 :ModelDataV2.Action;
+
+  laneLineMeta @3 :LaneLineMeta;
+  meta @4 :MetaData;
+
+  path @5 :PolyPath;
+
+  struct PolyPath {
+    xCoefficients @0 :List(Float32);
+    yCoefficients @1 :List(Float32);
+    zCoefficients @2 :List(Float32);
+  }
+
+  struct LaneLineMeta {
+    leftY @0 :Float32;
+    rightY @1 :Float32;
+    leftProb @2 :Float32;
+    rightProb @3 :Float32;
+  }
+
+  struct MetaData {
+    laneChangeState @0 :LaneChangeState;
+    laneChangeDirection @1 :LaneChangeDirection;
+  }
+}
+
 # All SI units and in device frame
 struct XYZTData @0xc3cbae1fd505ae80 {
   x @0 :List(Float32);
@@ -2263,6 +2293,7 @@ struct Event {
     driverMonitoringState @71: DriverMonitoringState;
     liveLocationKalman @72 :LiveLocationKalman;
     modelV2 @75 :ModelDataV2;
+    drivingModelData @128 :DrivingModelData;
     driverStateV2 @92 :DriverStateV2;
 
     # camera stuff, each camera state has a matching encode idx

cereal/services.py

@@ -60,7 +60,8 @@ _services: dict[str, tuple] = {
   "driverMonitoringState": (True, 20., 10),
   "wideRoadEncodeIdx": (False, 20., 1),
   "wideRoadCameraState": (True, 20., 20),
-  "modelV2": (True, 20., 40),
+  "drivingModelData": (True, 20., 10),
+  "modelV2": (True, 20., 0),
   "managerState": (True, 2., 1),
   "uploaderState": (True, 0., 1),
   "navInstruction": (True, 1., 10),

selfdrive/modeld/constants.py

@@ -59,6 +59,8 @@ class ModelConstants:
   RYG_GREEN = 0.01165
   RYG_YELLOW = 0.06157
 
+  POLY_PATH_DEGREE = 4
+
 # model outputs slices
 class Plan:
   POSITION = slice(0, 3)

selfdrive/modeld/fill_model_msg.py

@@ -41,13 +41,30 @@ def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std
   if a_std is not None:
     builder.aStd = a_std.tolist()
 
-def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, np.ndarray], publish_state: PublishState,
+def fill_xyz_poly(builder, degree, x, y, z):
+  xyz = np.stack([x, y, z], axis=1)
+  coeffs = np.polynomial.polynomial.polyfit(ModelConstants.T_IDXS, xyz, deg=degree)
+  builder.xCoefficients = coeffs[:, 0].tolist()
+  builder.yCoefficients = coeffs[:, 1].tolist()
+  builder.zCoefficients = coeffs[:, 2].tolist()
+
+def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder,
+                   net_output_data: dict[str, np.ndarray], publish_state: PublishState,
                    vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float,
                    timestamp_eof: int, model_execution_time: float, valid: bool) -> None:
   frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
-  msg.valid = valid
+  extended_msg.valid = valid
+  base_msg.valid = valid
 
-  modelV2 = msg.modelV2
+  driving_model_data = base_msg.drivingModelData
+
+  driving_model_data.frameId = vipc_frame_id
+  driving_model_data.frameIdExtra = vipc_frame_id_extra
+
+  action = driving_model_data.action
+  action.desiredCurvature = float(net_output_data['desired_curvature'][0,0])
+
+  modelV2 = extended_msg.modelV2
   modelV2.frameId = vipc_frame_id
   modelV2.frameIdExtra = vipc_frame_id_extra
   modelV2.frameAge = frame_age
@@ -67,6 +84,10 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str,
   orientation_rate = modelV2.orientationRate
   fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
 
+  # poly path
+  poly_path = driving_model_data.path
+  fill_xyz_poly(poly_path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
+
   # lateral planning
   action = modelV2.action
   action.desiredCurvature = float(net_output_data['desired_curvature'][0,0])
@@ -98,6 +119,12 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str,
   modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist()
   modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist()
 
+  lane_line_meta = driving_model_data.laneLineMeta
+  lane_line_meta.leftY = modelV2.laneLines[1].y[0]
+  lane_line_meta.leftProb = modelV2.laneLineProbs[1]
+  lane_line_meta.rightY = modelV2.laneLines[2].y[0]
+  lane_line_meta.rightProb = modelV2.laneLineProbs[2]
+
   # road edges
   modelV2.init('roadEdges', 2)
   for i in range(2):

selfdrive/modeld/modeld.py

@@ -149,7 +149,7 @@ 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})")
 
   # messaging
-  pm = PubMaster(["modelV2", "cameraOdometry"])
+  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
   sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"])
 
   publish_state = PublishState()
@@ -259,9 +259,10 @@ def main(demo=False):
 
     if model_output is not None:
       modelv2_send = messaging.new_message('modelV2')
+      drivingdata_send = messaging.new_message('drivingModelData')
       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, model_execution_time, live_calib_seen)
+      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, model_execution_time, live_calib_seen)
 
       desire_state = modelv2_send.modelV2.meta.desireState
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
@@ -270,9 +271,12 @@ def main(demo=False):
       DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
+      drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
+      drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
 
       fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen)
       pm.send('modelV2', modelv2_send)
+      pm.send('drivingModelData', drivingdata_send)
       pm.send('cameraOdometry', posenet_send)
 
     last_vipc_frame_id = meta_main.frame_id

selfdrive/test/process_replay/model_replay.py

@@ -96,10 +96,10 @@ if __name__ == "__main__":
       all_logs = list(LogReader(BASE_URL + log_fn))
       cmp_log = []
 
-      # logs are ordered based on type: modelV2, driverStateV2
+      # logs are ordered based on type: modelV2, drivingModelData, driverStateV2
       if not NO_MODEL:
-        model_start_index = next(i for i, m in enumerate(all_logs) if m.which() in ("modelV2", "cameraOdometry"))
-        cmp_log += all_logs[model_start_index:model_start_index + MAX_FRAMES*2]
+        model_start_index = next(i for i, m in enumerate(all_logs) if m.which() in ("modelV2", "drivingModelData", "cameraOdometry"))
+        cmp_log += all_logs[model_start_index:model_start_index + MAX_FRAMES*3]
         dmon_start_index = next(i for i, m in enumerate(all_logs) if m.which() == "driverStateV2")
         cmp_log += all_logs[dmon_start_index:dmon_start_index + MAX_FRAMES]
 

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-73eb11111fb6407fa307c3f2bdd3331f2514c707
+b0d295609abb0f84c34ffb104c1b1a0bb855352f

selfdrive/test/process_replay/process_replay.py

@@ -565,7 +565,7 @@ CONFIGS = [
   ProcessConfig(
     proc_name="modeld",
     pubs=["deviceState", "roadCameraState", "wideRoadCameraState", "liveCalibration", "driverMonitoringState"],
-    subs=["modelV2", "cameraOdometry"],
+    subs=["modelV2", "drivingModelData", "cameraOdometry"],
     ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime"],
     should_recv_callback=ModeldCameraSyncRcvCallback(),
     tolerance=NUMPY_TOLERANCE,

selfdrive/test/test_onroad.py

@@ -206,7 +206,7 @@ class TestOnroad:
       if f.name == "qcamera.ts":
         assert 2.15 < sz < 2.35
       elif f.name == "qlog":
-        assert 0.7 < sz < 1.0
+        assert 0.6 < sz < 1.0
       elif f.name == "rlog":
         assert 5 < sz < 50
       elif f.name.endswith('.hevc'):