Retuned desire model (#21919)

* New model: d8e7f76f-7bec-4a83-af00-c0fae792527f/950

* Updated process replay refs

* Updated model replay ref
old-commit-hash: 05b37552f3

models/supercombo.dlc

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

models/supercombo.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:77a31e5a3a70c39a3fcb07e939a668baf80b1eb778fe792fbe256de983fab5cd
+oid sha256:69c1f8f71fd815c9d30361b99b1dfd39df5460176c628c038d3f7d91e4801704
-size 58823636
+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),
+      "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA),
-      "yRel": float(-lead_msg.xyva[1]),
+      "yRel": float(-lead_msg.y[0]),
-      "vRel": float(lead_msg.xyva[2]),
+      "vRel": float(lead_msg.v[0] - v_ego),
-      "vLead": float(v_ego + lead_msg.xyva[2]),
+      "vLead": float(lead_msg.v[0]),
-      "vLeadK": float(v_ego + lead_msg.xyva[2]),
+      "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_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xStd[0])
-    prob_y = laplacian_cdf(c.yRel, -lead.xyva[1], lead.xyvaStd[1])
+    prob_y = laplacian_cdf(c.yRel, -lead.y[0], lead.yStd[0])
-    prob_v = laplacian_cdf(c.vRel, lead.xyva[2], lead.xyvaStd[2])
+    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:
+      if len(sm['modelV2'].leadsV3) > 1:
-        radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leads[0], low_speed_override=True)
+        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'].leads[1], low_speed_override=False)
+        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);
+  lead.setProbTime(prob_t);
-  float xyva_arr[LEAD_MHP_VALS];
+  float x_arr[LEAD_TRAJ_LEN];
-  float xyva_stds_arr[LEAD_MHP_VALS];
+  float y_arr[LEAD_TRAJ_LEN];
-  for (int i=0; i<LEAD_MHP_VALS; i++) {
+  float v_arr[LEAD_TRAJ_LEN];
-    xyva_arr[i] = data[i];
+  float a_arr[LEAD_TRAJ_LEN];
-    xyva_stds_arr[i] = exp(data[LEAD_MHP_VALS + i]);
+  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.setT(t);
-  lead.setXyvaStd(xyva_stds_arr);
+  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
+a6a2358fe21ee43f10ed26100eb2f0811b877670

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-eedd8e26cfdbf1bb3cdc566c3ed30c4054efb8cf
+dfa788ebfb5a87a2c76c921cb0b54eaf39175071

selfdrive/ui/paint.cc

@@ -67,15 +67,15 @@ static void ui_draw_circle_image(const UIState *s, int center_x, int center_y, i
   ui_draw_circle_image(s, center_x, center_y, radius, image, nvgRGBA(0, 0, 0, (255 * bg_alpha)), img_alpha);
 }
 
-static void draw_lead(UIState *s, const cereal::ModelDataV2::LeadDataV2::Reader &lead_data, const vertex_data &vd) {
+static void draw_lead(UIState *s, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const vertex_data &vd) {
   // Draw lead car indicator
   auto [x, y] = vd;
 
   float fillAlpha = 0;
   float speedBuff = 10.;
   float leadBuff = 40.;
-  float d_rel = lead_data.getXyva()[0];
+  float d_rel = lead_data.getX()[0];
-  float v_rel = lead_data.getXyva()[2];
+  float v_rel = lead_data.getV()[0];
   if (d_rel < leadBuff) {
     fillAlpha = 255*(1.0-(d_rel/leadBuff));
     if (v_rel < 0) {
@@ -167,12 +167,12 @@ static void ui_draw_world(UIState *s) {
 
   // Draw lead indicators if openpilot is handling longitudinal
   if (s->scene.longitudinal_control) {
-    auto lead_one = (*s->sm)["modelV2"].getModelV2().getLeads()[0];
+    auto lead_one = (*s->sm)["modelV2"].getModelV2().getLeadsV3()[0];
-    auto lead_two = (*s->sm)["modelV2"].getModelV2().getLeads()[1];
+    auto lead_two = (*s->sm)["modelV2"].getModelV2().getLeadsV3()[1];
     if (lead_one.getProb() > .5) {
       draw_lead(s, lead_one, s->scene.lead_vertices[0]);
     }
-   if (lead_two.getProb() > .5 && (std::abs(lead_one.getXyva()[0] - lead_two.getXyva()[0]) > 3.0)) {
+   if (lead_two.getProb() > .5 && (std::abs(lead_one.getX()[0] - lead_two.getX()[0]) > 3.0)) {
       draw_lead(s, lead_two, s->scene.lead_vertices[1]);
     }
   }

selfdrive/ui/ui.cc

@@ -64,13 +64,12 @@ static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line
 }
 
 static void update_leads(UIState *s, const cereal::ModelDataV2::Reader &model) {
-  auto leads = model.getLeads();
+  auto leads = model.getLeadsV3();
   auto model_position = model.getPosition();
   for (int i = 0; i < 2; ++i) {
     if (leads[i].getProb() > 0.5) {
-      auto xyva = leads[i].getXyva();
+      float z = model_position.getZ()[get_path_length_idx(model_position, leads[i].getX()[0])];
-      float z = model_position.getZ()[get_path_length_idx(model_position, xyva[0])];
+      calib_frame_to_full_frame(s, leads[i].getX()[0], leads[i].getY()[0], z + 1.22, &s->scene.lead_vertices[i]);
-      calib_frame_to_full_frame(s, xyva[0], xyva[1], z + 1.22, &s->scene.lead_vertices[i]);
     }
   }
 }
@@ -113,9 +112,9 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
   }
 
   // update path
-  auto lead_one = model.getLeads()[0];
+  auto lead_one = model.getLeadsV3()[0];
   if (lead_one.getProb() > 0.5) {
-    const float lead_d = lead_one.getXyva()[0] * 2.;
+    const float lead_d = lead_one.getX()[0] * 2.;
     max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance);
   }
   max_idx = get_path_length_idx(model_position, max_distance);