diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index cfc71a0e27..0a8a8d6358 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -15,6 +15,7 @@ #include "common/util.h" #include "system/hardware/hw.h" #include "selfdrive/modeld/models/driving.h" +#include "selfdrive/modeld/models/nav.h" ExitHandler do_exit; @@ -72,6 +73,8 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl mat3 model_transform_main = {}; mat3 model_transform_extra = {}; bool live_calib_seen = false; + float driving_style[DRIVING_STYLE_LEN] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0}; + float nav_features[NAV_FEATURE_LEN] = {0}; VisionBuf *buf_main = nullptr; VisionBuf *buf_extra = nullptr; @@ -151,7 +154,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl } double mt1 = millis_since_boot(); - ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, is_rhd, prepare_only); + ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, is_rhd, driving_style, nav_features, prepare_only); double mt2 = millis_since_boot(); float model_execution_time = (mt2 - mt1) / 1000.0; diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 9cb216ff19..ac101bfee7 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -46,10 +46,19 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) { #ifdef TRAFFIC_CONVENTION s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN); #endif + +#ifdef DRIVING_STYLE + s->m->addDrivingStyle(s->driving_style, DRIVING_STYLE_LEN); +#endif + +#ifdef NAV + s->m->addNavFeatures(s->nav_features, NAV_FEATURE_LEN); +#endif + } ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, - const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only) { + const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only) { #ifdef DESIRE std::memmove(&s->pulse_desire[0], &s->pulse_desire[DESIRE_LEN], sizeof(float) * DESIRE_LEN*HISTORY_BUFFER_LEN); if (desire_in != NULL) { @@ -67,6 +76,14 @@ ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, LOGT("Desire enqueued"); #endif +#ifdef NAV + std::memcpy(s->nav_features, nav_features, sizeof(float)*NAV_FEATURE_LEN); +#endif + +#ifdef DRIVING_STYLE + std::memcpy(s->driving_style, driving_style, sizeof(float)*DRIVING_STYLE_LEN); +#endif + int rhd_idx = is_rhd; s->traffic_convention[rhd_idx] = 1.0; s->traffic_convention[1-rhd_idx] = 0.0; diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index b23691a56a..5734c68cb9 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -14,6 +14,7 @@ #include "common/modeldata.h" #include "common/util.h" #include "selfdrive/modeld/models/commonmodel.h" +#include "selfdrive/modeld/models/nav.h" #include "selfdrive/modeld/runners/run.h" constexpr int FEATURE_LEN = 128; @@ -21,6 +22,7 @@ constexpr int HISTORY_BUFFER_LEN = 99; constexpr int DESIRE_LEN = 8; constexpr int DESIRE_PRED_LEN = 4; constexpr int TRAFFIC_CONVENTION_LEN = 2; +constexpr int DRIVING_STYLE_LEN = 12; constexpr int MODEL_FREQ = 20; constexpr int DISENGAGE_LEN = 5; @@ -259,11 +261,17 @@ struct ModelState { #ifdef TRAFFIC_CONVENTION float traffic_convention[TRAFFIC_CONVENTION_LEN] = {}; #endif +#ifdef DRIVING_STYLE + float driving_style[DRIVING_STYLE_LEN] = {}; +#endif +#ifdef NAV + float nav_features[NAV_FEATURE_LEN] = {}; +#endif }; void model_init(ModelState* s, cl_device_id device_id, cl_context context); ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide, - const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only); + const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only); void model_free(ModelState* s); void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop, const ModelOutput &net_outputs, uint64_t timestamp_eof, diff --git a/selfdrive/modeld/models/nav.cc b/selfdrive/modeld/models/nav.cc index dae87c7ce5..861795e172 100644 --- a/selfdrive/modeld/models/nav.cc +++ b/selfdrive/modeld/models/nav.cc @@ -9,11 +9,11 @@ void navmodel_init(NavModelState* s) { -#ifdef USE_ONNX_MODEL - s->m = new ONNXModel("models/navmodel.onnx", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, false, true); -#else - s->m = new SNPEModel("models/navmodel_q.dlc", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, false, true); -#endif + #ifdef USE_ONNX_MODEL + s->m = new ONNXModel("models/navmodel.onnx", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, false, true); + #else + s->m = new SNPEModel("models/navmodel_q.dlc", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, false, true); + #endif } NavModelResult* navmodel_eval_frame(NavModelState* s, VisionBuf* buf) { @@ -56,7 +56,7 @@ void navmodel_publish(PubMaster &pm, uint32_t frame_id, const NavModelResult &mo framed.setDspExecutionTime(model_res.dsp_execution_time); framed.setFeatures(to_kj_array_ptr(model_res.features.values)); framed.setDesirePrediction(to_kj_array_ptr(model_res.desire_pred.values)); - fill_plan(framed, model_res.plans.get_best_prediction()); + fill_plan(framed, model_res.plan); pm.send("navModel", msg); } diff --git a/selfdrive/modeld/models/nav.h b/selfdrive/modeld/models/nav.h index b469f75987..b2955ad2c5 100644 --- a/selfdrive/modeld/models/nav.h +++ b/selfdrive/modeld/models/nav.h @@ -10,7 +10,6 @@ constexpr int NAV_INPUT_SIZE = 256*256; constexpr int NAV_FEATURE_LEN = 64; constexpr int NAV_DESIRE_LEN = 32; -constexpr int NAV_PLAN_MHP_N = 5; struct NavModelOutputXY { float x; @@ -21,24 +20,8 @@ static_assert(sizeof(NavModelOutputXY) == sizeof(float)*2); struct NavModelOutputPlan { std::array mean; std::array std; - float prob; }; -static_assert(sizeof(NavModelOutputPlan) == sizeof(NavModelOutputXY)*TRAJECTORY_SIZE*2 + sizeof(float)); - -struct NavModelOutputPlans { - std::array predictions; - - constexpr const NavModelOutputPlan &get_best_prediction() const { - int max_idx = 0; - for (int i = 1; i < predictions.size(); i++) { - if (predictions[i].prob > predictions[max_idx].prob) { - max_idx = i; - } - } - return predictions[max_idx]; - } -}; -static_assert(sizeof(NavModelOutputPlans) == sizeof(NavModelOutputPlan)*NAV_PLAN_MHP_N); +static_assert(sizeof(NavModelOutputPlan) == sizeof(NavModelOutputXY)*TRAJECTORY_SIZE*2); struct NavModelOutputDesirePrediction { std::array values; @@ -51,12 +34,12 @@ struct NavModelOutputFeatures { static_assert(sizeof(NavModelOutputFeatures) == sizeof(float)*NAV_FEATURE_LEN); struct NavModelResult { - const NavModelOutputPlans plans; + const NavModelOutputPlan plan; const NavModelOutputDesirePrediction desire_pred; const NavModelOutputFeatures features; float dsp_execution_time; }; -static_assert(sizeof(NavModelResult) == sizeof(NavModelOutputPlans) + sizeof(NavModelOutputDesirePrediction) + sizeof(NavModelOutputFeatures) + sizeof(float)); +static_assert(sizeof(NavModelResult) == sizeof(NavModelOutputPlan) + sizeof(NavModelOutputDesirePrediction) + sizeof(NavModelOutputFeatures) + sizeof(float)); constexpr int NAV_OUTPUT_SIZE = sizeof(NavModelResult) / sizeof(float); constexpr int NAV_NET_OUTPUT_SIZE = NAV_OUTPUT_SIZE - 1; diff --git a/selfdrive/modeld/models/navmodel.onnx b/selfdrive/modeld/models/navmodel.onnx index 60dd8c0e7f..397b8d90e2 100644 --- a/selfdrive/modeld/models/navmodel.onnx +++ b/selfdrive/modeld/models/navmodel.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:eab4b986e14d7d842d6d5487011c329d356fb56995b2ae7dc7188aefe6df9d97 -size 12285002 +oid sha256:9028b36a591763724e95205b48f37f09260b4434fb1f3c6f228db1710a401aa8 +size 12258591 diff --git a/selfdrive/modeld/models/navmodel_q.dlc b/selfdrive/modeld/models/navmodel_q.dlc index 7d9b36ed4d..984cf0a553 100644 --- a/selfdrive/modeld/models/navmodel_q.dlc +++ b/selfdrive/modeld/models/navmodel_q.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:83d53efc40053b02fe7d3da4ef6213a4a5a1ae4d1bd49c121b9beb6a54ea1148 -size 3154868 +oid sha256:bc7ade56bb4f9525c84a46df22252ea1e321a0518cbcef8bdfc76ccd8ad10b41 +size 3154304 diff --git a/selfdrive/modeld/runners/onnxmodel.cc b/selfdrive/modeld/runners/onnxmodel.cc index 447d90fd7e..5616a6c9c7 100644 --- a/selfdrive/modeld/runners/onnxmodel.cc +++ b/selfdrive/modeld/runners/onnxmodel.cc @@ -97,6 +97,16 @@ void ONNXModel::addDesire(float *state, int state_size) { desire_state_size = state_size; } +void ONNXModel::addNavFeatures(float *state, int state_size) { + nav_features_input_buf = state; + nav_features_size = state_size; +} + +void ONNXModel::addDrivingStyle(float *state, int state_size) { + driving_style_input_buf = state; + driving_style_size = state_size; +} + void ONNXModel::addTrafficConvention(float *state, int state_size) { traffic_convention_input_buf = state; traffic_convention_size = state_size; @@ -128,7 +138,13 @@ void ONNXModel::execute() { if (desire_input_buf != NULL) { pwrite(desire_input_buf, desire_state_size); } - if (traffic_convention_input_buf != NULL) { + if (nav_features_input_buf != NULL) { + pwrite(nav_features_input_buf, nav_features_size); + } + if (driving_style_input_buf != NULL) { + pwrite(driving_style_input_buf, driving_style_size); + } + if (traffic_convention_input_buf != NULL) { pwrite(traffic_convention_input_buf, traffic_convention_size); } if (calib_input_buf != NULL) { diff --git a/selfdrive/modeld/runners/onnxmodel.h b/selfdrive/modeld/runners/onnxmodel.h index d5b7bfecf0..9990bf1b45 100644 --- a/selfdrive/modeld/runners/onnxmodel.h +++ b/selfdrive/modeld/runners/onnxmodel.h @@ -10,6 +10,8 @@ public: ~ONNXModel(); void addRecurrent(float *state, int state_size); void addDesire(float *state, int state_size); + void addNavFeatures(float *state, int state_size); + void addDrivingStyle(float *state, int state_size); void addTrafficConvention(float *state, int state_size); void addCalib(float *state, int state_size); void addImage(float *image_buf, int buf_size); @@ -25,6 +27,10 @@ private: int rnn_state_size; float *desire_input_buf = NULL; int desire_state_size; + float *nav_features_input_buf = NULL; + int nav_features_size; + float *driving_style_input_buf = NULL; + int driving_style_size; float *traffic_convention_input_buf = NULL; int traffic_convention_size; float *calib_input_buf = NULL; diff --git a/selfdrive/modeld/runners/runmodel.h b/selfdrive/modeld/runners/runmodel.h index c607811401..673ddb50b5 100644 --- a/selfdrive/modeld/runners/runmodel.h +++ b/selfdrive/modeld/runners/runmodel.h @@ -5,6 +5,8 @@ public: virtual ~RunModel() {} virtual void addRecurrent(float *state, int state_size) {} virtual void addDesire(float *state, int state_size) {} + virtual void addNavFeatures(float *state, int state_size) {} + virtual void addDrivingStyle(float *state, int state_size) {} virtual void addTrafficConvention(float *state, int state_size) {} virtual void addCalib(float *state, int state_size) {} virtual void addImage(float *image_buf, int buf_size) {} diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc index ff4adcd8d3..609a7a6657 100644 --- a/selfdrive/modeld/runners/snpemodel.cc +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -153,6 +153,16 @@ void SNPEModel::addDesire(float *state, int state_size) { desireBuffer = this->addExtra(state, state_size, 1); } +void SNPEModel::addNavFeatures(float *state, int state_size) { + navFeatures = state; + navFeaturesBuffer = this->addExtra(state, state_size, 1); +} + +void SNPEModel::addDrivingStyle(float *state, int state_size) { + drivingStyle = state; + drivingStyleBuffer = this->addExtra(state, state_size, 2); +} + void SNPEModel::addCalib(float *state, int state_size) { calib = state; calibBuffer = this->addExtra(state, state_size, 1); diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h index 08ae16c2b1..0d84d1d482 100644 --- a/selfdrive/modeld/runners/snpemodel.h +++ b/selfdrive/modeld/runners/snpemodel.h @@ -28,6 +28,8 @@ public: void addTrafficConvention(float *state, int state_size); void addCalib(float *state, int state_size); void addDesire(float *state, int state_size); + void addDrivingStyle(float *state, int state_size); + void addNavFeatures(float *state, int state_size); void addImage(float *image_buf, int buf_size); void addExtra(float *image_buf, int buf_size); void execute(); @@ -75,6 +77,10 @@ private: std::unique_ptr trafficConventionBuffer; float *desire; std::unique_ptr desireBuffer; + float *navFeatures; + std::unique_ptr navFeaturesBuffer; + float *drivingStyle; + std::unique_ptr drivingStyleBuffer; float *calib; std::unique_ptr calibBuffer; }; diff --git a/selfdrive/modeld/runners/thneedmodel.cc b/selfdrive/modeld/runners/thneedmodel.cc index 67db01bb95..4fdd7ca466 100644 --- a/selfdrive/modeld/runners/thneedmodel.cc +++ b/selfdrive/modeld/runners/thneedmodel.cc @@ -24,6 +24,14 @@ void ThneedModel::addDesire(float *state, int state_size) { desire = state; } +void ThneedModel::addDrivingStyle(float *state, int state_size) { + drivingStyle = state; +} + +void ThneedModel::addNavFeatures(float *state, int state_size) { + navFeatures = state; +} + void ThneedModel::addImage(float *image_input_buf, int buf_size) { input = image_input_buf; } diff --git a/selfdrive/modeld/runners/thneedmodel.h b/selfdrive/modeld/runners/thneedmodel.h index f3f34dc7f4..63712f1d00 100644 --- a/selfdrive/modeld/runners/thneedmodel.h +++ b/selfdrive/modeld/runners/thneedmodel.h @@ -9,6 +9,8 @@ public: void addRecurrent(float *state, int state_size); void addTrafficConvention(float *state, int state_size); void addDesire(float *state, int state_size); + void addNavFeatures(float *state, int state_size); + void addDrivingStyle(float *state, int state_size); void addImage(float *image_buf, int buf_size); void addExtra(float *image_buf, int buf_size); void execute(); @@ -26,6 +28,8 @@ private: // recurrent and desire float *recurrent; float *trafficConvention; + float *drivingStyle; float *desire; + float *navFeatures; }; diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc index 0fed108045..91f31d06dc 100644 --- a/selfdrive/navd/map_renderer.cc +++ b/selfdrive/navd/map_renderer.cc @@ -70,7 +70,7 @@ MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_set if (online) { vipc_server.reset(new VisionIpcServer("navd")); - vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH, HEIGHT); + vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH/2, HEIGHT/2); vipc_server->start_listener(); pm.reset(new PubMaster({"navThumbnail", "mapRenderState"})); @@ -177,10 +177,12 @@ void MapRenderer::publish(const double render_time) { uint8_t* dst = (uint8_t*)buf->addr; uint8_t* src = cap.bits(); - // RGB to greyscale + // RGB to greyscale and crop memset(dst, 128, buf->len); - for (int i = 0; i < WIDTH * HEIGHT; i++) { - dst[i] = src[i * 3]; + for (int r = 0; r < HEIGHT/2; r++) { + for (int c = 0; c < WIDTH/2; c++) { + dst[r*WIDTH/2 + c] = src[((HEIGHT/4 + r)*WIDTH + (c+WIDTH/4)) * 3]; + } } vipc_server->send(buf, &extra); diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 4dcf0ecbb7..aa204e0f8a 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -30dfeed50ac562f99b0aad985bd6b0e6d8188fcb \ No newline at end of file +4ff972367fdb9546be68ee0ba0d45cf4f839dae7