Revert "New desire model (#21729)" (#21805)

* Revert "New desire model (#21729)"

This reverts commit 709932b3bfb4c3b4f33711b8d07615469e762832.

* Updated process replay ref and reverted release notes
old-commit-hash: 15d6bbb47c

RELEASES.md

@@ -1,9 +1,5 @@
 Version 0.8.7 (2021-07-31)
 ========================
- * 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 in laneless mode
-   * All new desire ground-truthing stack makes the model better at lane changes
  * Volkswagen T-Cross 2021 support thanks to jyoung8607!
 
 Version 0.8.6 (2021-07-21)

models/supercombo.dlc

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

models/supercombo.onnx

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

selfdrive/common/modeldata.h

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

selfdrive/controls/lib/lateral_planner.py

@@ -55,7 +55,6 @@ 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
 
@@ -158,16 +157,6 @@ 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

selfdrive/controls/lib/radar_helpers.py

@@ -132,11 +132,11 @@ class Cluster():
 
   def get_RadarState_from_vision(self, lead_msg, v_ego):
     return {
-      "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]),
+      "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]),
       "aLeadK": float(0),
       "aLeadTau": _LEAD_ACCEL_TAU,
       "fcw": False,

selfdrive/controls/radard.py

@@ -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.x[0] - RADAR_TO_CAMERA
+  offset_vision_dist = lead.xyva[0] - RADAR_TO_CAMERA
 
   def prob(c):
-    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])
+    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])
 
     # 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 + 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:
     return cluster
   else:
@@ -166,9 +166,9 @@ class RadarD():
     radarState.carStateMonoTime = sm.logMonoTime['carState']
 
     if enable_lead:
-      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)
+      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)
     return dat
 
 

selfdrive/modeld/models/driving.cc

@@ -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 LEAD_MHP_N = 5;
-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_VALS = 4;
 constexpr int LEAD_MHP_SELECTION = 3;
 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) {
-  float t[LEAD_TRAJ_LEN] = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0};
+void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float *lead_data, const float *prob, int t_offset, float t) {
   const float *data = get_lead_data(lead_data, 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.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);
+  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.setXyva(xyva_arr);
+  lead.setXyvaStd(xyva_stds_arr);
 }
 
 void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_data) {
@@ -325,10 +303,10 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
   fill_meta(framed.initMeta(), net_outputs.meta);
 
   // 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};
   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]);
   }
 }
 

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-5802f7fdb664e63de960cf0d486138eb390a51e5
+8f7ed52c84e9e2e6e8f4d2165130b46f27e76b30

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-028c81d39f306212101cc33e6f42503cc7e510d3
+658824c1198bd9e4c2b2c6d1c9de9bcae04c7057