New lane planner (#1276)
* better lane planner * works well on test * needs speed now * update ref
This commit is contained in:
parent
d85c5bc66f
commit
5f8e3b4d93
|
@ -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)
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -1 +1 @@
|
|||
50d2dc24d26005104affe75b736169cd0505ecac
|
||||
dfe0776d21e460a8f8e98321f56f421418132620
|
Loading…
Reference in a new issue