New lane planner (#1276)

* better lane planner

* works well on test

* needs speed now

* update ref

selfdrive/controls/lib/lane_planner.py

@@ -4,6 +4,7 @@ from cereal import log
 
 CAMERA_OFFSET = 0.06  # m from center car to camera
 
+
 def compute_path_pinv(l=50):
   deg = 3
   x = np.arange(l*1.0)
@@ -16,11 +17,22 @@ def model_polyfit(points, path_pinv):
   return np.dot(path_pinv, [float(x) for x in points])
 
 
-def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width):
+def eval_poly(poly, x):
+  return poly[3] + poly[2]*x + poly[1]*x**2 + poly[0]*x**3
+
+
+def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width, v_ego):
   # This will improve behaviour when lanes suddenly widen
+  # these numbers were tested on 2000segments and found to work well
   lane_width = min(4.0, lane_width)
-  l_prob = l_prob * interp(abs(l_poly[3]), [2, 2.5], [1.0, 0.0])
-  r_prob = r_prob * interp(abs(r_poly[3]), [2, 2.5], [1.0, 0.0])
+  width_poly = l_poly - r_poly
+  prob_mods = []
+  for t_check in [0.0, 1.5, 3.0]:
+    width_at_t = eval_poly(width_poly, t_check * (v_ego + 7))
+    prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0]))
+  mod = min(prob_mods)
+  l_prob = mod * l_prob
+  r_prob = mod * r_prob
 
   path_from_left_lane = l_poly.copy()
   path_from_left_lane[3] -= lane_width / 2.0
@@ -82,7 +94,7 @@ class LanePlanner():
     self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
                       (1 - self.lane_width_certainty) * speed_lane_width
 
-    self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width)
+    self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width, v_ego)
 
   def update(self, v_ego, md):
     self.parse_model(md)

selfdrive/controls/tests/test_lateral_mpc.py

@@ -25,7 +25,7 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
   p_p = poly_p.copy()
   p_p[3] += poly_shift
 
-  d_poly = calc_d_poly(p_l, p_r, p_p, l_prob, r_prob, lane_width)
+  d_poly = calc_d_poly(p_l, p_r, p_p, l_prob, r_prob, lane_width, v_ref)
 
   CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING")
   VM = VehicleModel(CP)

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-50d2dc24d26005104affe75b736169cd0505ecac
+dfe0776d21e460a8f8e98321f56f421418132620