sadmen
/
dragonpilot
mirror of https://github.com/dragonpilot-community/dragonpilot.git
1
0
Fork 0
Code Issues 64 Packages Projects Releases Wiki Activity

Test model refactor (#2720)

Browse Source 
* template funcion fill_meta

* add function get_plan_max_idx

* add function get_mdn_max_idx

* remove temp builder variables

* get_mdn_max_idx->get_lead_data

* get_pan_max_idx->get_plan_data

* fill_lane_line

* combine fill_lane_line&fill_path into one function

* fill_lead_vw

* using unique_ptr

* prefer using constexpr

* more constexpr,remove duplicate defined TRAJECTORY_SIZE

* remove suffix _arr from variable name

* misc

* remove extern c

* refactor model_publish

* remove unused paramaters

* traffic_convention to c style array

* c style array:prev_desire&pulse_desire

* fix error&make easy for review

* const mat3 &transform

* move cl_command_queue into ModelState

* use maco LEAD_MHP_SELECTION

* move constexpr from .h to .cc

* remove #define MODEL_NAME

* modeldata.h: contexpr

* remove param temporal from model_init

* helper function get_best_data

* fix probs

* int

Co-authored-by: deanlee <deanlee3@gmail.com>
pull/214/head
HaraldSchafer 5 years ago committed by GitHub
parent bbe868d312
commit 630c7309a5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 160 additions and 278 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. 16
      selfdrive/common/modeldata.h
  2. 16
      selfdrive/modeld/modeld.cc
  3. 2
      selfdrive/modeld/models/commonmodel.cc
  4. 17
      selfdrive/modeld/models/commonmodel.h
  5. 318
      selfdrive/modeld/models/driving.cc
  6. 67
      selfdrive/modeld/models/driving.h

16
selfdrive/common/modeldata.h
Unescape View File

@ -1,10 +1,10 @@
#pragma once
#define MODEL_PATH_DISTANCE 192
#define TRAJECTORY_SIZE 33
#define MIN_DRAW_DISTANCE 10.0
#define MAX_DRAW_DISTANCE 100.0
#define POLYFIT_DEGREE 4
#define SPEED_PERCENTILES 10
#define DESIRE_PRED_SIZE 32
#define OTHER_META_SIZE 4
constexpr int MODEL_PATH_DISTANCE = 192;
constexpr int TRAJECTORY_SIZE = 33;
constexpr float MIN_DRAW_DISTANCE = 10.0;
constexpr float MAX_DRAW_DISTANCE = 100.0;
constexpr int POLYFIT_DEGREE = 4;
constexpr int SPEED_PERCENTILES = 10;
constexpr int DESIRE_PRED_SIZE = 32;
constexpr int OTHER_META_SIZE = 4;

16
selfdrive/modeld/modeld.cc
Unescape View File

@ -117,11 +117,10 @@ int main(int argc, char **argv) {
// cl init
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
cl_command_queue q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
// init the models
ModelState model;
model_init(&model, device_id, context, true);
model_init(&model, device_id, context);
LOGW("models loaded, modeld starting");
// loop
@ -185,8 +184,8 @@ int main(int argc, char **argv) {
visionbuf_sync(&yuv_ion, VISIONBUF_SYNC_TO_DEVICE);
ModelDataRaw model_buf =
model_eval_frame(&model, q, yuv_ion.buf_cl, buf_info.width, buf_info.height,
model_transform, NULL, vec_desire);
model_eval_frame(&model, yuv_ion.buf_cl, buf_info.width, buf_info.height,
model_transform, vec_desire);
mt2 = millis_since_boot();
float model_execution_time = (mt2 - mt1) / 1000.0;
@ -196,10 +195,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);
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);
const float *raw_pred_ptr = send_raw_pred ? &model.output[0] : nullptr;
model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, raw_pred_ptr, extra.timestamp_eof, model_execution_time);
posenet_publish(pm, extra.frame_id, vipc_dropped_frames, 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);
last = mt1;
@ -217,8 +215,6 @@ int main(int argc, char **argv) {
err = pthread_join(live_thread_handle, NULL);
assert(err == 0);
CL_CHECK(clReleaseContext(context));
CL_CHECK(clReleaseCommandQueue(q));
pthread_mutex_destroy(&transform_lock);
return 0;
}

