diff --git a/cereal/log.capnp b/cereal/log.capnp index 3c720d4526..20b033196e 100644 --- a/cereal/log.capnp +++ b/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 diff --git a/cereal/services.py b/cereal/services.py index ca9139c431..5329719c2a 100755 --- a/cereal/services.py +++ b/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), diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index ca81bffcc8..871685797a 100644 --- a/selfdrive/modeld/constants.py +++ b/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) diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index f6a57f1a40..2f719c1ee6 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/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): diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 1e33b7b3f0..d5812d6f5a 100755 --- a/selfdrive/modeld/modeld.py +++ b/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 diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index 28826144e8..9a7b9695ff 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/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] diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 87f1adfada..995e024de0 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -73eb11111fb6407fa307c3f2bdd3331f2514c707 +b0d295609abb0f84c34ffb104c1b1a0bb855352f \ No newline at end of file diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index a55396002b..c7f57fd9bc 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/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, diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 7574a5e4ab..78486a176d 100644 --- a/selfdrive/test/test_onroad.py +++ b/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'):