diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 0ea88f9bc1..9dc2641c9b 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -101,30 +101,14 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # lateral planning 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 - PLAN_T_IDXS[0] = 0.0 - plan_x = net_output_data['plan'][0,:,Plan.POSITION][:,0].tolist() - for xidx in range(1, ModelConstants.IDX_N): - tidx = 0 - # increment tidx until we find an element that's further away than the current xidx - while tidx < ModelConstants.IDX_N - 1 and plan_x[tidx+1] < ModelConstants.X_IDXS[xidx]: - tidx += 1 - if tidx == ModelConstants.IDX_N - 1: - # if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break - PLAN_T_IDXS[xidx] = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1] - break - # interpolate to find `t` for the current xidx - current_x_val = plan_x[tidx] - next_x_val = plan_x[tidx+1] - p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs(next_x_val - current_x_val) > 1e-9 else float('nan') - PLAN_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx+1] + (1 - p) * ModelConstants.T_IDXS[tidx] + # times at X_IDXS of edges and lines aren't used + LINE_T_IDXS: list[float] = [] # lane lines modelV2.init('laneLines', 4) for i in range(4): lane_line = modelV2.laneLines[i] - fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1]) + fill_xyzt(lane_line, LINE_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1]) modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist() modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist() @@ -134,7 +118,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D modelV2.init('roadEdges', 2) for i in range(2): road_edge = modelV2.roadEdges[i] - fill_xyzt(road_edge, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['road_edges'][0,i,:,0], net_output_data['road_edges'][0,i,:,1]) + fill_xyzt(road_edge, LINE_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['road_edges'][0,i,:,0], net_output_data['road_edges'][0,i,:,1]) modelV2.roadEdgeStds = net_output_data['road_edges_stds'][0,:,0,0].tolist() # leads