2
selfdrive/modeld/models/commonmodel.cc
Unescape View File

@ -25,7 +25,7 @@ void frame_init(ModelFrame* frame, int width, int height,
float *frame_prepare(ModelFrame* frame, cl_command_queue q,
cl_mem yuv_cl, int width, int height,
mat3 transform) {
const mat3 &transform) {
transform_queue(&frame->transform, q,
yuv_cl, width, height,
frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl,

17
selfdrive/modeld/models/commonmodel.h
Unescape View File

@ -1,6 +1,4 @@
#ifndef COMMONMODEL_H
#define COMMONMODEL_H
#pragma once
#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
#ifdef __APPLE__
#include <OpenCL/cl.h>
@ -14,10 +12,6 @@
#include "transforms/transform.h"
#include "transforms/loadyuv.h"
#ifdef __cplusplus
extern "C" {
#endif
const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL;
void softmax(const float* input, float* output, size_t len);
@ -37,12 +31,5 @@ void frame_init(ModelFrame* frame, int width, int height,
cl_device_id device_id, cl_context context);
float *frame_prepare(ModelFrame* frame, cl_command_queue q,
cl_mem yuv_cl, int width, int height,
mat3 transform);
const mat3 &transform);
void frame_free(ModelFrame* frame);
#ifdef __cplusplus
}
#endif
#endif

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

@ -8,25 +8,42 @@
#include "common/timing.h"
#include "common/params.h"
#include "driving.h"
#define MIN_VALID_LEN 10.0
#define TRAJECTORY_SIZE 33
#define TRAJECTORY_TIME 10.0
#define TRAJECTORY_DISTANCE 192.0
#define PLAN_IDX 0
#define LL_IDX PLAN_IDX + PLAN_MHP_N*(PLAN_MHP_GROUP_SIZE)
#define LL_PROB_IDX LL_IDX + 4*2*2*33
#define RE_IDX LL_PROB_IDX + 4
#define LEAD_IDX RE_IDX + 2*2*2*33
#define LEAD_PROB_IDX LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE)
#define DESIRE_STATE_IDX LEAD_PROB_IDX + 3
#define META_IDX DESIRE_STATE_IDX + DESIRE_LEN
#define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE
#define OUTPUT_SIZE POSE_IDX + POSE_SIZE
#include "clutil.h"
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_SELECTION = 1;
constexpr int PLAN_MHP_GROUP_SIZE = (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION);
constexpr int LEAD_MHP_N = 5;
constexpr int LEAD_MHP_VALS = 4;
constexpr int LEAD_MHP_SELECTION = 3;
constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION);
constexpr int POSE_SIZE = 12;
constexpr int MIN_VALID_LEN = 10;
constexpr int TRAJECTORY_TIME = 10;
constexpr float TRAJECTORY_DISTANCE = 192.0;
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 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;
constexpr int META_IDX = DESIRE_STATE_IDX + DESIRE_LEN;
constexpr int POSE_IDX = META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE;
constexpr int OUTPUT_SIZE = POSE_IDX + POSE_SIZE;
#ifdef TEMPORAL
#define TEMPORAL_SIZE 512
constexpr int TEMPORAL_SIZE = 512;
#else
#define TEMPORAL_SIZE 0
constexpr int TEMPORAL_SIZE = 0;
#endif
// #define DUMP_YUV
@ -35,36 +52,26 @@ Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE - 1> vander;
float X_IDXS[TRAJECTORY_SIZE];
float T_IDXS[TRAJECTORY_SIZE];
void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) {
void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context);
s->input_frames = (float*)calloc(MODEL_FRAME_SIZE * 2, sizeof(float));
const int output_size = OUTPUT_SIZE + TEMPORAL_SIZE;
s->output = (float*)calloc(output_size, sizeof(float));
s->input_frames = std::make_unique<float[]>(MODEL_FRAME_SIZE * 2);
s->m = new DefaultRunModel("../../models/supercombo.dlc", s->output, output_size, USE_GPU_RUNTIME);
constexpr int output_size = OUTPUT_SIZE + TEMPORAL_SIZE;
s->output = std::make_unique<float[]>(output_size);
s->m = std::make_unique<DefaultRunModel>("../../models/supercombo.dlc", &s->output[0], output_size, USE_GPU_RUNTIME);
#ifdef TEMPORAL
assert(temporal);
s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE);
#endif
#ifdef DESIRE
s->prev_desire = std::make_unique<float[]>(DESIRE_LEN);
s->pulse_desire = std::make_unique<float[]>(DESIRE_LEN);
s->m->addDesire(s->pulse_desire.get(), DESIRE_LEN);
s->m->addDesire(s->pulse_desire, DESIRE_LEN);
#endif
#ifdef TRAFFIC_CONVENTION
s->traffic_convention = std::make_unique<float[]>(TRAFFIC_CONVENTION_LEN);
s->m->addTrafficConvention(s->traffic_convention.get(), TRAFFIC_CONVENTION_LEN);
bool is_rhd = Params().read_db_bool("IsRHD");
if (is_rhd) {
s->traffic_convention[1] = 1.0;
} else {
s->traffic_convention[0] = 1.0;
}
const int idx = Params().read_db_bool("IsRHD") ? 1 : 0;
s->traffic_convention[idx] = 1.0;
s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN);
#endif
// Build Vandermonde matrix
@ -75,12 +82,12 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t
vander(i, j) = pow(X_IDXS[i], POLYFIT_DEGREE-j-1);
}
}
s->q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
}
ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
cl_mem yuv_cl, int width, int height,
mat3 transform, void* sock,
float *desire_in) {
ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height,
const mat3 &transform, float *desire_in) {
#ifdef DESIRE
if (desire_in != NULL) {
for (int i = 1; i < DESIRE_LEN; i++) {
@ -98,10 +105,10 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
//for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");
float *new_frame_buf = frame_prepare(&s->frame, q, yuv_cl, width, height, transform);
float *new_frame_buf = frame_prepare(&s->frame, s->q, yuv_cl, width, height, transform);
memmove(&s->input_frames[0], &s->input_frames[MODEL_FRAME_SIZE], sizeof(float)*MODEL_FRAME_SIZE);
memmove(&s->input_frames[MODEL_FRAME_SIZE], new_frame_buf, sizeof(float)*MODEL_FRAME_SIZE);
s->m->execute(s->input_frames, MODEL_FRAME_SIZE*2);
s->m->execute(&s->input_frames[0], MODEL_FRAME_SIZE*2);
#ifdef DUMP_YUV
FILE *dump_yuv_file = fopen("/sdcard/dump.yuv", "wb");
@ -110,7 +117,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
assert(1==2);
#endif
clEnqueueUnmapMemObject(q, s->frame.net_input, (void*)new_frame_buf, 0, NULL, NULL);
clEnqueueUnmapMemObject(s->q, s->frame.net_input, (void*)new_frame_buf, 0, NULL, NULL);
// net outputs
ModelDataRaw net_outputs;
@ -126,10 +133,8 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
}
void model_free(ModelState* s) {
free(s->output);
free(s->input_frames);
frame_free(&s->frame);
delete s->m;
CL_CHECK(clReleaseCommandQueue(s->q));
}
void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) {
@ -157,48 +162,53 @@ void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) {
out[3] = y0;
}
void fill_path(cereal::ModelData::PathData::Builder path, const float * data, float valid_len, int valid_len_idx) {
float points_arr[TRAJECTORY_SIZE];
float stds_arr[TRAJECTORY_SIZE];
float poly_arr[POLYFIT_DEGREE];
float std;
static const float *get_best_data(const float *data, int size, int group_size, int offset) {
int max_idx = 0;
for (int i = 1; i < size; i++) {
if (data[(i + 1) * group_size + offset] >
data[(max_idx + 1) * group_size + offset]) {
max_idx = i;
}
}
return &data[max_idx * group_size];
}
for (int i=0; i<TRAJECTORY_SIZE; i++) {
// negative sign because mpc has left positive
points_arr[i] = -data[30*i + 16];
stds_arr[i] = exp(data[30*(33 + i) + 16]);
static const float *get_plan_data(float *plan) {
return get_best_data(plan, PLAN_MHP_N, PLAN_MHP_GROUP_SIZE, -1);
}
std = stds_arr[0];
poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx);
path.setPoly(poly_arr);
path.setProb(1.0);
path.setStd(std);
path.setValidLen(valid_len);
static const float *get_lead_data(const float *lead, int t_offset) {
return get_best_data(lead, LEAD_MHP_N, LEAD_MHP_GROUP_SIZE, t_offset - LEAD_MHP_SELECTION);
}
void fill_lane_line(cereal::ModelData::PathData::Builder path, const float * data, int ll_idx, float valid_len, int valid_len_idx, float prob) {
float points_arr[TRAJECTORY_SIZE];
float stds_arr[TRAJECTORY_SIZE];
float poly_arr[POLYFIT_DEGREE];
float std;
void fill_path(cereal::ModelData::PathData::Builder path, const float *data, const float prob,
float valid_len, int valid_len_idx, int ll_idx) {
float points[TRAJECTORY_SIZE] = {};
float stds[TRAJECTORY_SIZE] = {};
float poly[POLYFIT_DEGREE] = {};
for (int i=0; i<TRAJECTORY_SIZE; i++) {
// negative sign because mpc has left positive
points_arr[i] = -data[2*33*ll_idx + 2*i];
stds_arr[i] = exp(data[2*33*(4 + ll_idx) + 2*i]);
if (ll_idx == 0) {
points[i] = -data[30 * i + 16];
stds[i] = exp(data[30 * (33 + i) + 16]);
} else {
points[i] = -data[2 * 33 * ll_idx + 2 * i];
stds[i] = exp(data[2 * 33 * (4 + ll_idx) + 2 * i]);
}
std = stds_arr[0];
poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx);
}
const float std = stds[0];
poly_fit(points, stds, poly, valid_len_idx);
path.setPoly(poly_arr);
path.setPoly(poly);
path.setProb(prob);
path.setStd(std);
path.setValidLen(valid_len);
}
void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float * data, float prob, float t) {
lead.setProb(prob);
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]));
lead.setT(t);
float xyva_arr[LEAD_MHP_VALS];
float xyva_stds_arr[LEAD_MHP_VALS];
@ -210,8 +220,9 @@ void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float * d
lead.setXyvaStd(xyva_stds_arr);
}
void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, float prob) {
lead.setProb(prob);
void fill_lead(cereal::ModelData::LeadData::Builder lead, const float *lead_data, const float *prob, int t_offset) {
const float *data = get_lead_data(lead_data, t_offset);
lead.setProb(sigmoid(prob[t_offset]));
lead.setDist(data[0]);
lead.setStd(exp(data[LEAD_MHP_VALS]));
// TODO make all msgs same format
@ -223,7 +234,8 @@ void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, fl
lead.setRelAStd(exp(data[LEAD_MHP_VALS + 3]));
}
void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) {
template <class MetaBuilder>
void fill_meta(MetaBuilder meta, const float *meta_data) {
float desire_state_softmax[DESIRE_LEN];
float desire_pred_softmax[4*DESIRE_LEN];
softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN);
@ -239,22 +251,6 @@ void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_dat
meta.setDesirePrediction(desire_pred_softmax);
}
void fill_meta_v2(cereal::ModelDataV2::MetaData::Builder meta, const float * meta_data) {
float desire_state_softmax[DESIRE_LEN] = {};
float desire_pred_softmax[4*DESIRE_LEN] = {};
softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN);
for (int i=0; i<4; i++) {
softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN],
&desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN);
}
meta.setDesireState(desire_state_softmax);
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);
}
void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data,
int columns, int column_offset, float * plan_t_arr) {
float x_arr[TRAJECTORY_SIZE] = {};
@ -292,34 +288,9 @@ void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data,
xyzt.setT(t_arr);
}
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, const float* raw_pred, uint64_t timestamp_eof,
float model_execution_time) {
// make msg
MessageBuilder msg;
auto framed = msg.initEvent().initModelV2();
uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
framed.setFrameId(vipc_frame_id);
framed.setFrameAge(frame_age);
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)));
}
void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_outputs) {
// plan
int plan_mhp_max_idx = 0;
for (int i=1; i<PLAN_MHP_N; i++) {
if (net_outputs.plan[(i + 1)*(PLAN_MHP_GROUP_SIZE) - 1] >
net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) {
plan_mhp_max_idx = i;
}
}
float * best_plan = &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)];
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];
@ -352,59 +323,24 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
framed.setRoadEdgeStds(road_edge_stds_arr);
// meta
auto meta = framed.initMeta();
fill_meta_v2(meta, net_outputs.meta);
fill_meta(framed.initMeta(), net_outputs.meta);
// leads
auto leads = framed.initLeads(LEAD_MHP_SELECTION);
float t_offsets[LEAD_MHP_SELECTION] = {0.0, 2.0, 4.0};
for (int t_offset=0; t_offset<LEAD_MHP_SELECTION; t_offset++) {
int mdn_max_idx = 0;
for (int i=1; i<LEAD_MHP_N; i++) {
if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - LEAD_MHP_SELECTION] >
net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - LEAD_MHP_SELECTION]) {
mdn_max_idx = i;
}
fill_lead_v2(leads[t_offset], net_outputs.lead, net_outputs.lead_prob, t_offset, t_offsets[t_offset]);
}
fill_lead_v2(leads[t_offset], &net_outputs.lead[mdn_max_idx * (LEAD_MHP_GROUP_SIZE)],
sigmoid(net_outputs.lead_prob[t_offset]), t_offsets[t_offset]);
}
pm.send("modelV2", msg);
}
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, 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;
MessageBuilder msg;
auto framed = msg.initEvent().initModel();
framed.setFrameId(vipc_frame_id);
framed.setFrameAge(frame_age);
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)));
}
void fill_model(cereal::ModelData::Builder &framed, const ModelDataRaw &net_outputs) {
// Find the distribution that corresponds to the most probable plan
int plan_mhp_max_idx = 0;
for (int i=1; i<PLAN_MHP_N; i++) {
if (net_outputs.plan[(i + 1)*(PLAN_MHP_GROUP_SIZE) - 1] >
net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) {
plan_mhp_max_idx = i;
}
}
const float *best_plan = get_plan_data(net_outputs.plan);
// x pos at 10s is a good valid_len
float valid_len = 0;
float valid_len_candidate;
for (int i=1; i<TRAJECTORY_SIZE; i++) {
valid_len_candidate = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*i];
if (valid_len_candidate >= valid_len){
valid_len = valid_len_candidate;
if (const float len = best_plan[30*i]; len >= valid_len){
valid_len = len;
}
}
// clamp to 10 and MODEL_PATH_DISTANCE
@ -415,47 +351,39 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
valid_len_idx = i;
}
}
fill_path(framed.initPath(), best_plan, 1.0, valid_len, valid_len_idx, 0);
fill_path(framed.initLeftLane(), net_outputs.lane_lines, sigmoid(net_outputs.lane_lines_prob[1]), valid_len, valid_len_idx, 1);
fill_path(framed.initRightLane(), net_outputs.lane_lines, sigmoid(net_outputs.lane_lines_prob[2]), valid_len, valid_len_idx, 2);
auto lpath = framed.initPath();
fill_path(lpath, &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)], valid_len, valid_len_idx);
auto left_lane = framed.initLeftLane();
int ll_idx = 1;
fill_lane_line(left_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx,
sigmoid(net_outputs.lane_lines_prob[ll_idx]));
auto right_lane = framed.initRightLane();
ll_idx = 2;
fill_lane_line(right_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx,
sigmoid(net_outputs.lane_lines_prob[ll_idx]));
// Find the distribution that corresponds to the current lead
int mdn_max_idx = 0;
int t_offset = 0;
for (int i=1; i<LEAD_MHP_N; i++) {
if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3] >
net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3]) {
mdn_max_idx = i;
}
}
fill_lead(framed.initLead(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[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_MHP_N; i++) {
if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3] >
net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3]) {
mdn_max_idx = i;
}
}
fill_lead(framed.initLeadFuture(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[t_offset]));
fill_lead(framed.initLead(), net_outputs.lead, net_outputs.lead_prob, 0);
fill_lead(framed.initLeadFuture(), net_outputs.lead, net_outputs.lead_prob, 1);
fill_meta(framed.initMeta(), net_outputs.meta);
}
pm.send("model", msg);
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
const ModelDataRaw &net_outputs, const float *raw_pred, uint64_t timestamp_eof,
float model_execution_time) {
const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
auto do_publish = [&](auto init_model_func, const char *pub_name) {
MessageBuilder msg;
auto framed = (msg.initEvent().*(init_model_func))();
framed.setFrameId(vipc_frame_id);
framed.setFrameAge(frame_age);
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)));
}
fill_model(framed, net_outputs);
pm.send(pub_name, msg);
};
do_publish(&cereal::Event::Builder::initModel, "model");
do_publish(&cereal::Event::Builder::initModelV2, "modelV2");
}
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
uint32_t vipc_dropped_frames, float frame_drop,
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
float trans_arr[3];
float trans_std_arr[3];

