|  |  | @ -234,28 +234,19 @@ void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  | void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, |  |  |  | void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, | 
			
		
	
		
		
			
				
					
					|  |  |  |                    uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { |  |  |  |                    uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { | 
			
		
	
		
		
			
				
					
					|  |  |  |   // make msg
 |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   capnp::MallocMessageBuilder msg; |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   event.setLogMonoTime(nanos_since_boot()); |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; |  |  |  |   uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  |   auto framed = event.initModel(); |  |  |  |   MessageBuilder msg; | 
			
				
				
			
		
	
		
		
	
		
		
			
				
					
					|  |  |  |  |  |  |  |   auto framed = msg.initEvent(frame_drop < MAX_FRAME_DROP).initModel(); | 
			
		
	
		
		
			
				
					
					|  |  |  |   framed.setFrameId(vipc_frame_id); |  |  |  |   framed.setFrameId(vipc_frame_id); | 
			
		
	
		
		
			
				
					
					|  |  |  |   framed.setFrameAge(frame_age); |  |  |  |   framed.setFrameAge(frame_age); | 
			
		
	
		
		
			
				
					
					|  |  |  |   framed.setFrameDropPerc(frame_drop * 100); |  |  |  |   framed.setFrameDropPerc(frame_drop * 100); | 
			
		
	
		
		
			
				
					
					|  |  |  |   framed.setTimestampEof(timestamp_eof); |  |  |  |   framed.setTimestampEof(timestamp_eof); | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  |   auto lpath = framed.initPath(); |  |  |  |   fill_path(framed.initPath(), net_outputs.path, false, 0); | 
			
				
				
			
		
	
		
		
			
				
					
					|  |  |  |   fill_path(lpath, net_outputs.path, false, 0); |  |  |  |   fill_path(framed.initLeftLane(), net_outputs.left_lane, true, 1.8); | 
			
				
				
			
		
	
		
		
			
				
					
					|  |  |  |   auto left_lane = framed.initLeftLane(); |  |  |  |   fill_path(framed.initRightLane(), net_outputs.right_lane, true, -1.8); | 
			
				
				
			
		
	
		
		
			
				
					
					|  |  |  |   fill_path(left_lane, net_outputs.left_lane, true, 1.8); |  |  |  |   fill_longi(framed.initLongitudinal(), net_outputs.long_x, net_outputs.long_v, net_outputs.long_a); | 
			
				
				
			
		
	
		
		
			
				
					
					|  |  |  |   auto right_lane = framed.initRightLane(); |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   fill_path(right_lane, net_outputs.right_lane, true, -1.8); |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   auto longi = framed.initLongitudinal(); |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   fill_longi(longi, net_outputs.long_x, net_outputs.long_v, net_outputs.long_a); |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  |  | 
			
		
	
		
		
	
		
		
	
		
		
	
		
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  |   // Find the distribution that corresponds to the current lead
 |  |  |  |   // Find the distribution that corresponds to the current lead
 | 
			
		
	
		
		
			
				
					
					|  |  |  |   int mdn_max_idx = 0; |  |  |  |   int mdn_max_idx = 0; | 
			
		
	
	
		
		
			
				
					|  |  | @ -265,8 +256,7 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, | 
			
		
	
		
		
			
				
					
					|  |  |  |       mdn_max_idx = i; |  |  |  |       mdn_max_idx = i; | 
			
		
	
		
		
			
				
					
					|  |  |  |     } |  |  |  |     } | 
			
		
	
		
		
			
				
					
					|  |  |  |   } |  |  |  |   } | 
			
		
	
		
		
			
				
					
					|  |  |  |   auto lead = framed.initLead(); |  |  |  |   fill_lead(framed.initLead(), net_outputs.lead, mdn_max_idx, t_offset); | 
			
				
				
			
		
	
		
		
			
				
					
					|  |  |  |   fill_lead(lead, net_outputs.lead, mdn_max_idx, t_offset); |  |  |  |  | 
			
		
	
		
		
	
		
		
			
				
					
					|  |  |  |   // Find the distribution that corresponds to the lead in 2s
 |  |  |  |   // Find the distribution that corresponds to the lead in 2s
 | 
			
		
	
		
		
			
				
					
					|  |  |  |   mdn_max_idx = 0; |  |  |  |   mdn_max_idx = 0; | 
			
		
	
		
		
			
				
					
					|  |  |  |   t_offset = 1; |  |  |  |   t_offset = 1; | 
			
		
	
	
		
		
			
				
					|  |  | @ -275,23 +265,14 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, | 
			
		
	
		
		
			
				
					
					|  |  |  |       mdn_max_idx = i; |  |  |  |       mdn_max_idx = i; | 
			
		
	
		
		
			
				
					
					|  |  |  |     } |  |  |  |     } | 
			
		
	
		
		
			
				
					
					|  |  |  |   } |  |  |  |   } | 
			
		
	
		
		
			
				
					
					|  |  |  |   auto lead_future = framed.initLeadFuture(); |  |  |  |   fill_lead(framed.initLeadFuture(), net_outputs.lead, mdn_max_idx, t_offset); | 
			
				
				
			
		
	
		
		
			
				
					
					|  |  |  |   fill_lead(lead_future, net_outputs.lead, mdn_max_idx, t_offset); |  |  |  |   fill_meta(framed.initMeta(), net_outputs.meta); | 
			
				
				
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   auto meta = framed.initMeta(); |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   fill_meta(meta, net_outputs.meta); |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   event.setValid(frame_drop < MAX_FRAME_DROP); |  |  |  |  | 
			
		
	
		
		
	
		
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  |   pm.send("model", msg); |  |  |  |   pm.send("model", msg); | 
			
		
	
		
		
			
				
					
					|  |  |  | } |  |  |  | } | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  | void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, |  |  |  | void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, | 
			
		
	
		
		
			
				
					
					|  |  |  |                      uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { |  |  |  |                      uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { | 
			
		
	
		
		
			
				
					
					|  |  |  |   capnp::MallocMessageBuilder msg; |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   event.setLogMonoTime(nanos_since_boot()); |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   float trans_arr[3]; |  |  |  |   float trans_arr[3]; | 
			
		
	
		
		
			
				
					
					|  |  |  |   float trans_std_arr[3]; |  |  |  |   float trans_std_arr[3]; | 
			
		
	
		
		
			
				
					
					|  |  |  |   float rot_arr[3]; |  |  |  |   float rot_arr[3]; | 
			
		
	
	
		
		
			
				
					|  |  | @ -305,7 +286,8 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, | 
			
		
	
		
		
			
				
					
					|  |  |  |     rot_std_arr[i] = M_PI * (softplus(net_outputs.pose[9 + i]) + 1e-6) / 180.0; |  |  |  |     rot_std_arr[i] = M_PI * (softplus(net_outputs.pose[9 + i]) + 1e-6) / 180.0; | 
			
		
	
		
		
			
				
					
					|  |  |  |   } |  |  |  |   } | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  |   auto posenetd = event.initCameraOdometry(); |  |  |  |   MessageBuilder msg; | 
			
				
				
			
		
	
		
		
	
		
		
			
				
					
					|  |  |  |  |  |  |  |   auto posenetd = msg.initEvent(vipc_dropped_frames < 1).initCameraOdometry(); | 
			
		
	
		
		
			
				
					
					|  |  |  |   kj::ArrayPtr<const float> trans_vs(&trans_arr[0], 3); |  |  |  |   kj::ArrayPtr<const float> trans_vs(&trans_arr[0], 3); | 
			
		
	
		
		
			
				
					
					|  |  |  |   posenetd.setTrans(trans_vs); |  |  |  |   posenetd.setTrans(trans_vs); | 
			
		
	
		
		
			
				
					
					|  |  |  |   kj::ArrayPtr<const float> rot_vs(&rot_arr[0], 3); |  |  |  |   kj::ArrayPtr<const float> rot_vs(&rot_arr[0], 3); | 
			
		
	
	
		
		
			
				
					|  |  | @ -315,11 +297,8 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, | 
			
		
	
		
		
			
				
					
					|  |  |  |   kj::ArrayPtr<const float> rot_std_vs(&rot_std_arr[0], 3); |  |  |  |   kj::ArrayPtr<const float> rot_std_vs(&rot_std_arr[0], 3); | 
			
		
	
		
		
			
				
					
					|  |  |  |   posenetd.setRotStd(rot_std_vs); |  |  |  |   posenetd.setRotStd(rot_std_vs); | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   posenetd.setTimestampEof(timestamp_eof); |  |  |  |   posenetd.setTimestampEof(timestamp_eof); | 
			
		
	
		
		
			
				
					
					|  |  |  |   posenetd.setFrameId(vipc_frame_id); |  |  |  |   posenetd.setFrameId(vipc_frame_id); | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  |   event.setValid(vipc_dropped_frames < 1); |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  |  | 
			
		
	
		
		
			
				
					
					|  |  |  |   pm.send("cameraOdometry", msg); |  |  |  |   pm.send("cameraOdometry", msg); | 
			
		
	
		
		
			
				
					
					|  |  |  | } |  |  |  | } | 
			
		
	
	
		
		
			
				
					|  |  | 
 |