Revert model (#21571)
* Revert "New desire model (#21458)"
This reverts commit 4230d5d212
.
* revert rel notes
local_plotjuggler
parent
a42d8f3a14
commit
88424ede2c
|
@ -1,9 +1,5 @@
|
||||||
Version 0.8.6 (2021-XX-XX)
|
Version 0.8.6 (2021-XX-XX)
|
||||||
========================
|
========================
|
||||||
* New driving model with improved laneless performance
|
|
||||||
* Trained on 5000+ hours of diverse driving data from 3000+ users in 40+ countries
|
|
||||||
* Better anti-cheating methods during simulator training ensure the model hugs less when in laneless mode
|
|
||||||
* All new desire ground-truthing stack makes the model better at lane changes and keeps
|
|
||||||
* Revamp lateral and longitudinal planners
|
* Revamp lateral and longitudinal planners
|
||||||
* Refactor planner output API to be more readable and verbose
|
* Refactor planner output API to be more readable and verbose
|
||||||
* Planners now output desired trajectories for speed, acceleration, curvature, and curvature rate
|
* Planners now output desired trajectories for speed, acceleration, curvature, and curvature rate
|
||||||
|
|
BIN
models/supercombo.dlc (Stored with Git LFS)
BIN
models/supercombo.dlc (Stored with Git LFS)
Binary file not shown.
BIN
models/supercombo.onnx (Stored with Git LFS)
BIN
models/supercombo.onnx (Stored with Git LFS)
Binary file not shown.
|
@ -1,7 +1,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
const int TRAJECTORY_SIZE = 33;
|
const int TRAJECTORY_SIZE = 33;
|
||||||
const int LAT_MPC_N = 16;
|
|
||||||
const int LON_MPC_N = 32;
|
const int LON_MPC_N = 32;
|
||||||
|
const int LAT_MPC_N = 16;
|
||||||
const float MIN_DRAW_DISTANCE = 10.0;
|
const float MIN_DRAW_DISTANCE = 10.0;
|
||||||
const float MAX_DRAW_DISTANCE = 100.0;
|
const float MAX_DRAW_DISTANCE = 100.0;
|
||||||
|
|
||||||
|
|
|
@ -28,13 +28,13 @@ DESIRES = {
|
||||||
},
|
},
|
||||||
LaneChangeDirection.left: {
|
LaneChangeDirection.left: {
|
||||||
LaneChangeState.off: log.LateralPlan.Desire.none,
|
LaneChangeState.off: log.LateralPlan.Desire.none,
|
||||||
LaneChangeState.preLaneChange: log.LateralPlan.Desire.keepLeft,
|
LaneChangeState.preLaneChange: log.LateralPlan.Desire.none,
|
||||||
LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeLeft,
|
LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeLeft,
|
||||||
LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeLeft,
|
LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeLeft,
|
||||||
},
|
},
|
||||||
LaneChangeDirection.right: {
|
LaneChangeDirection.right: {
|
||||||
LaneChangeState.off: log.LateralPlan.Desire.none,
|
LaneChangeState.off: log.LateralPlan.Desire.none,
|
||||||
LaneChangeState.preLaneChange: log.LateralPlan.Desire.keepRight,
|
LaneChangeState.preLaneChange: log.LateralPlan.Desire.none,
|
||||||
LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeRight,
|
LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeRight,
|
||||||
LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeRight,
|
LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeRight,
|
||||||
},
|
},
|
||||||
|
@ -55,7 +55,6 @@ class LateralPlanner():
|
||||||
self.lane_change_direction = LaneChangeDirection.none
|
self.lane_change_direction = LaneChangeDirection.none
|
||||||
self.lane_change_timer = 0.0
|
self.lane_change_timer = 0.0
|
||||||
self.lane_change_ll_prob = 1.0
|
self.lane_change_ll_prob = 1.0
|
||||||
self.keep_pulse_timer = 0.0
|
|
||||||
self.prev_one_blinker = False
|
self.prev_one_blinker = False
|
||||||
self.desire = log.LateralPlan.Desire.none
|
self.desire = log.LateralPlan.Desire.none
|
||||||
|
|
||||||
|
@ -158,16 +157,6 @@ class LateralPlanner():
|
||||||
|
|
||||||
self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
|
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
|
# Turn off lanes during lane change
|
||||||
if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
|
if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
|
||||||
self.LP.lll_prob *= self.lane_change_ll_prob
|
self.LP.lll_prob *= self.lane_change_ll_prob
|
||||||
|
|
|
@ -132,11 +132,11 @@ class Cluster():
|
||||||
|
|
||||||
def get_RadarState_from_vision(self, lead_msg, v_ego):
|
def get_RadarState_from_vision(self, lead_msg, v_ego):
|
||||||
return {
|
return {
|
||||||
"dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA),
|
"dRel": float(lead_msg.xyva[0] - RADAR_TO_CAMERA),
|
||||||
"yRel": float(-lead_msg.y[0]),
|
"yRel": float(-lead_msg.xyva[1]),
|
||||||
"vRel": float(lead_msg.v[0] - v_ego),
|
"vRel": float(lead_msg.xyva[2]),
|
||||||
"vLead": float(lead_msg.v[0]),
|
"vLead": float(v_ego + lead_msg.xyva[2]),
|
||||||
"vLeadK": float(lead_msg.v[0]),
|
"vLeadK": float(v_ego + lead_msg.xyva[2]),
|
||||||
"aLeadK": float(0),
|
"aLeadK": float(0),
|
||||||
"aLeadTau": _LEAD_ACCEL_TAU,
|
"aLeadTau": _LEAD_ACCEL_TAU,
|
||||||
"fcw": False,
|
"fcw": False,
|
||||||
|
|
|
@ -38,12 +38,12 @@ def laplacian_cdf(x, mu, b):
|
||||||
|
|
||||||
def match_vision_to_cluster(v_ego, lead, clusters):
|
def match_vision_to_cluster(v_ego, lead, clusters):
|
||||||
# match vision point to best statistical cluster match
|
# match vision point to best statistical cluster match
|
||||||
offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
|
offset_vision_dist = lead.xyva[0] - RADAR_TO_CAMERA
|
||||||
|
|
||||||
def prob(c):
|
def prob(c):
|
||||||
prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xStd[0])
|
prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xyvaStd[0])
|
||||||
prob_y = laplacian_cdf(c.yRel, -lead.y[0], lead.yStd[0])
|
prob_y = laplacian_cdf(c.yRel, -lead.xyva[1], lead.xyvaStd[1])
|
||||||
prob_v = laplacian_cdf(c.vRel + v_ego, lead.v[0], lead.vStd[0])
|
prob_v = laplacian_cdf(c.vRel, lead.xyva[2], lead.xyvaStd[2])
|
||||||
|
|
||||||
# This is isn't exactly right, but good heuristic
|
# This is isn't exactly right, but good heuristic
|
||||||
return prob_d * prob_y * prob_v
|
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
|
# if no 'sane' match is found return -1
|
||||||
# stationary radar points can be false positives
|
# stationary radar points can be false positives
|
||||||
dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
|
dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
|
||||||
vel_sane = (abs(cluster.vRel + v_ego - lead.v[0]) < 10) or (v_ego + cluster.vRel > 3)
|
vel_sane = (abs(cluster.vRel - lead.xyva[2]) < 10) or (v_ego + cluster.vRel > 3)
|
||||||
if dist_sane and vel_sane:
|
if dist_sane and vel_sane:
|
||||||
return cluster
|
return cluster
|
||||||
else:
|
else:
|
||||||
|
@ -166,9 +166,9 @@ class RadarD():
|
||||||
radarState.carStateMonoTime = sm.logMonoTime['carState']
|
radarState.carStateMonoTime = sm.logMonoTime['carState']
|
||||||
|
|
||||||
if enable_lead:
|
if enable_lead:
|
||||||
if len(sm['modelV2'].leadsV3) > 1:
|
if len(sm['modelV2'].leads) > 1:
|
||||||
radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leadsV3[0], low_speed_override=True)
|
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'].leadsV3[1], low_speed_override=False)
|
radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leads[1], low_speed_override=False)
|
||||||
return dat
|
return dat
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -24,9 +24,7 @@ constexpr int PLAN_MHP_SELECTION = 1;
|
||||||
constexpr int PLAN_MHP_GROUP_SIZE = (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION);
|
constexpr int PLAN_MHP_GROUP_SIZE = (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION);
|
||||||
|
|
||||||
constexpr int LEAD_MHP_N = 5;
|
constexpr int LEAD_MHP_N = 5;
|
||||||
constexpr int LEAD_TRAJ_LEN = 6;
|
constexpr int LEAD_MHP_VALS = 4;
|
||||||
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_SELECTION = 3;
|
||||||
constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION);
|
constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION);
|
||||||
|
|
||||||
|
@ -149,38 +147,18 @@ void fill_sigmoid(const float *input, float *output, int len, int stride) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void fill_lead_v3(cereal::ModelDataV2::LeadDataV3::Builder lead, const float *lead_data, const float *prob, int t_offset, float prob_t) {
|
void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float *lead_data, const float *prob, int t_offset, float 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);
|
const float *data = get_lead_data(lead_data, t_offset);
|
||||||
lead.setProb(sigmoid(prob[t_offset]));
|
lead.setProb(sigmoid(prob[t_offset]));
|
||||||
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.setT(t);
|
lead.setT(t);
|
||||||
lead.setX(x_arr);
|
float xyva_arr[LEAD_MHP_VALS];
|
||||||
lead.setY(y_arr);
|
float xyva_stds_arr[LEAD_MHP_VALS];
|
||||||
lead.setV(v_arr);
|
for (int i=0; i<LEAD_MHP_VALS; i++) {
|
||||||
lead.setA(a_arr);
|
xyva_arr[i] = data[i];
|
||||||
lead.setXStd(x_stds_arr);
|
xyva_stds_arr[i] = exp(data[LEAD_MHP_VALS + i]);
|
||||||
lead.setYStd(y_stds_arr);
|
}
|
||||||
lead.setVStd(v_stds_arr);
|
lead.setXyva(xyva_arr);
|
||||||
lead.setAStd(a_stds_arr);
|
lead.setXyvaStd(xyva_stds_arr);
|
||||||
}
|
}
|
||||||
|
|
||||||
void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_data) {
|
void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_data) {
|
||||||
|
@ -327,10 +305,10 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
|
||||||
fill_meta(framed.initMeta(), net_outputs.meta);
|
fill_meta(framed.initMeta(), net_outputs.meta);
|
||||||
|
|
||||||
// leads
|
// leads
|
||||||
auto leads = framed.initLeadsV3(LEAD_MHP_SELECTION);
|
auto leads = framed.initLeads(LEAD_MHP_SELECTION);
|
||||||
float t_offsets[LEAD_MHP_SELECTION] = {0.0, 2.0, 4.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 t_offset=0; t_offset<LEAD_MHP_SELECTION; t_offset++) {
|
||||||
fill_lead_v3(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, net_outputs.lead_prob, t_offset, t_offsets[t_offset]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1 +1 @@
|
||||||
c3c2a80c32c1f08efa8a2c8cc3c37c1038de96cb
|
8f7ed52c84e9e2e6e8f4d2165130b46f27e76b30
|
Loading…
Reference in New Issue