From 205c2b63aaf95c2002710ae76a45619eee17a3c8 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Mon, 20 Jan 2025 08:21:02 +0800 Subject: [PATCH] modeld: simplify model message construction by removing redundant variables (#34354) Simplify model message construction by removing redundant variables --- selfdrive/modeld/fill_model_msg.py | 28 +++++++++------------------- 1 file changed, 9 insertions(+), 19 deletions(-) diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 3056da9374..00176c5d37 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -71,9 +71,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D driving_model_data.frameIdExtra = vipc_frame_id_extra driving_model_data.frameDropPerc = frame_drop_perc driving_model_data.modelExecutionTime = model_execution_time - - action = driving_model_data.action - action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) + driving_model_data.action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) modelV2 = extended_msg.modelV2 modelV2.frameId = vipc_frame_id @@ -84,16 +82,11 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D modelV2.modelExecutionTime = model_execution_time # plan - position = modelV2.position - fill_xyzt(position, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.POSITION].T, *net_output_data['plan_stds'][0,:,Plan.POSITION].T) - velocity = modelV2.velocity - fill_xyzt(velocity, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.VELOCITY].T) - acceleration = modelV2.acceleration - fill_xyzt(acceleration, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ACCELERATION].T) - orientation = modelV2.orientation - fill_xyzt(orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T) - orientation_rate = modelV2.orientationRate - fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T) + fill_xyzt(modelV2.position, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.POSITION].T, *net_output_data['plan_stds'][0,:,Plan.POSITION].T) + fill_xyzt(modelV2.velocity, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.VELOCITY].T) + fill_xyzt(modelV2.acceleration, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ACCELERATION].T) + 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) # temporal pose temporal_pose = modelV2.temporalPose @@ -103,12 +96,10 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist() # 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) + fill_xyz_poly(driving_model_data.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]) + modelV2.action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) # times at X_IDXS according to model plan PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N @@ -137,8 +128,7 @@ 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() - lane_line_meta = driving_model_data.laneLineMeta - fill_lane_line_meta(lane_line_meta, modelV2.laneLines, modelV2.laneLineProbs) + fill_lane_line_meta(driving_model_data.laneLineMeta, modelV2.laneLines, modelV2.laneLineProbs) # road edges modelV2.init('roadEdges', 2)