67
selfdrive/modeld/models/driving.h
Unescape View File

@ -1,12 +1,10 @@
#ifndef MODEL_H
#define MODEL_H
#pragma once
// gate this here
#define TEMPORAL
#define DESIRE
#define TRAFFIC_CONVENTION
#include "common/mat.h"
#include "common/util.h"
#include "common/modeldata.h"
@ -17,29 +15,9 @@
#include <memory>
#include "messaging.hpp"
#define MODEL_NAME "supercombo_dlc"
#define MODEL_WIDTH 512
#define MODEL_HEIGHT 256
#define MODEL_FRAME_SIZE MODEL_WIDTH * MODEL_HEIGHT * 3 / 2
#define DESIRE_LEN 8
#define TRAFFIC_CONVENTION_LEN 2
#define PLAN_MHP_N 5
#define PLAN_MHP_COLUMNS 30
#define PLAN_MHP_VALS 30*33
#define PLAN_MHP_SELECTION 1
#define PLAN_MHP_GROUP_SIZE (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION)
#define LEAD_MHP_N 5
#define LEAD_MHP_VALS 4
#define LEAD_MHP_SELECTION 3
#define LEAD_MHP_GROUP_SIZE (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION)
#define POSE_SIZE 12
#define MODEL_FREQ 20
constexpr int DESIRE_LEN = 8;
constexpr int TRAFFIC_CONVENTION_LEN = 2;
constexpr int MODEL_FREQ = 20;
struct ModelDataRaw {
float *plan;
float *lane_lines;
@ -56,33 +34,26 @@ struct ModelDataRaw {
typedef struct ModelState {
ModelFrame frame;
float *output;
float *input_frames;
RunModel *m;
std::unique_ptr<float[]> output;
std::unique_ptr<float[]> input_frames;
std::unique_ptr<RunModel> m;
cl_command_queue q;
#ifdef DESIRE
std::unique_ptr<float[]> prev_desire;
std::unique_ptr<float[]> pulse_desire;
float prev_desire[DESIRE_LEN] = {};
float pulse_desire[DESIRE_LEN] = {};
#endif
#ifdef TRAFFIC_CONVENTION
std::unique_ptr<float[]> traffic_convention;
float traffic_convention[TRAFFIC_CONVENTION_LEN] = {};
#endif
} ModelState;
void model_init(ModelState* s, cl_device_id device_id,
cl_context context, int temporal);
ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
cl_mem yuv_cl, int width, int height,
mat3 transform, void* sock, float *desire_in);
void model_init(ModelState* s, cl_device_id device_id, cl_context context);
ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height,
const mat3 &transform, float *desire_in);
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, 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, 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,
uint64_t timestamp_eof);
#endif
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
const ModelDataRaw &net_outputs, const float *raw_pred, uint64_t timestamp_eof,
float model_execution_time);
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
const ModelDataRaw &net_outputs, uint64_t timestamp_eof);

Write Preview
Loading…
Cancel
Save