New desire model (#21729)

* 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>

models/supercombo.dlc

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

models/supercombo.onnx

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

selfdrive/common/modeldata.h

@@ -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;
 

selfdrive/controls/lib/lateral_planner.py

@@ -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

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.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,

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.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
 
 

selfdrive/modeld/models/driving.cc

@@ -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]);
   }
 }
 

selfdrive/test/process_replay/model_replay_ref_commit

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