retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

save valid_len

albatross
George Hotz 2020-06-03 14:18:42 -07:00
parent 73db079199
commit 4e7fe3a9ec
2 changed files with 43 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cereal

@ -1 +1 @@
Subproject commit 46728e7e3b47cf73586e33540dc0b65317e308ce
Subproject commit 7dcbf6c41fb30b85dbb597fe771dc9f5e7f29161

83
selfdrive/modeld/models/driving.cc
View File

@ -175,7 +175,7 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bo
float valid_len;
// clamp to 5 and 192
valid_len = fmin(192, fmax(5, data[MODEL_PATH_DISTANCE*2]));
valid_len = fmin(192, fmax(5, data[MODEL_PATH_DISTANCE*2]));
for (int i=0; i<MODEL_PATH_DISTANCE; i++) {
points_arr[i] = data[i] + offset;
stds_arr[i] = softplus(data[MODEL_PATH_DISTANCE + i]) + 1e-6;
@ -200,6 +200,7 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bo
path.setPoly(poly);
path.setProb(prob);
path.setStd(std);
path.setValidLen(valid_len);
}
void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, int mdn_max_idx, int t_offset) {
@ -248,52 +249,52 @@ void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float
void model_publish(PubMaster &pm, uint32_t frame_id,
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());
// make msg
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto framed = event.initModel();
framed.setFrameId(frame_id);
framed.setTimestampEof(timestamp_eof);
auto framed = event.initModel();
framed.setFrameId(frame_id);
framed.setTimestampEof(timestamp_eof);
auto lpath = framed.initPath();
fill_path(lpath, net_outputs.path, false, 0);
auto left_lane = framed.initLeftLane();
fill_path(left_lane, net_outputs.left_lane, true, 1.8);
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);
auto lpath = framed.initPath();
fill_path(lpath, net_outputs.path, false, 0);
auto left_lane = framed.initLeftLane();
fill_path(left_lane, net_outputs.left_lane, true, 1.8);
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
int mdn_max_idx = 0;
int t_offset = 0;
for (int i=1; i<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8 + t_offset] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) {
mdn_max_idx = i;
}
// Find the distribution that corresponds to the current lead
int mdn_max_idx = 0;
int t_offset = 0;
for (int i=1; i<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8 + t_offset] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) {
mdn_max_idx = i;
}
auto lead = framed.initLead();
fill_lead(lead, net_outputs.lead, mdn_max_idx, t_offset);
// Find the distribution that corresponds to the lead in 2s
mdn_max_idx = 0;
t_offset = 1;
for (int i=1; i<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8 + t_offset] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) {
mdn_max_idx = i;
}
}
auto lead_future = framed.initLeadFuture();
fill_lead(lead_future, net_outputs.lead, mdn_max_idx, t_offset);
auto meta = framed.initMeta();
fill_meta(meta, net_outputs.meta);
pm.send("model", msg);
}
auto lead = framed.initLead();
fill_lead(lead, net_outputs.lead, mdn_max_idx, t_offset);
// Find the distribution that corresponds to the lead in 2s
mdn_max_idx = 0;
t_offset = 1;
for (int i=1; i<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8 + t_offset] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) {
mdn_max_idx = i;
}
}
auto lead_future = framed.initLeadFuture();
fill_lead(lead_future, net_outputs.lead, mdn_max_idx, t_offset);
auto meta = framed.initMeta();
fill_meta(meta, net_outputs.meta);
pm.send("model", msg);
}
void posenet_publish(PubMaster &pm, uint32_t frame_id,
const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {