diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 82c4c92b1..8e63d29ea 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -55,26 +55,29 @@ def fill_lane_line_meta(builder, lane_lines, lane_line_probs): builder.rightY = lane_lines[2].y[0] builder.rightProb = lane_line_probs[2] -def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder, - net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action, +def fill_driving_model_data(msg: capnp._DynamicStructBuilder, modelv2_send: capnp._DynamicStructBuilder) -> None: + msg.valid = modelv2_send.valid + modelV2 = modelv2_send.modelV2 + driving_model_data = msg.drivingModelData + driving_model_data.frameId = modelV2.frameId + driving_model_data.frameIdExtra = modelV2.frameIdExtra + driving_model_data.frameDropPerc = modelV2.frameDropPerc + driving_model_data.modelExecutionTime = modelV2.modelExecutionTime + driving_model_data.action = modelV2.action + driving_model_data.meta.laneChangeState = modelV2.meta.laneChangeState + driving_model_data.meta.laneChangeDirection = modelV2.meta.laneChangeDirection + fill_lane_line_meta(driving_model_data.laneLineMeta, modelV2.laneLines, modelV2.laneLineProbs) + fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, modelV2.position.x, modelV2.position.y, modelV2.position.z) + +def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action, 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 frame_drop_perc = frame_drop * 100 - extended_msg.valid = valid - base_msg.valid = valid + msg.valid = valid - driving_model_data = base_msg.drivingModelData - - driving_model_data.frameId = vipc_frame_id - driving_model_data.frameIdExtra = vipc_frame_id_extra - driving_model_data.frameDropPerc = frame_drop_perc - driving_model_data.modelExecutionTime = model_execution_time - - driving_model_data.action = action - - modelV2 = extended_msg.modelV2 + modelV2 = msg.modelV2 modelV2.frameId = vipc_frame_id modelV2.frameIdExtra = vipc_frame_id_extra modelV2.frameAge = frame_age @@ -89,9 +92,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D fill_xyzt(modelV2.orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T) fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T) - # poly path - fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T) - # action modelV2.action = action @@ -106,8 +106,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist() modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist() - fill_lane_line_meta(driving_model_data.laneLineMeta, modelV2.laneLines, modelV2.laneLineProbs) - # road edges modelV2.init('roadEdges', 2) for i in range(2): diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 49ea0d5cd..cf53eb23d 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -21,7 +21,7 @@ from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value, get_curvature_from_plan from openpilot.selfdrive.modeld.parse_model_outputs import Parser from openpilot.selfdrive.modeld.compile_modeld import make_input_queues, WARP_INPUTS, POLICY_INPUTS -from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState +from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_driving_model_data, fill_pose_msg, PublishState from openpilot.common.file_chunker import read_file_chunked, get_manifest_path from openpilot.selfdrive.modeld.constants import ModelConstants, Plan from openpilot.selfdrive.modeld.helpers import usbgpu_present, modeld_pkl_path, get_tg_input_devices @@ -311,7 +311,7 @@ def main(demo=False): action = get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego) prev_action = action - fill_model_msg(drivingdata_send, modelv2_send, model_output, action, + fill_model_msg(modelv2_send, model_output, action, 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) @@ -322,9 +322,8 @@ 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_driving_model_data(drivingdata_send, modelv2_send) 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)