Use laneful policy to prevent planner aggression

selfdrive/controls/lib/lateral_planner.py

@@ -59,13 +59,9 @@ class LateralPlanner:
     # Calculate final driving path and set MPC costs
     if self.use_lanelines:
       d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
-      self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost)
     else:
       d_path_xyz = self.path_xyz
-      path_cost = np.clip(abs(self.path_xyz[0, 1] / self.path_xyz_stds[0, 1]), 0.5, 1.5) * MPC_COST_LAT.PATH
-      # Heading cost is useful at low speed, otherwise end of plan can be off-heading
-      heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0])
-      self.lat_mpc.set_weights(path_cost, heading_cost, self.steer_rate_cost)
+    self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost)
 
     y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
     heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)

selfdrive/modeld/models/driving.cc

@@ -219,19 +219,24 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic
 
 void fill_lane_lines(cereal::ModelDataV2::Builder &framed, const std::array<float, TRAJECTORY_SIZE> &plan_t,
                      const ModelOutputLaneLines &lanes) {
+
+  const auto &left_far = lanes.get_lane_idx(0);
+  const auto &left_near = lanes.get_lane_idx(1);
+  const auto &right_near = lanes.get_lane_idx(2);
+  const auto &right_far = lanes.get_lane_idx(3);
   std::array<float, TRAJECTORY_SIZE> left_far_y, left_far_z;
   std::array<float, TRAJECTORY_SIZE> left_near_y, left_near_z;
   std::array<float, TRAJECTORY_SIZE> right_near_y, right_near_z;
   std::array<float, TRAJECTORY_SIZE> right_far_y, right_far_z;
   for (int j=0; j<TRAJECTORY_SIZE; j++) {
-    left_far_y[j] = lanes.mean.left_far[j].y;
-    left_far_z[j] = lanes.mean.left_far[j].z;
-    left_near_y[j] = lanes.mean.left_near[j].y;
-    left_near_z[j] = lanes.mean.left_near[j].z;
-    right_near_y[j] = lanes.mean.right_near[j].y;
-    right_near_z[j] = lanes.mean.right_near[j].z;
-    right_far_y[j] = lanes.mean.right_far[j].y;
-    right_far_z[j] = lanes.mean.right_far[j].z;
+    left_far_y[j] = left_far.mean[j].y;
+    left_far_z[j] = left_far.mean[j].z;
+    left_near_y[j] = left_near.mean[j].y;
+    left_near_z[j] = left_near.mean[j].z;
+    right_near_y[j] = right_near.mean[j].y;
+    right_near_z[j] = right_near.mean[j].z;
+    right_far_y[j] = right_far.mean[j].y;
+    right_far_z[j] = right_far.mean[j].z;
   }
 
   auto lane_lines = framed.initLaneLines(4);
@@ -241,10 +246,10 @@ void fill_lane_lines(cereal::ModelDataV2::Builder &framed, const std::array<floa
   fill_xyzt(lane_lines[3], plan_t, X_IDXS_FLOAT, right_far_y, right_far_z);
 
   framed.setLaneLineStds({
-    exp(lanes.std.left_far[0].y),
-    exp(lanes.std.left_near[0].y),
-    exp(lanes.std.right_near[0].y),
-    exp(lanes.std.right_far[0].y),
+    exp(left_far.std[0].y),
+    exp(left_near.std[0].y),
+    exp(right_near.std[0].y),
+    exp(right_far.std[0].y),
   });
 
   framed.setLaneLineProbs({

selfdrive/modeld/models/driving.h

@@ -28,6 +28,8 @@ constexpr int META_STRIDE = 7;
 constexpr int PLAN_MHP_N = 5;
 constexpr int STOP_LINE_MHP_N = 3;
 
+constexpr int LANELINES_MHP_N = 4;
+
 constexpr int LEAD_MHP_N = 2;
 constexpr int LEAD_TRAJ_LEN = 6;
 constexpr int LEAD_PRED_DIM = 4;
@@ -77,14 +79,6 @@ struct ModelOutputPlans {
 };
 static_assert(sizeof(ModelOutputPlans) == sizeof(ModelOutputPlanPrediction)*PLAN_MHP_N);
 
-struct ModelOutputLinesXY {
-  std::array<ModelOutputYZ, TRAJECTORY_SIZE> left_far;
-  std::array<ModelOutputYZ, TRAJECTORY_SIZE> left_near;
-  std::array<ModelOutputYZ, TRAJECTORY_SIZE> right_near;
-  std::array<ModelOutputYZ, TRAJECTORY_SIZE> right_far;
-};
-static_assert(sizeof(ModelOutputLinesXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*4);
-
 struct ModelOutputLineProbVal {
   float val_deprecated;
   float val;
@@ -99,12 +93,28 @@ struct ModelOutputLinesProb {
 };
 static_assert(sizeof(ModelOutputLinesProb) == sizeof(ModelOutputLineProbVal)*4);
 
-struct ModelOutputLaneLines {
-  ModelOutputLinesXY mean;
-  ModelOutputLinesXY std;
-  ModelOutputLinesProb prob;
+struct ModelOutputLaneLinesElement {
+  std::array<ModelOutputYZ, TRAJECTORY_SIZE> mean;
+  std::array<ModelOutputYZ, TRAJECTORY_SIZE>  std;
+  std::array<float, LANELINES_MHP_N> prob;
 };
-static_assert(sizeof(ModelOutputLaneLines) == (sizeof(ModelOutputLinesXY)*2) + sizeof(ModelOutputLinesProb));
+static_assert(sizeof(ModelOutputLaneLinesElement) == (sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*2) + sizeof(float)*LANELINES_MHP_N);
+
+struct ModelOutputLaneLines {
+  std::array<ModelOutputLaneLinesElement, LANELINES_MHP_N> prediction;
+  ModelOutputLinesProb prob;
+
+  constexpr const ModelOutputLaneLinesElement &get_lane_idx(int lane_idx) const {
+    int max_idx = 0;
+    for (int i = 1; i < prediction.size(); i++) {
+      if (prediction[i].prob[lane_idx] > prediction[max_idx].prob[lane_idx]) {
+        max_idx = i;
+      }
+    }
+    return prediction[max_idx];
+  }
+};
+static_assert(sizeof(ModelOutputLaneLines) == (sizeof(ModelOutputLaneLinesElement)*LANELINES_MHP_N) + (sizeof(ModelOutputLinesProb)));
 
 struct ModelOutputEdgessXY {
   std::array<ModelOutputYZ, TRAJECTORY_SIZE> left;