From b791d4ed55a963027db9c86391dc9d5dd1f9f9df Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 2 Mar 2025 20:52:44 +0100 Subject: [PATCH] Model: Handle missing 'sim_pose' gracefully in model outputs. (#644) Handle missing 'sim_pose' gracefully in model outputs. Check for the presence of 'sim_pose' in output data before processing. If absent, fallback to using 'plan' data to populate temporal pose fields, ensuring robustness and preventing potential runtime errors. --- selfdrive/modeld/fill_model_msg.py | 15 +++++++++++---- selfdrive/modeld/parse_model_outputs.py | 3 ++- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index f53ed2e1a..753ac89c7 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -112,10 +112,17 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # temporal pose temporal_pose = modelV2.temporalPose - temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist() - temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist() - temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist() - temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist() + if 'sim_pose' in net_output_data: + temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist() + temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist() + temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist() + temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist() + else: + temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist() + temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist() + temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist() + temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist() + # poly path fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T) diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py index 9b162efe8..d3c090649 100644 --- a/selfdrive/modeld/parse_model_outputs.py +++ b/selfdrive/modeld/parse_model_outputs.py @@ -91,7 +91,8 @@ class Parser: self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) - self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) + if 'sim_pose' in outs: + self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,)) self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION, out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))