@ -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 ) ;
}
}