sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

New desire model (#21458)

Browse Source 
* New model: b822ee32-917c-4639-99f8-25be3e6aabad/900

* Enable keeps

* use new packet

* Send keep pulses once per second

* Updated model_replay_ref_commit

* New model: 0f160bf9-1bce-43e6-9785-564b2453eeac

* Bump model_replay_ref_commit

Co-authored-by: mitchell <mitchellgoffpc@gmail.com>
old-commit-hash: 4230d5d212
commatwo_master
HaraldSchafer 4 years ago committed by GitHub
parent 96d4bfbff3
commit 7fcdd04541
8 changed files with 66 additions and 33 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      models/supercombo.dlc
  2. 4
      models/supercombo.onnx
  3. 2
      selfdrive/common/modeldata.h
  4. 15
      selfdrive/controls/lib/lateral_planner.py
  5. 10
      selfdrive/controls/lib/radar_helpers.py
  6. 16
      selfdrive/controls/radard.py
  7. 46
      selfdrive/modeld/models/driving.cc
  8. 2
      selfdrive/test/process_replay/model_replay_ref_commit

4
models/supercombo.dlc
Unescape View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:dc46a24d4b4afa9730785264834e7f7c04c84b6a28a689acb503d6663818c256
size 58797567
oid sha256:d79c015ef322704ad7fa3b0048f3d65965f46aa73ea60110ec21487d15cc012a
size 56720671

4
models/supercombo.onnx
Unescape View File

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:77a31e5a3a70c39a3fcb07e939a668baf80b1eb778fe792fbe256de983fab5cd
size 58823636
oid sha256:b66036aebb132973243f641a7fe69cd29fd7928a3f6d7bafe8a37ce3572f8a92
size 56742706

2
selfdrive/common/modeldata.h
Unescape 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;

15
selfdrive/controls/lib/lateral_planner.py
Unescape View File

@ -28,13 +28,13 @@ DESIRES = {
},
LaneChangeDirection.left: {
LaneChangeState.off: log.LateralPlan.Desire.none,
LaneChangeState.preLaneChange: log.LateralPlan.Desire.none,
LaneChangeState.preLaneChange: log.LateralPlan.Desire.keepLeft,
LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeLeft,
LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeLeft,
},
LaneChangeDirection.right: {
LaneChangeState.off: log.LateralPlan.Desire.none,
LaneChangeState.preLaneChange: log.LateralPlan.Desire.none,
LaneChangeState.preLaneChange: log.LateralPlan.Desire.keepRight,
LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeRight,
LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeRight,
},
@ -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
Unescape 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
Unescape 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
Unescape 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) {
@ -305,10 +327,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
Unescape View File

@ -1 +1 @@
8f7ed52c84e9e2e6e8f4d2165130b46f27e76b30
c3c2a80c32c1f08efa8a2c8cc3c37c1038de96cb
Write Preview
Loading…
Cancel
Save