mawei/onepilot
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Retuned desire model (#21919)

Browse Source 
* New model: d8e7f76f-7bec-4a83-af00-c0fae792527f/950

* Updated process replay refs

* Updated model replay ref
old-commit-hash: 05b37552f3a38f914af41f44ccc7c633ad152a15
This commit is contained in:
Mitchell Goff
2021-08-12 21:06:26 -07:00
committed by GitHub
parent 9733e2f5f1
commit b7b87efed9
11 changed files with 76 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
models/supercombo.dlc LFS
View File

Binary file not shown.

BIN
models/supercombo.onnx LFS
View File

Binary file not shown.

2
selfdrive/common/modeldata.h
View File

@@ -1,7 +1,7 @@
#pragma once
const int TRAJECTORY_SIZE = 33;
const int LON_MPC_N = 32;
const int LAT_MPC_N = 16;
const int LON_MPC_N = 32;
const float MIN_DRAW_DISTANCE = 10.0;
const float MAX_DRAW_DISTANCE = 100.0;

11
selfdrive/controls/lib/lateral_planner.py
View File

@@ -55,6 +55,7 @@ class LateralPlanner():
self.lane_change_direction = LaneChangeDirection.none
self.lane_change_timer = 0.0
self.lane_change_ll_prob = 1.0
self.keep_pulse_timer = 0.0
self.prev_one_blinker = False
self.desire = log.LateralPlan.Desire.none
@@ -157,6 +158,16 @@ class LateralPlanner():
self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# Send keep pulse once per second during LaneChangeStart.preLaneChange
if self.lane_change_state in [LaneChangeState.off, LaneChangeState.laneChangeStarting]:
self.keep_pulse_timer = 0.0
elif self.lane_change_state == LaneChangeState.preLaneChange:
self.keep_pulse_timer += DT_MDL
if self.keep_pulse_timer > 1.0:
self.keep_pulse_timer = 0.0
elif self.desire in [log.LateralPlan.Desire.keepLeft, log.LateralPlan.Desire.keepRight]:
self.desire = log.LateralPlan.Desire.none
# Turn off lanes during lane change
if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
self.LP.lll_prob *= self.lane_change_ll_prob

10
selfdrive/controls/lib/radar_helpers.py
View File

@@ -132,11 +132,11 @@ class Cluster():
def get_RadarState_from_vision(self, lead_msg, v_ego):
return {
"dRel": float(lead_msg.xyva[0] - RADAR_TO_CAMERA),
"yRel": float(-lead_msg.xyva[1]),
"vRel": float(lead_msg.xyva[2]),
"vLead": float(v_ego + lead_msg.xyva[2]),
"vLeadK": float(v_ego + lead_msg.xyva[2]),
"dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA),
"yRel": float(-lead_msg.y[0]),
"vRel": float(lead_msg.v[0] - v_ego),
"vLead": float(lead_msg.v[0]),
"vLeadK": float(lead_msg.v[0]),
"aLeadK": float(0),
"aLeadTau": _LEAD_ACCEL_TAU,
"fcw": False,

16
selfdrive/controls/radard.py
View File

@@ -38,12 +38,12 @@ def laplacian_cdf(x, mu, b):
def match_vision_to_cluster(v_ego, lead, clusters):
# match vision point to best statistical cluster match
offset_vision_dist = lead.xyva[0] - RADAR_TO_CAMERA
offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
def prob(c):
prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xyvaStd[0])
prob_y = laplacian_cdf(c.yRel, -lead.xyva[1], lead.xyvaStd[1])
prob_v = laplacian_cdf(c.vRel, lead.xyva[2], lead.xyvaStd[2])
prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xStd[0])
prob_y = laplacian_cdf(c.yRel, -lead.y[0], lead.yStd[0])
prob_v = laplacian_cdf(c.vRel + v_ego, lead.v[0], lead.vStd[0])
# This is isn't exactly right, but good heuristic
return prob_d * prob_y * prob_v
@@ -53,7 +53,7 @@ def match_vision_to_cluster(v_ego, lead, clusters):
# if no 'sane' match is found return -1
# stationary radar points can be false positives
dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
vel_sane = (abs(cluster.vRel - lead.xyva[2]) < 10) or (v_ego + cluster.vRel > 3)
vel_sane = (abs(cluster.vRel + v_ego - lead.v[0]) < 10) or (v_ego + cluster.vRel > 3)
if dist_sane and vel_sane:
return cluster
else:
@@ -166,9 +166,9 @@ class RadarD():
radarState.carStateMonoTime = sm.logMonoTime['carState']
if enable_lead:
if len(sm['modelV2'].leads) > 1:
radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leads[0], low_speed_override=True)
radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leads[1], low_speed_override=False)
if len(sm['modelV2'].leadsV3) > 1:
radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leadsV3[0], low_speed_override=True)
radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leadsV3[1], low_speed_override=False)
return dat

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

