mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-30 19:33:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Clean up model_publish args to simplify cython bindings (#29203)

Browse Source 
* Clean up model_publish args to simplify cython bindings

* pass by reference

* Move FCW and model confidence queues into PublishState
old-commit-hash: 663fc0d8fe7cb203259fceb5b8aef9fde270b9cb
This commit is contained in:
Mitchell Goff
2023-08-01 19:23:18 -07:00
committed by GitHub
parent e29eebdc51
commit 181e9775f7
3 changed files with 34 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/modeld/modeld.cc
+2 -1
View File
@@ -63,6 +63,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel"}); SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel"});
Params params; Params params;
PublishState ps = {};
// setup filter to track dropped frames // setup filter to track dropped frames
FirstOrderFilter frame_dropped_filter(0., 10., 1. / MODEL_FREQ); FirstOrderFilter frame_dropped_filter(0., 10., 1. / MODEL_FREQ);
@@ -179,7 +180,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
float model_execution_time = (mt2 - mt1) / 1000.0; float model_execution_time = (mt2 - mt1) / 1000.0;
if (model_output != nullptr) { if (model_output != nullptr) {
model_publish(&model, pm, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, *model_output, meta_main.timestamp_eof, timestamp_llk, model_execution_time, model_publish(pm, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, *model_output, model, ps, meta_main.timestamp_eof, timestamp_llk, model_execution_time,
nav_enabled, live_calib_seen); nav_enabled, live_calib_seen);
posenet_publish(pm, meta_main.frame_id, vipc_dropped_frames, *model_output, meta_main.timestamp_eof, live_calib_seen); posenet_publish(pm, meta_main.frame_id, vipc_dropped_frames, *model_output, meta_main.timestamp_eof, live_calib_seen);
} }
selfdrive/modeld/models/driving.cc
+20 -26
View File
@@ -13,12 +13,6 @@
#include "common/timing.h" #include "common/timing.h"
#include "common/swaglog.h" #include "common/swaglog.h"
constexpr float FCW_THRESHOLD_5MS2_HIGH = 0.15;
constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05;
constexpr float FCW_THRESHOLD_3MS2 = 0.7;
std::array<float, 5> prev_brake_5ms2_probs = {0,0,0,0,0};
std::array<float, 3> prev_brake_3ms2_probs = {0,0,0};
// #define DUMP_YUV // #define DUMP_YUV
@@ -152,7 +146,7 @@ void fill_lead(cereal::ModelDataV2::LeadDataV3::Builder lead, const ModelOutputL
lead.setAStd(to_kj_array_ptr(lead_a_std)); lead.setAStd(to_kj_array_ptr(lead_a_std));
} }
void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const ModelOutputMeta &meta_data) { void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const ModelOutputMeta &meta_data, PublishState &ps) {
std::array<float, DESIRE_LEN> desire_state_softmax; std::array<float, DESIRE_LEN> desire_state_softmax;
softmax(meta_data.desire_state_prob.array.data(), desire_state_softmax.data(), DESIRE_LEN); softmax(meta_data.desire_state_prob.array.data(), desire_state_softmax.data(), DESIRE_LEN);
@@ -174,18 +168,18 @@ void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const ModelOutputMet
//gas_pressed_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].gas_pressed); //gas_pressed_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].gas_pressed);
} }
std::memmove(prev_brake_5ms2_probs.data(), &prev_brake_5ms2_probs[1], 4*sizeof(float)); std::memmove(ps.prev_brake_5ms2_probs.data(), &ps.prev_brake_5ms2_probs[1], 4*sizeof(float));
std::memmove(prev_brake_3ms2_probs.data(), &prev_brake_3ms2_probs[1], 2*sizeof(float)); std::memmove(ps.prev_brake_3ms2_probs.data(), &ps.prev_brake_3ms2_probs[1], 2*sizeof(float));
prev_brake_5ms2_probs[4] = brake_5ms2_sigmoid[0]; ps.prev_brake_5ms2_probs[4] = brake_5ms2_sigmoid[0];
prev_brake_3ms2_probs[2] = brake_3ms2_sigmoid[0]; ps.prev_brake_3ms2_probs[2] = brake_3ms2_sigmoid[0];
bool above_fcw_threshold = true; bool above_fcw_threshold = true;
for (int i=0; i<prev_brake_5ms2_probs.size(); i++) { for (int i=0; i<ps.prev_brake_5ms2_probs.size(); i++) {
float threshold = i < 2 ? FCW_THRESHOLD_5MS2_LOW : FCW_THRESHOLD_5MS2_HIGH; float threshold = i < 2 ? FCW_THRESHOLD_5MS2_LOW : FCW_THRESHOLD_5MS2_HIGH;
above_fcw_threshold = above_fcw_threshold && prev_brake_5ms2_probs[i] > threshold; above_fcw_threshold = above_fcw_threshold && ps.prev_brake_5ms2_probs[i] > threshold;
} }
for (int i=0; i<prev_brake_3ms2_probs.size(); i++) { for (int i=0; i<ps.prev_brake_3ms2_probs.size(); i++) {
above_fcw_threshold = above_fcw_threshold && prev_brake_3ms2_probs[i] > FCW_THRESHOLD_3MS2; above_fcw_threshold = above_fcw_threshold && ps.prev_brake_3ms2_probs[i] > FCW_THRESHOLD_3MS2;
} }
auto disengage = meta.initDisengagePredictions(); auto disengage = meta.initDisengagePredictions();
@@ -203,7 +197,7 @@ void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const ModelOutputMet
meta.setHardBrakePredicted(above_fcw_threshold); meta.setHardBrakePredicted(above_fcw_threshold);
} }
void fill_confidence(ModelState* s, cereal::ModelDataV2::Builder &framed) { void fill_confidence(cereal::ModelDataV2::Builder &framed, PublishState &ps) {
if (framed.getFrameId() % (2*MODEL_FREQ) == 0) { if (framed.getFrameId() % (2*MODEL_FREQ) == 0) {
// update every 2s to match predictions interval // update every 2s to match predictions interval
auto dbps = framed.getMeta().getDisengagePredictions().getBrakeDisengageProbs(); auto dbps = framed.getMeta().getDisengagePredictions().getBrakeDisengageProbs();
@@ -223,13 +217,13 @@ void fill_confidence(ModelState* s, cereal::ModelDataV2::Builder &framed) {
} }
// rolling buf for 2, 4, 6, 8, 10s // rolling buf for 2, 4, 6, 8, 10s
std::memmove(&s->disengage_buffer[0], &s->disengage_buffer[DISENGAGE_LEN], sizeof(float) * DISENGAGE_LEN * (DISENGAGE_LEN-1)); std::memmove(&ps.disengage_buffer[0], &ps.disengage_buffer[DISENGAGE_LEN], sizeof(float) * DISENGAGE_LEN * (DISENGAGE_LEN-1));
std::memcpy(&s->disengage_buffer[DISENGAGE_LEN * (DISENGAGE_LEN-1)], &dp_ind[0], sizeof(float) * DISENGAGE_LEN); std::memcpy(&ps.disengage_buffer[DISENGAGE_LEN * (DISENGAGE_LEN-1)], &dp_ind[0], sizeof(float) * DISENGAGE_LEN);
} }
float score = 0; float score = 0;
for (int i = 0; i < DISENGAGE_LEN; i++) { for (int i = 0; i < DISENGAGE_LEN; i++) {
score += s->disengage_buffer[i*DISENGAGE_LEN+DISENGAGE_LEN-1-i] / DISENGAGE_LEN; score += ps.disengage_buffer[i*DISENGAGE_LEN+DISENGAGE_LEN-1-i] / DISENGAGE_LEN;
} }
if (score < RYG_GREEN) { if (score < RYG_GREEN) {
@@ -355,7 +349,7 @@ void fill_road_edges(cereal::ModelDataV2::Builder &framed, const std::array<floa
}); });
} }
void fill_model(ModelState* s, cereal::ModelDataV2::Builder &framed, const ModelOutput &net_outputs) { void fill_model(cereal::ModelDataV2::Builder &framed, const ModelOutput &net_outputs, PublishState &ps) {
const auto &best_plan = net_outputs.plans.get_best_prediction(); const auto &best_plan = net_outputs.plans.get_best_prediction();
std::array<float, TRAJECTORY_SIZE> plan_t; std::array<float, TRAJECTORY_SIZE> plan_t;
std::fill_n(plan_t.data(), plan_t.size(), NAN); std::fill_n(plan_t.data(), plan_t.size(), NAN);
@@ -383,10 +377,10 @@ void fill_model(ModelState* s, cereal::ModelDataV2::Builder &framed, const Model
fill_road_edges(framed, plan_t, net_outputs.road_edges); fill_road_edges(framed, plan_t, net_outputs.road_edges);
// meta // meta
fill_meta(framed.initMeta(), net_outputs.meta); fill_meta(framed.initMeta(), net_outputs.meta, ps);
// confidence // confidence
fill_confidence(s, framed); fill_confidence(framed, ps);
// leads // leads
auto leads = framed.initLeadsV3(LEAD_MHP_SELECTION); auto leads = framed.initLeadsV3(LEAD_MHP_SELECTION);
@@ -407,8 +401,8 @@ void fill_model(ModelState* s, cereal::ModelDataV2::Builder &framed, const Model
temporal_pose.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)}); temporal_pose.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)});
} }
void model_publish(ModelState* s, PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop, void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
const ModelOutput &net_outputs, uint64_t timestamp_eof, uint64_t timestamp_llk, const ModelOutput &net_outputs, ModelState &s, PublishState &ps, uint64_t timestamp_eof, uint64_t timestamp_llk,
float model_execution_time, const bool nav_enabled, const bool valid) { float model_execution_time, const bool nav_enabled, const bool valid) {
const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
MessageBuilder msg; MessageBuilder msg;
@@ -422,9 +416,9 @@ void model_publish(ModelState* s, PubMaster &pm, uint32_t vipc_frame_id, uint32_
framed.setModelExecutionTime(model_execution_time); framed.setModelExecutionTime(model_execution_time);
framed.setNavEnabled(nav_enabled); framed.setNavEnabled(nav_enabled);
if (send_raw_pred) { if (send_raw_pred) {
framed.setRawPredictions((kj::ArrayPtr<const float>(s->output.data(), s->output.size())).asBytes()); framed.setRawPredictions((kj::ArrayPtr<const float>(s.output.data(), s.output.size())).asBytes());
} }
fill_model(s, framed, net_outputs); fill_model(framed, net_outputs, ps);
pm.send("modelV2", msg); pm.send("modelV2", msg);
} }
selfdrive/modeld/models/driving.h
+12 -3
View File
@@ -39,6 +39,10 @@ constexpr int LEAD_MHP_SELECTION = 3;
// Padding to get output shape as multiple of 4 // Padding to get output shape as multiple of 4
constexpr int PAD_SIZE = 2; constexpr int PAD_SIZE = 2;
constexpr float FCW_THRESHOLD_5MS2_HIGH = 0.15;
constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05;
constexpr float FCW_THRESHOLD_3MS2 = 0.7;
struct ModelOutputXYZ { struct ModelOutputXYZ {
float x; float x;
float y; float y;
@@ -262,7 +266,6 @@ struct ModelState {
ModelFrame *frame = nullptr; ModelFrame *frame = nullptr;
ModelFrame *wide_frame = nullptr; ModelFrame *wide_frame = nullptr;
std::array<float, HISTORY_BUFFER_LEN * FEATURE_LEN> feature_buffer = {}; std::array<float, HISTORY_BUFFER_LEN * FEATURE_LEN> feature_buffer = {};
std::array<float, DISENGAGE_LEN * DISENGAGE_LEN> disengage_buffer = {};
std::array<float, NET_OUTPUT_SIZE> output = {}; std::array<float, NET_OUTPUT_SIZE> output = {};
std::unique_ptr<RunModel> m; std::unique_ptr<RunModel> m;
#ifdef DESIRE #ifdef DESIRE
@@ -280,12 +283,18 @@ struct ModelState {
#endif #endif
}; };
struct PublishState {
std::array<float, DISENGAGE_LEN * DISENGAGE_LEN> disengage_buffer = {};
std::array<float, 5> prev_brake_5ms2_probs = {};
std::array<float, 3> prev_brake_3ms2_probs = {};
};
void model_init(ModelState* s, cl_device_id device_id, cl_context context); void model_init(ModelState* s, cl_device_id device_id, cl_context context);
ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide, ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide,
const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only); const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only);
void model_free(ModelState* s); void model_free(ModelState* s);
void model_publish(ModelState* s, PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop, void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
const ModelOutput &net_outputs, uint64_t timestamp_eof, uint64_t timestamp_llk, const ModelOutput &net_outputs, ModelState &s, PublishState &ps, uint64_t timestamp_eof, uint64_t timestamp_llk,
float model_execution_time, const bool nav_enabled, const bool valid); float model_execution_time, const bool nav_enabled, const bool valid);
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
const ModelOutput &net_outputs, uint64_t timestamp_eof, const bool valid); const ModelOutput &net_outputs, uint64_t timestamp_eof, const bool valid);