|  |  |  | @ -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 | 
			
		
	
	
		
			
				
					|  |  |  | 
 |