sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

New model outputs (#20933)

Browse Source 
* New model: 51a237cd-9c43-4281-9072-802325e6744d/950

* Added new meta outputs and proper interp for plan_t_arr

* Added meta-based trigger for FCW

* No nans in planner, separate alert for model FCW

* Cleaned up plan_t interp logic

* Added empty cameraError alert to fix unit tests

* Use the normal FCW event

* Updated filter logic

* Updated model replay ref

* Moved disengage predictions into their own cereal struct

* Updated model replay ref for new cereal structs

* Apparently this breaks stuff?

* Bumped cereal commit and model replay ref
pull/20967/head
Mitchell Goff 4 years ago committed by GitHub
parent 2defc730db
commit 29cc3500a5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 90 additions and 20 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      cereal
  2. 4
      models/supercombo.dlc
  3. 4
      models/supercombo.onnx
  4. 6
      selfdrive/common/modeldata.h
  5. 2
      selfdrive/controls/controlsd.py
  6. 90
      selfdrive/modeld/models/driving.cc
  7. 2
      selfdrive/test/process_replay/model_replay_ref_commit

2
cereal

@ -1 +1 @@
Subproject commit 7a3b02dabe9bc0193e741d97ccfeed3cb0953d56
Subproject commit 6fe062eca0c576cd0629cc00f4df0300abdcf5db

4
models/supercombo.dlc
Unescape View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:059efa4e4c5f1430271362147e4b2eda650dc7ca9bad6a63725cc616afdf7f77
size 63447299
oid sha256:dc46a24d4b4afa9730785264834e7f7c04c84b6a28a689acb503d6663818c256
size 58797567

4
models/supercombo.onnx
Unescape View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:f3c6f00e1e847a285fc0b834f610f4064d682d3e9f0878ba11fccb4e43692b4c
size 63313286
oid sha256:77a31e5a3a70c39a3fcb07e939a668baf80b1eb778fe792fbe256de983fab5cd
size 58823636

6
selfdrive/common/modeldata.h
Unescape View File

@ -3,14 +3,16 @@ const int TRAJECTORY_SIZE = 33;
const float MIN_DRAW_DISTANCE = 10.0;
const float MAX_DRAW_DISTANCE = 100.0;
const double T_IDXS[TRAJECTORY_SIZE] = {0. , 0.00976562, 0.0390625 , 0.08789062, 0.15625 ,
const double T_IDXS[TRAJECTORY_SIZE] = {
0. , 0.00976562, 0.0390625 , 0.08789062, 0.15625 ,
0.24414062, 0.3515625 , 0.47851562, 0.625 , 0.79101562,
0.9765625 , 1.18164062, 1.40625 , 1.65039062, 1.9140625 ,
2.19726562, 2.5 , 2.82226562, 3.1640625 , 3.52539062,
3.90625 , 4.30664062, 4.7265625 , 5.16601562, 5.625 ,
6.10351562, 6.6015625 , 7.11914062, 7.65625 , 8.21289062,
8.7890625 , 9.38476562, 10.};
const double X_IDXS[TRAJECTORY_SIZE] = { 0. , 0.1875, 0.75 , 1.6875, 3. , 4.6875,
const double X_IDXS[TRAJECTORY_SIZE] = {
0. , 0.1875, 0.75 , 1.6875, 3. , 4.6875,
6.75 , 9.1875, 12. , 15.1875, 18.75 , 22.6875,
27. , 31.6875, 36.75 , 42.1875, 48. , 54.1875,
60.75 , 67.6875, 75. , 82.6875, 90.75 , 99.1875,

2
selfdrive/controls/controlsd.py
Unescape View File

@ -253,7 +253,7 @@ class Controls:
self.events.add(EventName.deviceFalling)
if log.PandaState.FaultType.relayMalfunction in self.sm['pandaState'].faults:
self.events.add(EventName.relayMalfunction)
if self.sm['longitudinalPlan'].fcw:
if self.sm['longitudinalPlan'].fcw or (self.enabled and self.sm['modelV2'].meta.hardBrakePredicted):
self.events.add(EventName.fcw)
if TICI and self.enable_lte_onroad:

90
selfdrive/modeld/models/driving.cc
Unescape View File

@ -13,15 +13,17 @@
#include "selfdrive/common/timing.h"
constexpr int DESIRE_PRED_SIZE = 32;
constexpr int OTHER_META_SIZE = 4;
constexpr int OTHER_META_SIZE = 32;
constexpr int NUM_META_INTERVALS = 5;
constexpr int META_STRIDE = 6;
constexpr int MODEL_WIDTH = 512;
constexpr int MODEL_HEIGHT = 256;
constexpr int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2;
constexpr int PLAN_MHP_N = 5;
constexpr int PLAN_MHP_COLUMNS = 30;
constexpr int PLAN_MHP_VALS = 30*33;
constexpr int PLAN_MHP_COLUMNS = 15;
constexpr int PLAN_MHP_VALS = 15*33;
constexpr int PLAN_MHP_SELECTION = 1;
constexpr int PLAN_MHP_GROUP_SIZE = (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION);
@ -35,7 +37,7 @@ constexpr int POSE_SIZE = 12;
constexpr int PLAN_IDX = 0;
constexpr int LL_IDX = PLAN_IDX + PLAN_MHP_N*PLAN_MHP_GROUP_SIZE;
constexpr int LL_PROB_IDX = LL_IDX + 4*2*2*33;
constexpr int RE_IDX = LL_PROB_IDX + 4;
constexpr int RE_IDX = LL_PROB_IDX + 8;
constexpr int LEAD_IDX = RE_IDX + 2*2*2*33;
constexpr int LEAD_PROB_IDX = LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE);
constexpr int DESIRE_STATE_IDX = LEAD_PROB_IDX + 3;
@ -48,6 +50,13 @@ constexpr int OUTPUT_SIZE = POSE_IDX + POSE_SIZE;
constexpr int TEMPORAL_SIZE = 0;
#endif
constexpr float FCW_THRESHOLD_5MS2_HIGH = 0.15;
constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05;
constexpr float FCW_THRESHOLD_3MS2 = 0.7;
float prev_brake_5ms2_probs[5] = {0,0,0,0,0};
float prev_brake_3ms2_probs[3] = {0,0,0};
// #define DUMP_YUV
void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
@ -151,6 +160,12 @@ static const float *get_lead_data(const float *lead, int t_offset) {
}
void fill_sigmoid(const float *input, float *output, int len, int stride) {
for (int i=0; i<len; i++) {
output[i] = sigmoid(input[i*stride]);
}
}
void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float *lead_data, const float *prob, int t_offset, float t) {
const float *data = get_lead_data(lead_data, t_offset);
lead.setProb(sigmoid(prob[t_offset]));
@ -173,12 +188,48 @@ void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_da
softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN],
&desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN);
}
meta.setDesireState(desire_state_softmax);
float gas_disengage_sigmoid[NUM_META_INTERVALS];
float brake_disengage_sigmoid[NUM_META_INTERVALS];
float steer_override_sigmoid[NUM_META_INTERVALS];
float brake_3ms2_sigmoid[NUM_META_INTERVALS];
float brake_4ms2_sigmoid[NUM_META_INTERVALS];
float brake_5ms2_sigmoid[NUM_META_INTERVALS];
fill_sigmoid(&meta_data[DESIRE_LEN+1], gas_disengage_sigmoid, NUM_META_INTERVALS, META_STRIDE);
fill_sigmoid(&meta_data[DESIRE_LEN+2], brake_disengage_sigmoid, NUM_META_INTERVALS, META_STRIDE);
fill_sigmoid(&meta_data[DESIRE_LEN+3], steer_override_sigmoid, NUM_META_INTERVALS, META_STRIDE);
fill_sigmoid(&meta_data[DESIRE_LEN+4], brake_3ms2_sigmoid, NUM_META_INTERVALS, META_STRIDE);
fill_sigmoid(&meta_data[DESIRE_LEN+5], brake_4ms2_sigmoid, NUM_META_INTERVALS, META_STRIDE);
fill_sigmoid(&meta_data[DESIRE_LEN+6], brake_5ms2_sigmoid, NUM_META_INTERVALS, META_STRIDE);
memmove(prev_brake_5ms2_probs, &prev_brake_5ms2_probs[1], 4*sizeof(float));
memmove(prev_brake_3ms2_probs, &prev_brake_3ms2_probs[1], 2*sizeof(float));
prev_brake_5ms2_probs[4] = brake_5ms2_sigmoid[0];
prev_brake_3ms2_probs[2] = brake_3ms2_sigmoid[0];
bool above_fcw_threshold = true;
for (int i=0; i<5; i++) {
float threshold = i < 2 ? FCW_THRESHOLD_5MS2_LOW : FCW_THRESHOLD_5MS2_HIGH;
above_fcw_threshold = above_fcw_threshold && prev_brake_5ms2_probs[i] > threshold;
}
for (int i=0; i<3; i++) {
above_fcw_threshold = above_fcw_threshold && prev_brake_3ms2_probs[i] > FCW_THRESHOLD_3MS2;
}
auto disengage = meta.initDisengagePredictions();
disengage.setT({2,4,6,8,10});
disengage.setGasDisengageProbs(gas_disengage_sigmoid);
disengage.setBrakeDisengageProbs(brake_disengage_sigmoid);
disengage.setSteerOverrideProbs(steer_override_sigmoid);
disengage.setBrake3MetersPerSecondSquaredProbs(brake_3ms2_sigmoid);
disengage.setBrake4MetersPerSecondSquaredProbs(brake_4ms2_sigmoid);
disengage.setBrake5MetersPerSecondSquaredProbs(brake_5ms2_sigmoid);
meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN]));
meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1]));
meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2]));
meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3]));
meta.setDesirePrediction(desire_pred_softmax);
meta.setDesireState(desire_state_softmax);
meta.setHardBrakePredicted(above_fcw_threshold);
}
void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data,
@ -221,8 +272,25 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
// plan
const float *best_plan = get_plan_data(net_outputs.plan);
float plan_t_arr[TRAJECTORY_SIZE];
for (int i=0; i<TRAJECTORY_SIZE; i++) {
plan_t_arr[i] = best_plan[i*PLAN_MHP_COLUMNS + 15];
int xidx = 0, tidx = 0;
for (; xidx<TRAJECTORY_SIZE; xidx++) {
// increment tidx until we find an element that's further away than the current xidx
for (; tidx < TRAJECTORY_SIZE - 1 && best_plan[(tidx+1)*PLAN_MHP_COLUMNS] < X_IDXS[xidx]; tidx++) {}
float current_x_val = best_plan[tidx*PLAN_MHP_COLUMNS];
float next_x_val = best_plan[(tidx+1)*PLAN_MHP_COLUMNS];
if (next_x_val < X_IDXS[xidx]) {
// if the plan doesn't extend far enough, set plan_t to the max value (10s), then break and fill the rest with nans
plan_t_arr[xidx] = T_IDXS[TRAJECTORY_SIZE-1];
xidx++;
break;
} else {
// otherwise, interpolate to find `t` for the current xidx
float p = (X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val);
plan_t_arr[xidx] = p * T_IDXS[tidx+1] + (1 - p) * T_IDXS[tidx];
}
}
for (; xidx<TRAJECTORY_SIZE; xidx++) {
plan_t_arr[xidx] = NAN;
}
fill_xyzt(framed.initPosition(), best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr, true);
@ -236,7 +304,7 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
float lane_line_stds_arr[4];
for (int i = 0; i < 4; i++) {
fill_xyzt(lane_lines[i], &net_outputs.lane_lines[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr, false);
lane_line_probs_arr[i] = sigmoid(net_outputs.lane_lines_prob[i]);
lane_line_probs_arr[i] = sigmoid(net_outputs.lane_lines_prob[i*2+1]);
lane_line_stds_arr[i] = exp(net_outputs.lane_lines[2*TRAJECTORY_SIZE*(4 + i)]);
}
framed.setLaneLineProbs(lane_line_probs_arr);

2
selfdrive/test/process_replay/model_replay_ref_commit
Unescape View File

@ -1 +1 @@
15390f9a445a1fd775079d1938ed14b0d6afacc9
3a98c2fc53e9a9628eb63c81c4cbd4d72a756386

Write Preview
Loading…
Cancel
Save