From 8bfce0979d976e410eb0e7d539f03ac81b0ef099 Mon Sep 17 00:00:00 2001 From: YassineYousfi Date: Sun, 26 Jan 2025 10:29:58 -0800 Subject: [PATCH] modeld: use temporalPose from model (#34460) * modeld: use temporalPose from model * cleanup --- selfdrive/modeld/constants.py | 1 - selfdrive/modeld/fill_model_msg.py | 8 ++++---- selfdrive/modeld/parse_model_outputs.py | 1 + 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index beb0d22ef5b051..8c2f6b3450b697 100644 --- a/selfdrive/modeld/constants.py +++ b/selfdrive/modeld/constants.py @@ -31,7 +31,6 @@ class ModelConstants: DISENGAGE_WIDTH = 5 POSE_WIDTH = 6 WIDE_FROM_DEVICE_WIDTH = 3 - SIM_POSE_WIDTH = 6 LEAD_WIDTH = 4 LANE_LINES_WIDTH = 2 ROAD_EDGES_WIDTH = 2 diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 00176c5d37aaa0..59b7508ed546ff 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -90,10 +90,10 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # temporal pose temporal_pose = modelV2.temporalPose - 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() + 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() # 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 4367e9db8a2bcd..9b162efe897a5e 100644 --- a/selfdrive/modeld/parse_model_outputs.py +++ b/selfdrive/modeld/parse_model_outputs.py @@ -91,6 +91,7 @@ def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: 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,)) 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))