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 (#21729)

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

* Bump submodules

* New model: fd904bf9-5dd8-4619-8997-c5668400e078/550

* New model: fd904bf9-5dd8-4619-8997-c5668400e078/950

* No keeps

* Bumped model replay ref

Co-authored-by: Harald Schafer <harald.the.engineer@gmail.com>
old-commit-hash: e83973b9b3
commatwo_master
Mitchell Goff 4 years ago committed by GitHub
parent 5938e836cf
commit 3fb1a1c746
8 changed files with 64 additions and 31 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. 11
      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:d5d753debf1796223896637cd62f2bee881eeb3290d9e739f5d3c7910ab542f2
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:adcc169da199333b315548cc210a4860f24e9b3144f776315632488e194316c6
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;

11
selfdrive/controls/lib/lateral_planner.py
Unescape 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
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) {
@ -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
Unescape View File

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