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

Torch model (#2452)

Browse Source 
* refactor draw model

* rebase master

* correct valid_len

* rename function

* rename variables

* white space

* rebase to master

* e16c13ac-927d-455e-ae0a-81b482a2c787

* start rewriting

* save proress

* compiles!

* oops

* many fixes

* seems to work

* fix desires

* finally cleaned

* wrong std for ll

* dont pulse none

* compiles!

* ready to test

* WIP does not compile

* compiles

* various fixes

* does something!

* full 3d

* not needed

* draw up to 100m

* fix segfault

* wrong sign

* fix flicker

* add road edges

* finish v2 packet

* Added pytorch supercombo

* fix rebase

* no more keras

* Hacky solution to the NCHW/NHWC incompatibility between SNPE and our frame data

* dont break dmonitoringd, final model 229e3ce1-7259-412b-85e6-cc646d70f1d8/430

* fix hack

* Revert "fix hack"

This reverts commit 5550fc01a7881d065a5eddbbb42dac55ef7ec36c.

* Removed axis permutation hack

* Folded padding layers into conv layers

* Removed the last pad layer from the dlc

* Revert "Removed the last pad layer from the dlc"

This reverts commit b85f24b9e1d04abf64e85901a7ff49e00d82020a.

* Revert "Folded padding layers into conv layers"

This reverts commit b8d1773e4e76dea481acebbfad6a6235fbb58463.

* vision model: 5034ac8b-5703-4a49-948b-11c064d10880/780  temporal model: 229e3ce1-7259-412b-85e6-cc646d70f1d8/430  with permute + pool opt

* fix ui drawing with clips

* ./compile_torch.py 5034ac8b-5703-4a49-948b-11c064d10880/780 dfcd2375-81d8-49df-95bf-1d2d6ad86010/450 with variable history length

* std::clamp

* not sure how this compiled before

* 2895ace6-a296-47ac-86e6-17ea800a74e5/550

* db090195-8810-42de-ab38-bb835d775d87/601

* 5m is very little

* onnx runner

* add onnxruntime to pipfile

* run in real time without using the whole CPU

* bump cereal;

* add stds

* set road edge opacity based on stddev

* don't access the model packet in paint

* convert mat.h to a c++ header file (#2499)

* update tests

* safety first

Co-authored-by: deanlee <deanlee3@gmail.com>
Co-authored-by: mitchell <mitchell@comma.ai>
Co-authored-by: Comma Device <device@comma.ai>
Co-authored-by: George Hotz <george@comma.ai>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: 08846b5c0ec3a5299c8cb73904da15a18d3aaccf
This commit is contained in:
HaraldSchafer
2020-11-11 20:31:46 -08:00
committed by GitHub
parent b0bb9daf21
commit 008f37c749
29 changed files with 574 additions and 406 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitattributes
+1 -1
View File
@@ -1,5 +1,6 @@
*.keras filter=lfs diff=lfs merge=lfs -text
*.dlc filter=lfs diff=lfs merge=lfs -text
*.onnx filter=lfs diff=lfs merge=lfs -text
*.pb filter=lfs diff=lfs merge=lfs -text
*.bin filter=lfs diff=lfs merge=lfs -text
*.apk filter=lfs diff=lfs merge=lfs -text
@@ -26,7 +27,6 @@ external/opencl/*.deb filter=lfs diff=lfs merge=lfs -text
phonelibs/zmq/aarch64-linux/lib/libzmq.a filter=lfs diff=lfs merge=lfs -text
external/azcopy/azcopy filter=lfs diff=lfs merge=lfs -text
*.ico filter=lfs diff=lfs merge=lfs -text
*.onnx filter=lfs diff=lfs merge=lfs -text
*.svg filter=lfs diff=lfs merge=lfs -text
*.png filter=lfs diff=lfs merge=lfs -text
*.gif filter=lfs diff=lfs merge=lfs -text
Pipfile
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:e826ede56274173f6fe320f8ad7e37781f2adaf946c419ae46d89f28d1dedea0
size 2032
oid sha256:caaf65aef8040796230e063638568a3c601a3318cfc419ac9033590141162342
size 2050
Pipfile.lock
Generated
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:c8f1d6fed63cdefaab74df5dab924646eb904ac0a63fa589d74b0e038878529b
size 195108
oid sha256:d7f30c8b6b62eb1743f4fc8bdd8d0d2bc278dc43c08c4e8d43ee0c7de5a70dc5
size 199263
cereal
+1 -1
Submodule cereal updated: eb0ede91af...72b40bf2ee
models/supercombo.dlc
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:227e9cd978c463791a1703ff68f2b305a7ae1af26a86d0046672a37a8ede5d3d
size 52636596
oid sha256:cfeed2def0f57975066afee34565dd926fc8f3059aa7e36bb6a64a2d3a517d23
size 56036227
models/supercombo.keras
-3
View File
@@ -1,3 +0,0 @@
version https://git-lfs.github.com/spec/v1
oid sha256:14d312f37e9278779bf68b4639142e8d31a569bde8046d5c60a2a3f746c1c590
size 53232072
models/supercombo.onnx
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:3bb5e51ee68ec63a1ecf3ef295edb2ea3e030d89539721d4d9c0a13fbff64145
size 56342654
release/files_common
+3 -2
View File
@@ -388,7 +388,7 @@ selfdrive/modeld/constants.py
selfdrive/modeld/modeld
selfdrive/modeld/dmonitoringmodeld
selfdrive/modeld/models/commonmodel.c
selfdrive/modeld/models/commonmodel.cc
selfdrive/modeld/models/commonmodel.h
selfdrive/modeld/models/driving.cc
selfdrive/modeld/models/driving.h
@@ -397,7 +397,8 @@ selfdrive/modeld/models/dmonitoring.h
selfdrive/modeld/transforms/loadyuv.[c,h]
selfdrive/modeld/transforms/loadyuv.cl
selfdrive/modeld/transforms/transform.[c,h]
selfdrive/modeld/transforms/transform.cc
selfdrive/modeld/transforms/transform.h
selfdrive/modeld/transforms/transform.cl
selfdrive/modeld/thneed/thneed.*
selfdrive/common/mat.h
+6 -9
View File
@@ -1,5 +1,4 @@
#ifndef COMMON_MAT_H
#define COMMON_MAT_H
#pragma once
typedef struct vec3 {
float v[3];
@@ -17,7 +16,7 @@ typedef struct mat4 {
float v[4*4];
} mat4;
static inline mat3 matmul3(const mat3 a, const mat3 b) {
static inline mat3 matmul3(const mat3 &a, const mat3 &b) {
mat3 ret = {{0.0}};
for (int r=0; r<3; r++) {
for (int c=0; c<3; c++) {
@@ -31,7 +30,7 @@ static inline mat3 matmul3(const mat3 a, const mat3 b) {
return ret;
}
static inline vec3 matvecmul3(const mat3 a, const vec3 b) {
static inline vec3 matvecmul3(const mat3 &a, const vec3 &b) {
vec3 ret = {{0.0}};
for (int r=0; r<3; r++) {
for (int c=0; c<3; c++) {
@@ -41,7 +40,7 @@ static inline vec3 matvecmul3(const mat3 a, const vec3 b) {
return ret;
}
static inline mat4 matmul(const mat4 a, const mat4 b) {
static inline mat4 matmul(const mat4 &a, const mat4 &b) {
mat4 ret = {{0.0}};
for (int r=0; r<4; r++) {
for (int c=0; c<4; c++) {
@@ -55,7 +54,7 @@ static inline mat4 matmul(const mat4 a, const mat4 b) {
return ret;
}
static inline vec4 matvecmul(const mat4 a, const vec4 b) {
static inline vec4 matvecmul(const mat4 &a, const vec4 &b) {
vec4 ret = {{0.0}};
for (int r=0; r<4; r++) {
for (int c=0; c<4; c++) {
@@ -67,7 +66,7 @@ static inline vec4 matvecmul(const mat4 a, const vec4 b) {
// scales the input and output space of a transformation matrix
// that assumes pixel-center origin.
static inline mat3 transform_scale_buffer(const mat3 in, float s) {
static inline mat3 transform_scale_buffer(const mat3 &in, float s) {
// in_pt = ( transform(out_pt/s + 0.5) - 0.5) * s
mat3 transform_out = {{
@@ -84,5 +83,3 @@ static inline mat3 transform_scale_buffer(const mat3 in, float s) {
return matmul3(transform_in, matmul3(in, transform_out));
}
#endif
selfdrive/common/modeldata.h
+3
View File
@@ -1,6 +1,9 @@
#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
selfdrive/controls/lib/lane_planner.py
+2 -2
View File
@@ -59,8 +59,8 @@ class LanePlanner:
self.r_prob = md.rightLane.prob # right line prob
if len(md.meta.desireState):
self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft - 1]
self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight - 1]
self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft]
self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight]
def update_d_poly(self, v_ego):
# only offset left and right lane lines; offsetting p_poly does not make sense
selfdrive/modeld/SConscript
+7 -7
View File
@@ -6,10 +6,10 @@ libs = [cereal, messaging, common, 'OpenCL', 'SNPE', 'symphony-cpu', 'capnp', 'z
TEST_THNEED = False
common_src = [
"models/commonmodel.c",
"models/commonmodel.cc",
"runners/snpemodel.cc",
"transforms/loadyuv.c",
"transforms/transform.c"
"transforms/transform.cc"
]
if arch == "aarch64":
@@ -23,12 +23,12 @@ elif arch == "larch64":
else:
libs += ['pthread']
# for tensorflow support
common_src += ['runners/tfmodel.cc']
# for onnx support
common_src += ['runners/onnxmodel.cc']
# tell runners to use tensorflow
lenv['CFLAGS'].append("-DUSE_TF_MODEL")
lenv['CXXFLAGS'].append("-DUSE_TF_MODEL")
# tell runners to use onnx
lenv['CFLAGS'].append("-DUSE_ONNX_MODEL")
lenv['CXXFLAGS'].append("-DUSE_ONNX_MODEL")
if arch == "Darwin":
# fix OpenCL
selfdrive/modeld/modeld.cc
+3 -2
View File
@@ -111,7 +111,7 @@ int main(int argc, char **argv) {
assert(err == 0);
// messaging
PubMaster pm({"model", "cameraOdometry"});
PubMaster pm({"modelV2", "model", "cameraOdometry"});
SubMaster sm({"pathPlan", "frame"});
#if defined(QCOM) || defined(QCOM2)
@@ -173,7 +173,7 @@ int main(int argc, char **argv) {
if (sm.update(0) > 0){
// TODO: path planner timeout?
desire = ((int)sm["pathPlan"].getPathPlan().getDesire()) - 1;
desire = ((int)sm["pathPlan"].getPathPlan().getDesire());
frame_id = sm["frame"].getFrame().getFrameId();
}
@@ -200,6 +200,7 @@ int main(int argc, char **argv) {
float frame_drop_perc = frames_dropped / MODEL_FREQ;
model_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof);
model_publish_v2(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof);
posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, 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_perc);
selfdrive/modeld/models/commonmodel.c → selfdrive/modeld/models/commonmodel.cc
+23
View File
@@ -59,6 +59,29 @@ void frame_free(ModelFrame* frame) {
clReleaseMemObject(frame->transformed_y_cl);
}
void softmax(const float* input, float* output, size_t len) {
float max_val = -FLT_MAX;
for(int i = 0; i < len; i++) {
const float v = input[i];
if( v > max_val ) {
max_val = v;
}
}
float denominator = 0;
for(int i = 0; i < len; i++) {
float const v = input[i];
float const v_exp = expf(v - max_val);
denominator += v_exp;
output[i] = v_exp;
}
const float inv_denominator = 1. / denominator;
for(int i = 0; i < len; i++) {
output[i] *= inv_denominator;
}
}
float sigmoid(float input) {
return 1 / (1 + expf(-input));
selfdrive/modeld/models/commonmodel.h
+2
View File
@@ -8,6 +8,7 @@
#include <CL/cl.h>
#endif
#include <float.h>
#include "common/mat.h"
#include "transforms/transform.h"
#include "transforms/loadyuv.h"
@@ -16,6 +17,7 @@
extern "C" {
#endif
void softmax(const float* input, float* output, size_t len);
float softplus(float input);
float sigmoid(float input);
selfdrive/modeld/models/driving.cc
+274 -85
View File
@@ -9,14 +9,17 @@
#include "common/params.h"
#include "driving.h"
#define PATH_IDX 0
#define LL_IDX PATH_IDX + MODEL_PATH_DISTANCE*2 + 1
#define RL_IDX LL_IDX + MODEL_PATH_DISTANCE*2 + 2
#define LEAD_IDX RL_IDX + MODEL_PATH_DISTANCE*2 + 2
#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION
#define LONG_V_IDX LONG_X_IDX + TIME_DISTANCE*2
#define LONG_A_IDX LONG_V_IDX + TIME_DISTANCE*2
#define DESIRE_STATE_IDX LONG_A_IDX + TIME_DISTANCE*2
#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
@@ -29,6 +32,8 @@
// #define DUMP_YUV
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) {
frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context);
@@ -63,9 +68,11 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t
#endif
// Build Vandermonde matrix
for(int i = 0; i < MODEL_PATH_DISTANCE; i++) {
for(int i = 0; i < TRAJECTORY_SIZE; i++) {
for(int j = 0; j < POLYFIT_DEGREE - 1; j++) {
vander(i, j) = pow(i, POLYFIT_DEGREE-j-1);
X_IDXS[i] = (TRAJECTORY_DISTANCE/1024.0) * (pow(i,2));
T_IDXS[i] = (TRAJECTORY_TIME/1024.0) * (pow(i,2));
vander(i, j) = pow(X_IDXS[i], POLYFIT_DEGREE-j-1);
}
}
}
@@ -76,7 +83,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
float *desire_in) {
#ifdef DESIRE
if (desire_in != NULL) {
for (int i = 0; i < DESIRE_LEN; i++) {
for (int i = 1; i < DESIRE_LEN; i++) {
// Model decides when action is completed
// so desire input is just a pulse triggered on rising edge
if (desire_in[i] - s->prev_desire[i] > .99) {
@@ -107,13 +114,12 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
// net outputs
ModelDataRaw net_outputs;
net_outputs.path = &s->output[PATH_IDX];
net_outputs.left_lane = &s->output[LL_IDX];
net_outputs.right_lane = &s->output[RL_IDX];
net_outputs.plan = &s->output[PLAN_IDX];
net_outputs.lane_lines = &s->output[LL_IDX];
net_outputs.lane_lines_prob = &s->output[LL_PROB_IDX];
net_outputs.road_edges = &s->output[RE_IDX];
net_outputs.lead = &s->output[LEAD_IDX];
net_outputs.long_x = &s->output[LONG_X_IDX];
net_outputs.long_v = &s->output[LONG_V_IDX];
net_outputs.long_a = &s->output[LONG_A_IDX];
net_outputs.lead_prob = &s->output[LEAD_PROB_IDX];
net_outputs.meta = &s->output[DESIRE_STATE_IDX];
net_outputs.pose = &s->output[POSE_IDX];
return net_outputs;
@@ -151,35 +157,40 @@ 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, bool has_prob, const float offset) {
float points_arr[MODEL_PATH_DISTANCE];
float stds_arr[MODEL_PATH_DISTANCE];
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;
float prob;
float valid_len;
// clamp to 5 and MODEL_PATH_DISTANCE
valid_len = fmin(MODEL_PATH_DISTANCE, 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;
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]);
}
if (has_prob) {
prob = sigmoid(data[MODEL_PATH_DISTANCE*2 + 1]);
} else {
prob = 1.0;
}
std = softplus(data[MODEL_PATH_DISTANCE]) + 1e-6;
poly_fit(points_arr, stds_arr, poly_arr, valid_len);
std = stds_arr[0];
poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx);
if (std::getenv("DEBUG")){
kj::ArrayPtr<const float> stds(&stds_arr[0], ARRAYSIZE(stds_arr));
path.setStds(stds);
kj::ArrayPtr<const float> poly(&poly_arr[0], ARRAYSIZE(poly_arr));
path.setPoly(poly);
path.setProb(1.0);
path.setStd(std);
path.setValidLen(valid_len);
}
kj::ArrayPtr<const float> points(&points_arr[0], ARRAYSIZE(points_arr));
path.setPoints(points);
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;
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]);
}
std = stds_arr[0];
poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx);
kj::ArrayPtr<const float> poly(&poly_arr[0], ARRAYSIZE(poly_arr));
path.setPoly(poly);
@@ -188,48 +199,197 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bo
path.setValidLen(valid_len);
}
void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, int mdn_max_idx, int t_offset) {
const double x_scale = 10.0;
const double y_scale = 10.0;
void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float * data, float prob, float t) {
lead.setProb(prob);
lead.setT(t);
float xyva_arr[LEAD_MHP_VALS];
float xyva_stds_arr[LEAD_MHP_VALS];
for (int i=0; i<LEAD_MHP_VALS; i++) {
xyva_arr[i] = data[LEAD_MHP_VALS + i];
xyva_stds_arr[i] = exp(data[LEAD_MHP_VALS + i]);
}
kj::ArrayPtr<const float> xyva(xyva_arr, LEAD_MHP_VALS);
kj::ArrayPtr<const float> xyva_stds(xyva_stds_arr, LEAD_MHP_VALS);
lead.setXyva(xyva);
lead.setXyvaStd(xyva_stds);
}
lead.setProb(sigmoid(data[LEAD_MDN_N*MDN_GROUP_SIZE + t_offset]));
lead.setDist(x_scale * data[mdn_max_idx*MDN_GROUP_SIZE]);
lead.setStd(x_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]));
lead.setRelY(y_scale * data[mdn_max_idx*MDN_GROUP_SIZE + 1]);
lead.setRelYStd(y_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]));
lead.setRelVel(data[mdn_max_idx*MDN_GROUP_SIZE + 2]);
lead.setRelVelStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]));
lead.setRelA(data[mdn_max_idx*MDN_GROUP_SIZE + 3]);
lead.setRelAStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]));
void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, float prob) {
lead.setProb(prob);
lead.setDist(data[0]);
lead.setStd(exp(data[LEAD_MHP_VALS]));
// TODO make all msgs same format
lead.setRelY(-data[1]);
lead.setRelYStd(exp(data[LEAD_MHP_VALS + 1]));
lead.setRelVel(data[2]);
lead.setRelVelStd(exp(data[LEAD_MHP_VALS + 2]));
lead.setRelA(data[3]);
lead.setRelAStd(exp(data[LEAD_MHP_VALS + 3]));
}
void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) {
kj::ArrayPtr<const float> desire_state(&meta_data[0], DESIRE_LEN);
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);
}
kj::ArrayPtr<const float> desire_state(desire_state_softmax, DESIRE_LEN);
meta.setDesireState(desire_state);
meta.setEngagedProb(meta_data[DESIRE_LEN]);
meta.setGasDisengageProb(meta_data[DESIRE_LEN + 1]);
meta.setBrakeDisengageProb(meta_data[DESIRE_LEN + 2]);
meta.setSteerOverrideProb(meta_data[DESIRE_LEN + 3]);
kj::ArrayPtr<const float> desire_pred(&meta_data[DESIRE_LEN + OTHER_META_SIZE], DESIRE_PRED_SIZE);
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]));
kj::ArrayPtr<const float> desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE);
meta.setDesirePrediction(desire_pred);
}
void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float * long_x_data, const float * long_v_data, const float * long_a_data) {
// just doing 10 vals, 1 every sec for now
float dist_arr[TIME_DISTANCE/10];
float speed_arr[TIME_DISTANCE/10];
float accel_arr[TIME_DISTANCE/10];
for (int i=0; i<TIME_DISTANCE/10; i++) {
dist_arr[i] = long_x_data[i*10];
speed_arr[i] = long_v_data[i*10];
accel_arr[i] = long_a_data[i*10];
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);
}
kj::ArrayPtr<const float> dist(&dist_arr[0], ARRAYSIZE(dist_arr));
longi.setDistances(dist);
kj::ArrayPtr<const float> speed(&speed_arr[0], ARRAYSIZE(speed_arr));
longi.setSpeeds(speed);
kj::ArrayPtr<const float> accel(&accel_arr[0], ARRAYSIZE(accel_arr));
longi.setAccelerations(accel);
kj::ArrayPtr<const float> desire_state(desire_state_softmax, DESIRE_LEN);
meta.setDesireState(desire_state);
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]));
kj::ArrayPtr<const float> desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE);
meta.setDesirePrediction(desire_pred);
}
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];
float y_arr[TRAJECTORY_SIZE];
float z_arr[TRAJECTORY_SIZE];
//float x_std_arr[TRAJECTORY_SIZE];
//float y_std_arr[TRAJECTORY_SIZE];
//float z_std_arr[TRAJECTORY_SIZE];
float t_arr[TRAJECTORY_SIZE];
for (int i=0; i<TRAJECTORY_SIZE; i++) {
// column_offset == -1 means this data is X indexed not T indexed
if (column_offset >= 0) {
t_arr[i] = T_IDXS[i];
x_arr[i] = data[i*columns + 0 + column_offset];
//x_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 0 + column_offset];
} else {
t_arr[i] = plan_t_arr[i];
x_arr[i] = X_IDXS[i];
//x_std_arr[i] = NAN;
}
y_arr[i] = data[i*columns + 1 + column_offset];
//y_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 1 + column_offset];
z_arr[i] = data[i*columns + 2 + column_offset];
//z_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 2 + column_offset];
}
kj::ArrayPtr<const float> x(x_arr, TRAJECTORY_SIZE);
kj::ArrayPtr<const float> y(y_arr, TRAJECTORY_SIZE);
kj::ArrayPtr<const float> z(z_arr, TRAJECTORY_SIZE);
//kj::ArrayPtr<const float> x_std(x_std_arr, TRAJECTORY_SIZE);
//kj::ArrayPtr<const float> y_std(y_std_arr, TRAJECTORY_SIZE);
//kj::ArrayPtr<const float> z_std(z_std_arr, TRAJECTORY_SIZE);
kj::ArrayPtr<const float> t(t_arr, TRAJECTORY_SIZE);
xyzt.setX(x);
xyzt.setY(y);
xyzt.setZ(z);
//xyzt.setXStd(x_std);
//xyzt.setYStd(y_std);
//xyzt.setZStd(z_std);
xyzt.setT(t);
}
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) {
// make msg
MessageBuilder msg;
auto framed = msg.initEvent(frame_drop < MAX_FRAME_DROP).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);
// 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 valid_len = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*32];
valid_len = fmin(MODEL_PATH_DISTANCE, fmax(MIN_VALID_LEN, valid_len));
int valid_len_idx = 0;
for (int i=1; i<TRAJECTORY_SIZE; i++) {
if (valid_len >= X_IDXS[valid_len_idx]){
valid_len_idx = i;
}
}
float * best_plan = &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)];
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];
}
auto position = framed.initPosition();
fill_xyzt(position, best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr);
auto orientation = framed.initOrientation();
fill_xyzt(orientation, best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr);
auto velocity = framed.initVelocity();
fill_xyzt(velocity, best_plan, PLAN_MHP_COLUMNS, 6, plan_t_arr);
auto orientation_rate = framed.initOrientationRate();
fill_xyzt(orientation_rate, best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr);
// lane lines
auto lane_lines = framed.initLaneLines(4);
float lane_line_probs_arr[4];
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);
lane_line_probs_arr[i] = sigmoid(net_outputs.lane_lines_prob[i]);
lane_line_stds_arr[i] = exp(net_outputs.lane_lines[2*TRAJECTORY_SIZE*(4 + i)]);
}
kj::ArrayPtr<const float> lane_line_probs(lane_line_probs_arr, 4);
framed.setLaneLineProbs(lane_line_probs);
framed.setLaneLineStds(lane_line_stds_arr);
// road edges
auto road_edges = framed.initRoadEdges(2);
float road_edge_stds_arr[2];
for (int i = 0; i < 2; i++) {
fill_xyzt(road_edges[i], &net_outputs.road_edges[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr);
road_edge_stds_arr[i] = exp(net_outputs.road_edges[2*TRAJECTORY_SIZE*(2 + i)]);
}
framed.setRoadEdgeStds(road_edge_stds_arr);
// meta
auto meta = framed.initMeta();
fill_meta_v2(meta, net_outputs.meta);
// leads
auto leads = framed.initLeads(LEAD_MHP_SELECTION);
int mdn_max_idx = 0;
float t_offsets[LEAD_MHP_SELECTION] = {0.0, 2.0, 4.0};
for (int t_offset=0; t_offset<LEAD_MHP_SELECTION; t_offset++) {
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[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,
@@ -243,29 +403,58 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
framed.setFrameDropPerc(frame_drop * 100);
framed.setTimestampEof(timestamp_eof);
fill_path(framed.initPath(), net_outputs.path, false, 0);
fill_path(framed.initLeftLane(), net_outputs.left_lane, true, 1.8);
fill_path(framed.initRightLane(), net_outputs.right_lane, true, -1.8);
fill_longi(framed.initLongitudinal(), net_outputs.long_x, net_outputs.long_v, net_outputs.long_a);
// 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;
}
}
// x pos at 10s is a good valid_len
float valid_len = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*32];
// clamp to 5 and MODEL_PATH_DISTANCE
valid_len = fmin(MODEL_PATH_DISTANCE, fmax(MIN_VALID_LEN, valid_len));
int valid_len_idx = 0;
for (int i=1; i<TRAJECTORY_SIZE; i++) {
if (valid_len >= X_IDXS[valid_len_idx]){
valid_len_idx = i;
}
}
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_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]) {
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, t_offset);
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_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]) {
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, t_offset);
fill_lead(framed.initLeadFuture(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[t_offset]));
fill_meta(framed.initMeta(), net_outputs.meta);
pm.send("model", msg);
@@ -280,10 +469,10 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
for (int i =0; i < 3; i++) {
trans_arr[i] = net_outputs.pose[i];
trans_std_arr[i] = softplus(net_outputs.pose[6 + i]) + 1e-6;
trans_std_arr[i] = exp(net_outputs.pose[6 + i]);
rot_arr[i] = M_PI * net_outputs.pose[3 + i] / 180.0;
rot_std_arr[i] = M_PI * (softplus(net_outputs.pose[9 + i]) + 1e-6) / 180.0;
rot_arr[i] = net_outputs.pose[3 + i];
rot_std_arr[i] = exp(net_outputs.pose[9 + i]);
}
MessageBuilder msg;
selfdrive/modeld/models/driving.h
+22 -13
View File
@@ -17,33 +17,40 @@
#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 MODEL_NAME "supercombo_dlc"
#define DESIRE_LEN 8
#define TRAFFIC_CONVENTION_LEN 2
#define LEAD_MDN_N 5 // probs for 5 groups
#define MDN_VALS 4 // output xyva for each lead group
#define SELECTION 3 //output 3 group (lead now, in 2s and 6s)
#define MDN_GROUP_SIZE 11
#define TIME_DISTANCE 100
#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
#define MAX_FRAME_DROP 0.05
struct ModelDataRaw {
float *path;
float *left_lane;
float *right_lane;
float *plan;
float *lane_lines;
float *lane_lines_prob;
float *road_edges;
float *lead;
float *long_x;
float *long_v;
float *long_a;
float *lead_prob;
float *desire_state;
float *meta;
float *desire_pred;
float *pose;
};
@@ -72,6 +79,8 @@ 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, uint64_t timestamp_eof);
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, uint64_t timestamp_eof);
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
selfdrive/modeld/runners/keras_runner.py
-49
View File
@@ -1,49 +0,0 @@
#!/usr/bin/env python3
# TODO: why are the keras models saved with python 2?
from __future__ import print_function
import tensorflow as tf # pylint: disable=import-error
import os
import sys
import numpy as np
from tensorflow.keras.models import load_model # pylint: disable=import-error
def read(sz):
dd = []
gt = 0
while gt < sz * 4:
st = os.read(0, sz * 4 - gt)
assert(len(st) > 0)
dd.append(st)
gt += len(st)
return np.frombuffer(b''.join(dd), dtype=np.float32)
def write(d):
os.write(1, d.tobytes())
def run_loop(m):
ishapes = [[1]+ii.shape[1:] for ii in m.inputs]
print("ready to run keras model", ishapes, file=sys.stderr)
while 1:
inputs = []
for shp in ishapes:
ts = np.product(shp)
#print("reshaping %s with offset %d" % (str(shp), offset), file=sys.stderr)
inputs.append(read(ts).reshape(shp))
ret = m.predict_on_batch(inputs)
#print(ret, file=sys.stderr)
for r in ret:
write(r)
if __name__ == "__main__":
print(tf.__version__, file=sys.stderr)
# limit gram alloc
gpus = tf.config.experimental.list_physical_devices('GPU')
if len(gpus) > 0:
tf.config.experimental.set_virtual_device_configuration(gpus[0], [tf.config.experimental.VirtualDeviceConfiguration(memory_limit=1024)])
m = load_model(sys.argv[1])
print(m, file=sys.stderr)
run_loop(m)
selfdrive/modeld/runners/onnx_runner.py
Executable
+56
View File
@@ -0,0 +1,56 @@
#!/usr/bin/env python3
# TODO: why are the keras models saved with python 2?
from __future__ import print_function
import os
import sys
import numpy as np
import onnxruntime as ort
def read(sz):
dd = []
gt = 0
while gt < sz * 4:
st = os.read(0, sz * 4 - gt)
assert(len(st) > 0)
dd.append(st)
gt += len(st)
return np.frombuffer(b''.join(dd), dtype=np.float32)
def write(d):
os.write(1, d.tobytes())
def run_loop(m):
ishapes = [[1]+ii.shape[1:] for ii in m.get_inputs()]
keys = [x.name for x in m.get_inputs()]
print("ready to run onnx model", keys, ishapes, file=sys.stderr)
while 1:
inputs = []
for shp in ishapes:
ts = np.product(shp)
#print("reshaping %s with offset %d" % (str(shp), offset), file=sys.stderr)
inputs.append(read(ts).reshape(shp))
ret = m.run(None, dict(zip(keys, inputs)))
#print(ret, file=sys.stderr)
for r in ret:
write(r)
if __name__ == "__main__":
print(ort.get_available_providers(), file=sys.stderr)
if 'OpenVINOExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ:
print("OnnxJit is using openvino", file=sys.stderr)
options = ort.SessionOptions()
options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_DISABLE_ALL
provider = 'OpenVINOExecutionProvider'
else:
print("OnnxJit is using CPU", file=sys.stderr)
options = ort.SessionOptions()
options.intra_op_num_threads = 4
options.inter_op_num_threads = 8
provider = 'CPUExecutionProvider'
ort_session = ort.InferenceSession(sys.argv[1], options)
ort_session.set_providers([provider], None)
run_loop(ort_session)
selfdrive/modeld/runners/tfmodel.cc → selfdrive/modeld/runners/onnxmodel.cc
+24 -17
View File
@@ -1,7 +1,8 @@
#include "tfmodel.h"
#include "onnxmodel.h"
#include <stdio.h>
#include <string>
#include <string.h>
#include <poll.h>
#include <signal.h>
#include <unistd.h>
#include <stdlib.h>
@@ -12,33 +13,33 @@
#include <cassert>
TFModel::TFModel(const char *path, float *_output, size_t _output_size, int runtime) {
ONNXModel::ONNXModel(const char *path, float *_output, size_t _output_size, int runtime) {
output = _output;
output_size = _output_size;
char tmp[1024];
strncpy(tmp, path, sizeof(tmp));
strstr(tmp, ".dlc")[0] = '\0';
strcat(tmp, ".keras");
strcat(tmp, ".onnx");
LOGD("loading model %s", tmp);
assert(pipe(pipein) == 0);
assert(pipe(pipeout) == 0);
std::string exe_dir = util::dir_name(util::readlink("/proc/self/exe"));
std::string keras_runner = exe_dir + "/runners/keras_runner.py";
std::string onnx_runner = exe_dir + "/runners/onnx_runner.py";
proc_pid = fork();
if (proc_pid == 0) {
LOGD("spawning keras process %s", keras_runner.c_str());
char *argv[] = {(char*)keras_runner.c_str(), tmp, NULL};
LOGD("spawning onnx process %s", onnx_runner.c_str());
char *argv[] = {(char*)onnx_runner.c_str(), tmp, NULL};
dup2(pipein[0], 0);
dup2(pipeout[1], 1);
close(pipein[0]);
close(pipein[1]);
close(pipeout[0]);
close(pipeout[1]);
execvp(keras_runner.c_str(), argv);
execvp(onnx_runner.c_str(), argv);
}
// parent
@@ -46,13 +47,13 @@ TFModel::TFModel(const char *path, float *_output, size_t _output_size, int runt
close(pipeout[1]);
}
TFModel::~TFModel() {
ONNXModel::~ONNXModel() {
close(pipein[1]);
close(pipeout[0]);
kill(proc_pid, SIGTERM);
}
void TFModel::pwrite(float *buf, int size) {
void ONNXModel::pwrite(float *buf, int size) {
char *cbuf = (char *)buf;
int tw = size*sizeof(float);
while (tw > 0) {
@@ -65,35 +66,41 @@ void TFModel::pwrite(float *buf, int size) {
LOGD("host write of size %d done", size);
}
void TFModel::pread(float *buf, int size) {
void ONNXModel::pread(float *buf, int size) {
char *cbuf = (char *)buf;
int tr = size*sizeof(float);
struct pollfd fds[1];
fds[0].fd = pipeout[0];
fds[0].events = POLLIN;
while (tr > 0) {
LOGD("host read remaining %d/%d", tr, size*sizeof(float));
int err = read(pipeout[0], cbuf, tr);
assert(err >= 0);
int err;
err = poll(fds, 1, 10000); // 10 second timeout
assert(err == 1 || (err == -1 && errno == EINTR));
LOGD("host read remaining %d/%d poll %d", tr, size*sizeof(float), err);
err = read(pipeout[0], cbuf, tr);
assert(err > 0 || (err == 0 && errno == EINTR));
cbuf += err;
tr -= err;
}
LOGD("host read done");
}
void TFModel::addRecurrent(float *state, int state_size) {
void ONNXModel::addRecurrent(float *state, int state_size) {
rnn_input_buf = state;
rnn_state_size = state_size;
}
void TFModel::addDesire(float *state, int state_size) {
void ONNXModel::addDesire(float *state, int state_size) {
desire_input_buf = state;
desire_state_size = state_size;
}
void TFModel::addTrafficConvention(float *state, int state_size) {
void ONNXModel::addTrafficConvention(float *state, int state_size) {
traffic_convention_input_buf = state;
traffic_convention_size = state_size;
}
void TFModel::execute(float *net_input_buf, int buf_size) {
void ONNXModel::execute(float *net_input_buf, int buf_size) {
// order must be this
pwrite(net_input_buf, buf_size);
if (desire_input_buf != NULL) {
selfdrive/modeld/runners/tfmodel.h → selfdrive/modeld/runners/onnxmodel.h
+5 -7
View File
@@ -1,15 +1,13 @@
#ifndef TFMODEL_H
#define TFMODEL_H
#ifndef ONNXMODEL_H
#define ONNXMODEL_H
#include <stdlib.h>
#include "runmodel.h"
struct TFState;
class TFModel : public RunModel {
class ONNXModel : public RunModel {
public:
TFModel(const char *path, float *output, size_t output_size, int runtime);
~TFModel();
ONNXModel(const char *path, float *output, size_t output_size, int runtime);
~ONNXModel();
void addRecurrent(float *state, int state_size);
void addDesire(float *state, int state_size);
void addTrafficConvention(float *state, int state_size);
selfdrive/modeld/runners/run.h
+3 -3
View File
@@ -7,9 +7,9 @@
#ifdef QCOM
#define DefaultRunModel SNPEModel
#else
#ifdef USE_TF_MODEL
#include "tfmodel.h"
#define DefaultRunModel TFModel
#ifdef USE_ONNX_MODEL
#include "onnxmodel.h"
#define DefaultRunModel ONNXModel
#else
#define DefaultRunModel SNPEModel
#endif
selfdrive/modeld/transforms/transform.c → selfdrive/modeld/transforms/transform.cc
+2 -2
View File
@@ -99,14 +99,14 @@ void transform_queue(Transform* s,
err = clSetKernelArg(s->krnl, 10, sizeof(cl_mem), &s->m_y_cl);
assert(err == 0);
const size_t work_size_y[2] = {out_y_width, out_y_height};
const size_t work_size_y[2] = {(size_t)out_y_width, (size_t)out_y_height};
err = clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
(const size_t*)&work_size_y, NULL, 0, 0, NULL);
assert(err == 0);
const size_t work_size_uv[2] = {out_uv_width, out_uv_height};
const size_t work_size_uv[2] = {(size_t)out_uv_width, (size_t)out_uv_height};
err = clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_uv_width);
assert(err == 0);
selfdrive/modeld/visiontest.mk
+1 -1
View File
@@ -48,7 +48,7 @@ endif
all: visiontest
libvisiontest_inputs := visiontest.c \
transforms/transform.c \
transforms/transform.cc \
transforms/loadyuv.c \
../common/clutil.c \
$(BASEDIR)/selfdrive/common/util.c \
selfdrive/test/process_replay/model_replay_ref_commit
+1 -1
View File
@@ -1 +1 @@
734ee7118a5bcab4accdd73ae8fdc03df104157a
f67a4ac1f387514a42c9f4d890495c4872288a63
selfdrive/test/test_cpu_usage.py
+1 -1
View File
@@ -16,7 +16,7 @@ def cputime_total(ct):
def print_cpu_usage(first_proc, last_proc):
procs = [
("selfdrive.controls.controlsd", 47.0),
("./loggerd", 38.0),
("./loggerd", 42.0),
("selfdrive.locationd.locationd", 35.0),
("selfdrive.locationd.paramsd", 12.0),
("selfdrive.controls.plannerd", 10.0),
selfdrive/ui/paint.cc
+81 -168
View File
@@ -4,6 +4,8 @@
#include <cmath>
#include <iostream>
#include "common/util.h"
#include <algorithm>
#define NANOVG_GLES3_IMPLEMENTATION
#include "nanovg_gl.h"
@@ -37,26 +39,23 @@ const mat3 intrinsic_matrix = (mat3){{
// Projects a point in car to space to the corresponding point in full frame
// image space.
vec3 car_space_to_full_frame(const UIState *s, vec4 car_space_projective) {
const UIScene *scene = &s->scene;
bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, float *out_x, float *out_y) {
const vec4 car_space_projective = (vec4){{in_x, in_y, in_z, 1.}};
// We'll call the car space point p.
// First project into normalized image coordinates with the extrinsics matrix.
const vec4 Ep4 = matvecmul(scene->extrinsic_matrix, car_space_projective);
const vec4 Ep4 = matvecmul(s->scene.extrinsic_matrix, car_space_projective);
// The last entry is zero because of how we store E (to use matvecmul).
const vec3 Ep = {{Ep4.v[0], Ep4.v[1], Ep4.v[2]}};
const vec3 KEp = matvecmul3(intrinsic_matrix, Ep);
// Project.
const vec3 p_image = {{KEp.v[0] / KEp.v[2], KEp.v[1] / KEp.v[2], 1.}};
return p_image;
*out_x = KEp.v[0] / KEp.v[2];
*out_y = KEp.v[1] / KEp.v[2];
return *out_x >= 0 && *out_y >= 0;
}
// Calculate an interpolation between two numbers at a specific increment
static float lerp(float v0, float v1, float t) {
return (1 - t) * v0 + t * v1;
}
static void ui_draw_text(NVGcontext *vg, float x, float y, const char* string, float size, NVGcolor color, int font){
nvgFontFaceId(vg, font);
@@ -67,12 +66,8 @@ static void ui_draw_text(NVGcontext *vg, float x, float y, const char* string, f
static void draw_chevron(UIState *s, float x_in, float y_in, float sz,
NVGcolor fillColor, NVGcolor glowColor) {
const vec4 p_car_space = (vec4){{x_in, y_in, 0., 1.}};
const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
float x = p_full_frame.v[0];
float y = p_full_frame.v[1];
if (x < 0 || y < 0.){
float x, y;
if (!car_space_to_full_frame(s, x_in, y_in, 0.0, &x, &y)) {
return;
}
@@ -134,110 +129,52 @@ static void draw_lead(UIState *s, const cereal::RadarState::LeadData::Reader &le
draw_chevron(s, d_rel, lead.getYRel(), 25, nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW);
}
static void ui_draw_lane_line(UIState *s, const model_path_vertices_data *pvd, NVGcolor color) {
if (pvd->cnt == 0) return;
static void ui_draw_line(UIState *s, const vertex_data *v, const int cnt, NVGcolor *color, NVGpaint *paint) {
if (cnt == 0) return;
nvgBeginPath(s->vg);
nvgMoveTo(s->vg, pvd->v[0].x, pvd->v[0].y);
for (int i=1; i<pvd->cnt; i++) {
nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y);
nvgMoveTo(s->vg, std::clamp<float>(v[0].x, 0, s->fb_w), std::clamp<float>(v[0].y, 0, s->fb_h));
for (int i = 1; i < cnt; i++) {
nvgLineTo(s->vg, std::clamp<float>(v[i].x, 0, s->fb_w), std::clamp<float>(v[i].y, 0, s->fb_h));
}
nvgClosePath(s->vg);
nvgFillColor(s->vg, color);
if (color) {
nvgFillColor(s->vg, *color);
} else if (paint) {
nvgFillPaint(s->vg, *paint);
}
nvgFill(s->vg);
}
static void update_track_data(UIState *s, bool is_mpc, track_vertices_data *pvd) {
static void update_track_data(UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, track_vertices_data *pvd) {
const UIScene *scene = &s->scene;
const float *points = scene->path_points;
const float *mpc_x_coords = &scene->mpc_x[0];
const float *mpc_y_coords = &scene->mpc_y[0];
float off = is_mpc?0.3:0.5;
float lead_d = scene->lead_data[0].getDRel()*2.;
float path_height = is_mpc?(lead_d>5.)?fmin(lead_d, 25.)-fmin(lead_d*0.35, 10.):20.
:(lead_d>0.)?fmin(lead_d, 50.)-fmin(lead_d*0.35, 10.):49.;
path_height = fmin(path_height, scene->model.getPath().getValidLen());
pvd->cnt = 0;
// left side up
for (int i=0; i<=path_height; i++) {
float px, py, mpx;
if (is_mpc) {
mpx = i==0?0.0:mpc_x_coords[i];
px = lerp(mpx+1.0, mpx, i/100.0);
py = mpc_y_coords[i] - off;
} else {
px = lerp(i+1.0, i, i/100.0);
py = points[i] - off;
}
vec4 p_car_space = (vec4){{px, py, 0., 1.}};
vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
if (p_full_frame.v[0] < 0. || p_full_frame.v[1] < 0.) {
continue;
}
pvd->v[pvd->cnt].x = p_full_frame.v[0];
pvd->v[pvd->cnt].y = p_full_frame.v[1];
pvd->cnt += 1;
}
// right side down
for (int i=path_height; i>=0; i--) {
float px, py, mpx;
if (is_mpc) {
mpx = i==0?0.0:mpc_x_coords[i];
px = lerp(mpx+1.0, mpx, i/100.0);
py = mpc_y_coords[i] + off;
} else {
px = lerp(i+1.0, i, i/100.0);
py = points[i] + off;
}
vec4 p_car_space = (vec4){{px, py, 0., 1.}};
vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
if (p_full_frame.v[0] < 0. || p_full_frame.v[1] < 0.) {
continue;
}
pvd->v[pvd->cnt].x = p_full_frame.v[0];
pvd->v[pvd->cnt].y = p_full_frame.v[1];
pvd->cnt += 1;
}
}
static void update_all_track_data(UIState *s) {
const UIScene *scene = &s->scene;
// Draw vision path
update_track_data(s, false, &s->track_vertices[0]);
if (scene->controls_state.getEnabled()) {
// Draw MPC path when engaged
update_track_data(s, true, &s->track_vertices[1]);
}
}
static void ui_draw_track(UIState *s, bool is_mpc, track_vertices_data *pvd) {
if (pvd->cnt == 0) return;
nvgBeginPath(s->vg);
nvgMoveTo(s->vg, pvd->v[0].x, pvd->v[0].y);
for (int i=1; i<pvd->cnt; i++) {
nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y);
}
nvgClosePath(s->vg);
NVGpaint track_bg;
if (is_mpc) {
// Draw colored MPC track
const NVGcolor clr = bg_colors[s->status];
track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h*.4,
nvgRGBA(clr.r, clr.g, clr.b, 255), nvgRGBA(clr.r, clr.g, clr.b, 255/2));
const float off = 0.5;
int max_idx;
float lead_d;
if(s->sm->updated("radarState")) {
lead_d = scene->lead_data[0].getDRel()*2.;
} else {
// Draw white vision track
track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h*.4,
COLOR_WHITE, COLOR_WHITE_ALPHA(0));
lead_d = MAX_DRAW_DISTANCE;
}
nvgFillPaint(s->vg, track_bg);
nvgFill(s->vg);
float path_length = (lead_d>0.)?lead_d-fmin(lead_d*0.35, 10.):MAX_DRAW_DISTANCE;
path_length = fmin(path_length, scene->max_distance);
vertex_data *v = &pvd->v[0];
for (int i = 0; line.getX()[i] <= path_length and i < TRAJECTORY_SIZE; i++) {
v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i], &v->x, &v->y);
max_idx = i;
}
for (int i = max_idx; i >= 0; i--) {
v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i], &v->x, &v->y);
}
pvd->cnt = v - pvd->v;
}
static void ui_draw_track(UIState *s, track_vertices_data *pvd) {
NVGpaint track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4,
COLOR_WHITE, COLOR_WHITE_ALPHA(0));
ui_draw_line(s, &pvd->v[0], pvd->cnt, nullptr, &track_bg);
}
static void draw_frame(UIState *s) {
@@ -271,72 +208,48 @@ static void draw_frame(UIState *s) {
glBindVertexArray(0);
}
static inline bool valid_frame_pt(UIState *s, float x, float y) {
return x >= 0 && x <= s->stream.bufs_info.width && y >= 0 && y <= s->stream.bufs_info.height;
}
static void update_lane_line_data(UIState *s, const float *points, float off, model_path_vertices_data *pvd, float valid_len) {
pvd->cnt = 0;
int rcount = fmin(MODEL_PATH_MAX_VERTICES_CNT / 2, valid_len);
for (int i = 0; i < rcount; i++) {
float px = (float)i;
float py = points[i] - off;
const vec4 p_car_space = (vec4){{px, py, 0., 1.}};
const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
if(!valid_frame_pt(s, p_full_frame.v[0], p_full_frame.v[1]))
continue;
pvd->v[pvd->cnt].x = p_full_frame.v[0];
pvd->v[pvd->cnt].y = p_full_frame.v[1];
pvd->cnt += 1;
static void update_line_data(UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, float off, line_vertices_data *pvd, float max_distance) {
// TODO check that this doesn't overflow max vertex buffer
int max_idx;
vertex_data *v = &pvd->v[0];
for (int i = 0; ((i < TRAJECTORY_SIZE) and (line.getX()[i] < fmax(MIN_DRAW_DISTANCE, max_distance))); i++) {
v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i] + 1.22, &v->x, &v->y);
max_idx = i;
}
for (int i = rcount - 1; i > 0; i--) {
float px = (float)i;
float py = points[i] + off;
const vec4 p_car_space = (vec4){{px, py, 0., 1.}};
const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
if(!valid_frame_pt(s, p_full_frame.v[0], p_full_frame.v[1]))
continue;
pvd->v[pvd->cnt].x = p_full_frame.v[0];
pvd->v[pvd->cnt].y = p_full_frame.v[1];
pvd->cnt += 1;
for (int i = max_idx - 1; i > 0; i--) {
v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i] + 1.22, &v->x, &v->y);
}
pvd->cnt = v - pvd->v;
}
static void update_all_lane_lines_data(UIState *s, const cereal::ModelData::PathData::Reader &path, const float *points, model_path_vertices_data *pstart) {
update_lane_line_data(s, points, 0.025*path.getProb(), pstart, path.getValidLen());
update_lane_line_data(s, points, fmin(path.getStd(), 0.7), pstart + 1, path.getValidLen());
}
static void ui_draw_lane(UIState *s, model_path_vertices_data *pstart, NVGcolor color) {
ui_draw_lane_line(s, pstart, color);
color.a /= 25;
ui_draw_lane_line(s, pstart + 1, color);
}
static void ui_draw_vision_lanes(UIState *s) {
static void ui_draw_vision_lane_lines(UIState *s) {
const UIScene *scene = &s->scene;
model_path_vertices_data *pvd = &s->model_path_vertices[0];
if(s->sm->updated("model")) {
update_all_lane_lines_data(s, scene->model.getLeftLane(), scene->left_lane_points, pvd);
update_all_lane_lines_data(s, scene->model.getRightLane(), scene->right_lane_points, pvd + MODEL_LANE_PATH_CNT);
// paint lanelines
line_vertices_data *pvd_ll = &s->lane_line_vertices[0];
for (int ll_idx = 0; ll_idx < 4; ll_idx++) {
if(s->sm->updated("modelV2")) {
update_line_data(s, scene->model.getLaneLines()[ll_idx], 0.025*scene->model.getLaneLineProbs()[ll_idx], pvd_ll + ll_idx, scene->max_distance);
}
NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene->lane_line_probs[ll_idx]);
ui_draw_line(s, (pvd_ll + ll_idx)->v, (pvd_ll + ll_idx)->cnt, &color, nullptr);
}
// Draw left lane edge
ui_draw_lane(s, pvd, nvgRGBAf(1.0, 1.0, 1.0, scene->model.getLeftLane().getProb()));
// Draw right lane edge
ui_draw_lane(s, pvd + MODEL_LANE_PATH_CNT, nvgRGBAf(1.0, 1.0, 1.0, scene->model.getRightLane().getProb()));
if(s->sm->updated("radarState")) {
update_all_track_data(s);
// paint road edges
line_vertices_data *pvd_re = &s->road_edge_vertices[0];
for (int re_idx = 0; re_idx < 2; re_idx++) {
if(s->sm->updated("modelV2")) {
update_line_data(s, scene->model.getRoadEdges()[re_idx], 0.025, pvd_re + re_idx, scene->max_distance);
}
NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp<float>(1.0-scene->road_edge_stds[re_idx], 0.0, 1.0));
ui_draw_line(s, (pvd_re + re_idx)->v, (pvd_re + re_idx)->cnt, &color, nullptr);
}
// Draw vision path
ui_draw_track(s, false, &s->track_vertices[0]);
if (scene->controls_state.getEnabled()) {
// Draw MPC path when engaged
ui_draw_track(s, true, &s->track_vertices[1]);
// paint path
if(s->sm->updated("modelV2")) {
update_track_data(s, scene->model.getPosition(), &s->track_vertices);
}
ui_draw_track(s, &s->track_vertices);
}
// Draw all world space objects.
@@ -359,7 +272,7 @@ static void ui_draw_world(UIState *s) {
nvgTranslate(s->vg, -intrinsic_matrix.v[2], -intrinsic_matrix.v[5]);
// Draw lane edges and vision/mpc tracks
ui_draw_vision_lanes(s);
ui_draw_vision_lane_lines(s);
// Draw lead indicators if openpilot is handling longitudinal
if (s->longitudinal_control) {
selfdrive/ui/ui.cc
+20 -13
View File
@@ -1,4 +1,5 @@
#include <stdio.h>
#include <cmath>
#include <stdlib.h>
#include <stdbool.h>
#include <signal.h>
@@ -24,7 +25,7 @@ int write_param_float(float param, const char* param_name, bool persistent_param
}
void ui_init(UIState *s) {
s->sm = new SubMaster({"model", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal",
s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal",
"health", "carParams", "ubloxGnss", "driverState", "dMonitoringState", "sensorEvents"});
s->started = false;
@@ -106,13 +107,6 @@ destroy:
s->vision_connected = false;
}
static inline void fill_path_points(const cereal::ModelData::PathData::Reader &path, float *points) {
const capnp::List<float>::Reader &poly = path.getPoly();
for (int i = 0; i < path.getValidLen(); i++) {
points[i] = poly[0] * (i * i * i) + poly[1] * (i * i) + poly[2] * i + poly[3];
}
}
void update_sockets(UIState *s) {
UIScene &scene = s->scene;
@@ -178,11 +172,24 @@ void update_sockets(UIState *s) {
scene.extrinsic_matrix.v[i] = extrinsicl[i];
}
}
if (sm.updated("model")) {
scene.model = sm["model"].getModel();
fill_path_points(scene.model.getPath(), scene.path_points);
fill_path_points(scene.model.getLeftLane(), scene.left_lane_points);
fill_path_points(scene.model.getRightLane(), scene.right_lane_points);
if (sm.updated("modelV2")) {
scene.model = sm["modelV2"].getModelV2();
scene.max_distance = fmin(scene.model.getPosition().getX()[TRAJECTORY_SIZE - 1], MAX_DRAW_DISTANCE);
for (int ll_idx = 0; ll_idx < 4; ll_idx++) {
if (scene.model.getLaneLineProbs().size() > ll_idx) {
scene.lane_line_probs[ll_idx] = scene.model.getLaneLineProbs()[ll_idx];
} else {
scene.lane_line_probs[ll_idx] = 0.0;
}
}
for (int re_idx = 0; re_idx < 2; re_idx++) {
if (scene.model.getRoadEdgeStds().size() > re_idx) {
scene.road_edge_stds[re_idx] = scene.model.getRoadEdgeStds()[re_idx];
} else {
scene.road_edge_stds[re_idx] = 1.0;
}
}
}
if (sm.updated("uiLayoutState")) {
auto data = sm["uiLayoutState"].getUiLayoutState();
selfdrive/ui/ui.hpp
+24 -13
View File
@@ -55,9 +55,8 @@ const Rect home_btn = {60, 1080 - 180 - 40, 180, 180};
const int UI_FREQ = 20; // Hz
const int MODEL_PATH_MAX_VERTICES_CNT = 98;
const int MODEL_LANE_PATH_CNT = 2;
const int TRACK_POINTS_MAX_CNT = 50 * 2;
const int MODEL_PATH_MAX_VERTICES_CNT = TRAJECTORY_SIZE*2;
const int TRACK_POINTS_MAX_CNT = TRAJECTORY_SIZE*4;
const int SET_SPEED_NA = 255;
@@ -83,10 +82,14 @@ static std::map<UIStatus, NVGcolor> bg_colors = {
{STATUS_ALERT, nvgRGBA(0xC9, 0x22, 0x31, 0xf1)},
};
typedef struct UIScene {
typedef struct {
float x[TRAJECTORY_SIZE];
float y[TRAJECTORY_SIZE];
float z[TRAJECTORY_SIZE];
} line;
float mpc_x[50];
float mpc_y[50];
typedef struct UIScene {
mat4 extrinsic_matrix; // Last row is 0 so we can use mat4.
bool world_objects_visible;
@@ -112,10 +115,17 @@ typedef struct UIScene {
cereal::ControlsState::Reader controls_state;
cereal::DriverState::Reader driver_state;
cereal::DMonitoringState::Reader dmonitoring_state;
cereal::ModelData::Reader model;
float left_lane_points[MODEL_PATH_DISTANCE];
float path_points[MODEL_PATH_DISTANCE];
float right_lane_points[MODEL_PATH_DISTANCE];
cereal::ModelDataV2::Reader model;
line path;
line outer_left_lane_line;
line left_lane_line;
line right_lane_line;
line outer_right_lane_line;
line left_road_edge;
line right_road_edge;
float max_distance;
float lane_line_probs[4];
float road_edge_stds[2];
} UIScene;
typedef struct {
@@ -125,7 +135,7 @@ typedef struct {
typedef struct {
vertex_data v[MODEL_PATH_MAX_VERTICES_CNT];
int cnt;
} model_path_vertices_data;
} line_vertices_data;
typedef struct {
vertex_data v[TRACK_POINTS_MAX_CNT];
@@ -190,8 +200,9 @@ typedef struct UIState {
bool alert_blinked;
float alert_blinking_alpha;
track_vertices_data track_vertices[2];
model_path_vertices_data model_path_vertices[MODEL_LANE_PATH_CNT * 2];
track_vertices_data track_vertices;
line_vertices_data lane_line_vertices[4];
line_vertices_data road_edge_vertices[2];
Rect video_rect;
} UIState;