@@ -24,7 +24,9 @@ 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_TRAJ_LEN = 6;
constexpr int LEAD_PRED_DIM = 4;
constexpr int LEAD_MHP_VALS = LEAD_PRED_DIM*LEAD_TRAJ_LEN;
constexpr int LEAD_MHP_SELECTION = 3;
constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION);
@@ -147,18 +149,38 @@ void fill_sigmoid(const float *input, float *output, int len, int stride) {
}
}
void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float *lead_data, const float *prob, int t_offset, float t) {
void fill_lead_v3(cereal::ModelDataV2::LeadDataV3::Builder lead, const float *lead_data, const float *prob, int t_offset, float prob_t) {
float t[LEAD_TRAJ_LEN] = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0};
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];
for (int i=0; i<LEAD_MHP_VALS; i++) {
xyva_arr[i] = data[i];
xyva_stds_arr[i] = exp(data[LEAD_MHP_VALS + i]);
lead.setProbTime(prob_t);
float x_arr[LEAD_TRAJ_LEN];
float y_arr[LEAD_TRAJ_LEN];
float v_arr[LEAD_TRAJ_LEN];
float a_arr[LEAD_TRAJ_LEN];
float x_stds_arr[LEAD_TRAJ_LEN];
float y_stds_arr[LEAD_TRAJ_LEN];
float v_stds_arr[LEAD_TRAJ_LEN];
float a_stds_arr[LEAD_TRAJ_LEN];
for (int i=0; i<LEAD_TRAJ_LEN; i++) {
x_arr[i] = data[i*LEAD_PRED_DIM+0];
y_arr[i] = data[i*LEAD_PRED_DIM+1];
v_arr[i] = data[i*LEAD_PRED_DIM+2];
a_arr[i] = data[i*LEAD_PRED_DIM+3];
x_stds_arr[i] = exp(data[LEAD_MHP_VALS + i*LEAD_PRED_DIM+0]);
y_stds_arr[i] = exp(data[LEAD_MHP_VALS + i*LEAD_PRED_DIM+1]);
v_stds_arr[i] = exp(data[LEAD_MHP_VALS + i*LEAD_PRED_DIM+2]);
a_stds_arr[i] = exp(data[LEAD_MHP_VALS + i*LEAD_PRED_DIM+3]);
}
lead.setXyva(xyva_arr);
lead.setXyvaStd(xyva_stds_arr);
lead.setT(t);
lead.setX(x_arr);
lead.setY(y_arr);
lead.setV(v_arr);
lead.setA(a_arr);
lead.setXStd(x_stds_arr);
lead.setYStd(y_stds_arr);
lead.setVStd(v_stds_arr);
lead.setAStd(a_stds_arr);
}
void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_data) {
@@ -303,10 +325,10 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
fill_meta(framed.initMeta(), net_outputs.meta);
// leads
auto leads = framed.initLeads(LEAD_MHP_SELECTION);
auto leads = framed.initLeadsV3(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++) {
fill_lead_v2(leads[t_offset], net_outputs.lead, net_outputs.lead_prob, t_offset, t_offsets[t_offset]);
fill_lead_v3(leads[t_offset], net_outputs.lead, net_outputs.lead_prob, t_offset, t_offsets[t_offset]);
}
}

