From f30d15ea24c3d122b55c2789dc996725f13d0bd6 Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Wed, 2 Dec 2020 18:46:12 -0800 Subject: [PATCH] raw pred api (#2611) * send both * update cereal * ptr type * this is fine * clean up * v1 and v2 * header * update cereal * cereal --- selfdrive/modeld/dmonitoringmodeld.cc | 3 ++- selfdrive/modeld/modeld.cc | 5 +++-- selfdrive/modeld/models/commonmodel.h | 3 +++ selfdrive/modeld/models/dmonitoring.cc | 5 ++++- selfdrive/modeld/models/dmonitoring.h | 2 +- selfdrive/modeld/models/driving.cc | 10 ++++++++-- selfdrive/modeld/models/driving.h | 4 ++-- 7 files changed, 23 insertions(+), 9 deletions(-) diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc index 3721fd4b9b..80af643d0a 100644 --- a/selfdrive/modeld/dmonitoringmodeld.cc +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -62,7 +62,8 @@ int main(int argc, char **argv) { double t2 = millis_since_boot(); // send dm packet - dmonitoring_publish(pm, extra.frame_id, res, (t2-t1)/1000.0); + const float* raw_pred_ptr = send_raw_pred ? (const float *)dmonitoringmodel.output : nullptr; + dmonitoring_publish(pm, extra.frame_id, res, raw_pred_ptr, (t2-t1)/1000.0); LOGD("dmonitoring process: %.2fms, from last %.2fms", t2-t1, t1-last); last = t1; diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 14561b5448..992473bbc6 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -201,8 +201,9 @@ int main(int argc, char **argv) { if (run_count < 10) frames_dropped = 0; // let frame drops warm up float frame_drop_ratio = frames_dropped / (1 + frames_dropped); - model_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time); - model_publish_v2(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time); + const float* raw_pred_ptr = send_raw_pred ? (const float *)model.output : nullptr; + model_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, raw_pred_ptr, extra.timestamp_eof, model_execution_time); + model_publish_v2(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, raw_pred_ptr, extra.timestamp_eof, model_execution_time); posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof); LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_ratio); diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 06d46b8f2f..dd3bf7a7fa 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -9,6 +9,7 @@ #endif #include +#include #include "common/mat.h" #include "transforms/transform.h" #include "transforms/loadyuv.h" @@ -17,6 +18,8 @@ extern "C" { #endif +const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL; + void softmax(const float* input, float* output, size_t len); float softplus(float input); float sigmoid(float input); diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index 0ce6565e54..9dc49d2828 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -181,7 +181,7 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ return ret; } -void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time){ +void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, const float* raw_pred, float execution_time){ // make msg MessageBuilder msg; auto framed = msg.initEvent().initDriverState(); @@ -198,6 +198,9 @@ void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResu framed.setLeftBlinkProb(res.left_blink_prob); framed.setRightBlinkProb(res.right_blink_prob); framed.setSgProb(res.sg_prob); + if (send_raw_pred) { + framed.setRawPred(kj::arrayPtr((const uint8_t*)raw_pred, OUTPUT_SIZE*sizeof(float))); + } pm.send("driverState", msg); } diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h index 7360af817c..8e9df273bf 100644 --- a/selfdrive/modeld/models/dmonitoring.h +++ b/selfdrive/modeld/models/dmonitoring.h @@ -37,7 +37,7 @@ typedef struct DMonitoringModelState { void dmonitoring_init(DMonitoringModelState* s); DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height); -void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time); +void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, const float* raw_pred, float execution_time); void dmonitoring_free(DMonitoringModelState* s); #ifdef __cplusplus diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 1067d69710..888a613f02 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -295,7 +295,7 @@ void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data, void model_publish_v2(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, + const ModelDataRaw &net_outputs, const float* raw_pred, uint64_t timestamp_eof, float model_execution_time) { // make msg MessageBuilder msg; @@ -306,6 +306,9 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, framed.setFrameDropPerc(frame_drop * 100); framed.setTimestampEof(timestamp_eof); framed.setModelExecutionTime(model_execution_time); + if (send_raw_pred) { + framed.setRawPred(kj::arrayPtr((const uint8_t*)raw_pred, (OUTPUT_SIZE+TEMPORAL_SIZE)*sizeof(float))); + } // plan int plan_mhp_max_idx = 0; @@ -371,7 +374,7 @@ void model_publish_v2(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, + const ModelDataRaw &net_outputs, const float* raw_pred, uint64_t timestamp_eof, float model_execution_time) { uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; @@ -382,6 +385,9 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, framed.setFrameDropPerc(frame_drop * 100); framed.setTimestampEof(timestamp_eof); framed.setModelExecutionTime(model_execution_time); + if (send_raw_pred) { + framed.setRawPred(kj::arrayPtr((const uint8_t*)raw_pred, (OUTPUT_SIZE+TEMPORAL_SIZE)*sizeof(float))); + } // Find the distribution that corresponds to the most probable plan int plan_mhp_max_idx = 0; diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 93177105e8..1f12e5e377 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -77,10 +77,10 @@ void model_free(ModelState* s); void poly_fit(float *in_pts, float *in_stds, float *out); void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, + uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, const float* raw_pred, uint64_t timestamp_eof, float model_execution_time); void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, - uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, + uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, const float* raw_pred, uint64_t timestamp_eof, float model_execution_time); void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data,