2
selfdrive/test/process_replay/model_replay_ref_commit
View File

@@ -1 +1 @@
8f7ed52c84e9e2e6e8f4d2165130b46f27e76b30
a6a2358fe21ee43f10ed26100eb2f0811b877670

2
selfdrive/test/process_replay/ref_commit
View File

@@ -1 +1 @@
eedd8e26cfdbf1bb3cdc566c3ed30c4054efb8cf
dfa788ebfb5a87a2c76c921cb0b54eaf39175071

12
selfdrive/ui/paint.cc
View File

@@ -67,15 +67,15 @@ static void ui_draw_circle_image(const UIState *s, int center_x, int center_y, i
ui_draw_circle_image(s, center_x, center_y, radius, image, nvgRGBA(0, 0, 0, (255 * bg_alpha)), img_alpha);
}
static void draw_lead(UIState *s, const cereal::ModelDataV2::LeadDataV2::Reader &lead_data, const vertex_data &vd) {
static void draw_lead(UIState *s, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const vertex_data &vd) {
// Draw lead car indicator
auto [x, y] = vd;
float fillAlpha = 0;
float speedBuff = 10.;
float leadBuff = 40.;
float d_rel = lead_data.getXyva()[0];
float v_rel = lead_data.getXyva()[2];
float d_rel = lead_data.getX()[0];
float v_rel = lead_data.getV()[0];
if (d_rel < leadBuff) {
fillAlpha = 255*(1.0-(d_rel/leadBuff));
if (v_rel < 0) {
@@ -167,12 +167,12 @@ static void ui_draw_world(UIState *s) {
// Draw lead indicators if openpilot is handling longitudinal
if (s->scene.longitudinal_control) {
auto lead_one = (*s->sm)["modelV2"].getModelV2().getLeads()[0];
auto lead_two = (*s->sm)["modelV2"].getModelV2().getLeads()[1];
auto lead_one = (*s->sm)["modelV2"].getModelV2().getLeadsV3()[0];
auto lead_two = (*s->sm)["modelV2"].getModelV2().getLeadsV3()[1];
if (lead_one.getProb() > .5) {
draw_lead(s, lead_one, s->scene.lead_vertices[0]);
}
if (lead_two.getProb() > .5 && (std::abs(lead_one.getXyva()[0] - lead_two.getXyva()[0]) > 3.0)) {
if (lead_two.getProb() > .5 && (std::abs(lead_one.getX()[0] - lead_two.getX()[0]) > 3.0)) {
draw_lead(s, lead_two, s->scene.lead_vertices[1]);
}
}

11
selfdrive/ui/ui.cc
View File

@@ -64,13 +64,12 @@ static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line
}
static void update_leads(UIState *s, const cereal::ModelDataV2::Reader &model) {
auto leads = model.getLeads();
auto leads = model.getLeadsV3();
auto model_position = model.getPosition();
for (int i = 0; i < 2; ++i) {
if (leads[i].getProb() > 0.5) {
auto xyva = leads[i].getXyva();
float z = model_position.getZ()[get_path_length_idx(model_position, xyva[0])];
calib_frame_to_full_frame(s, xyva[0], xyva[1], z + 1.22, &s->scene.lead_vertices[i]);
float z = model_position.getZ()[get_path_length_idx(model_position, leads[i].getX()[0])];
calib_frame_to_full_frame(s, leads[i].getX()[0], leads[i].getY()[0], z + 1.22, &s->scene.lead_vertices[i]);
}
}
}
@@ -113,9 +112,9 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
}
// update path
auto lead_one = model.getLeads()[0];
auto lead_one = model.getLeadsV3()[0];
if (lead_one.getProb() > 0.5) {
const float lead_d = lead_one.getXyva()[0] * 2.;
const float lead_d = lead_one.getX()[0] * 2.;
max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance);
}
max_idx = get_path_length_idx(model_position, max_distance);