def update(self, v_ego, md):
if md is not None:
p_poly = model_polyfit(md.model.path.points,
self._path_pinv) # predicted path
l_poly = model_polyfit(md.model.leftLane.points,
self._path_pinv) # left line
r_poly = model_polyfit(md.model.rightLane.points,
self._path_pinv) # right line
# only offset left and right lane lines; offsetting p_poly does not make sense
l_poly[3] += CAMERA_OFFSET
r_poly[3] += CAMERA_OFFSET
p_prob = 1. # model does not tell this probability yet, so set to 1 for now
l_prob = md.model.leftLane.prob # left line prob
r_prob = md.model.rightLane.prob # right line prob
# Find current lanewidth
lr_prob = l_prob * r_prob
self.lane_width_certainty += 0.05 * (lr_prob -
self.lane_width_certainty)
current_lane_width = abs(l_poly[3] - r_poly[3])
self.lane_width_estimate += 0.005 * (current_lane_width -
self.lane_width_estimate)
speed_lane_width = interp(v_ego, [0., 14., 20.],
[2.5, 3., 3.5]) # German Standards
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
(1 - self.lane_width_certainty) * speed_lane_width
lane_width_diff = abs(self.lane_width - current_lane_width)
lane_r_prob = interp(lane_width_diff, [0.3, 1.0], [1.0, 0.0])
r_prob *= lane_r_prob
self.lead_dist = md.model.lead.dist
self.lead_prob = md.model.lead.prob
self.lead_var = md.model.lead.std**2
# compute target path
self.d_poly, self.c_poly, self.c_prob = calc_desired_path(
l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego,
self.lane_width)
self.r_poly = r_poly
self.r_prob = r_prob
self.l_poly = l_poly
self.l_prob = l_prob
self.p_poly = p_poly
self.p_prob = p_prob
def parse_model(self, md):
if len(md.leftLane.poly):
self.l_poly = np.array(md.leftLane.poly)
self.r_poly = np.array(md.rightLane.poly)
self.p_poly = np.array(md.path.poly)
else:
self.l_poly = model_polyfit(md.leftLane.points,
self._path_pinv) # left line
self.r_poly = model_polyfit(md.rightLane.points,
self._path_pinv) # right line
self.p_poly = model_polyfit(md.path.points,
self._path_pinv) # predicted path
self.l_prob = md.leftLane.prob # left line prob
self.r_prob = md.rightLane.prob # right line prob
def update(self, v_ego, md):
if len(md.leftLane.poly):
l_poly = np.array(md.leftLane.poly)
r_poly = np.array(md.rightLane.poly)
p_poly = np.array(md.path.poly)
else:
l_poly = model_polyfit(md.leftLane.points,
self._path_pinv) # left line
r_poly = model_polyfit(md.rightLane.points,
self._path_pinv) # right line
p_poly = model_polyfit(md.path.points,
self._path_pinv) # predicted path
# only offset left and right lane lines; offsetting p_poly does not make sense
l_poly[3] += CAMERA_OFFSET
r_poly[3] += CAMERA_OFFSET
p_prob = 1. # model does not tell this probability yet, so set to 1 for now
l_prob = md.leftLane.prob # left line prob
r_prob = md.rightLane.prob # right line prob
# Find current lanewidth
lr_prob = l_prob * r_prob
self.lane_width_certainty += 0.05 * (lr_prob -
self.lane_width_certainty)
current_lane_width = abs(l_poly[3] - r_poly[3])
self.lane_width_estimate += 0.005 * (current_lane_width -
self.lane_width_estimate)
speed_lane_width = interp(v_ego, [0., 31.], [3., 3.8])
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
(1 - self.lane_width_certainty) * speed_lane_width
self.lead_dist = md.lead.dist
self.lead_prob = md.lead.prob
self.lead_var = md.lead.std**2
# compute target path
self.d_poly, self.c_poly, self.c_prob = calc_desired_path(
l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego,
self.lane_width)
self.r_poly = r_poly
self.r_prob = r_prob
self.l_poly = l_poly
self.l_prob = l_prob
self.p_poly = p_poly
self.p_prob = p_prob
def __init__(self, model_path):
if len(model_path.points) == 0:
self.valid = False
return
self.poly = model_polyfit(model_path.points, _PATH_PINV)
self.prob = model_path.prob
self.std = model_path.std
self.y = np.polyval(self.poly, _PATH_XD)
self.valid = True
def update(self, v_ego, md, LaC=None):
if md is not None:
p_poly = model_polyfit(md.model.path.points,
self._path_pinv) # predicted path
l_poly = model_polyfit(md.model.leftLane.points,
self._path_pinv) # left line
r_poly = model_polyfit(md.model.rightLane.points,
self._path_pinv) # right line
try:
if LaC is not None and LaC.angle_steers_des_mpc != 0.0:
angle_error = LaC.angle_steers_des_mpc - (
0.05 * LaC.avg_angle_steers + LaC.steerActuatorDelay *
LaC.projected_angle_steers) / (LaC.steerActuatorDelay +
0.05)
else:
angle_error = 0.0
if angle_error != 0.0:
LaC.lateral_error = 1.0 * np.clip(
v_ego * (LaC.steerActuatorDelay + 0.05) *
math.tan(math.radians(angle_error)), -1.2, 1.2)
lateral_error = LaC.lateral_error
else:
lateral_error = 0.0
except:
lateral_error = 0.0
# only offset left and right lane lines; offsetting p_poly does not make sense
l_poly[3] += CAMERA_OFFSET - lateral_error
r_poly[3] += CAMERA_OFFSET - lateral_error
p_prob = 1. # model does not tell this probability yet, so set to 1 for now
l_prob = md.model.leftLane.prob # left line prob
r_prob = md.model.rightLane.prob # right line prob
# Find current lanewidth
lr_prob = l_prob * r_prob
self.lane_width_certainty += 0.05 * (lr_prob -
self.lane_width_certainty)
current_lane_width = abs(l_poly[3] - r_poly[3])
self.lane_width_estimate += 0.005 * (current_lane_width -
self.lane_width_estimate)
speed_lane_width = interp(v_ego, [0., 31.], [3., 3.8])
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
(1 - self.lane_width_certainty) * speed_lane_width
lane_width_diff = abs(self.lane_width - current_lane_width)
lane_r_prob = interp(lane_width_diff, [0.3, 1.0], [1.0, 0.0])
r_prob *= lane_r_prob
self.lead_dist = md.model.lead.dist
self.lead_prob = md.model.lead.prob
self.lead_var = md.model.lead.std**2
# compute target path
self.d_poly, self.c_poly, self.c_prob = calc_desired_path(
l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego,
self.lane_width)
self.r_poly = r_poly
self.r_prob = r_prob
self.l_poly = l_poly
self.l_prob = l_prob
self.p_poly = p_poly
self.p_prob = p_prob
def update(self, v_ego, md):
if len(md.leftLane.poly):
l_poly = np.array(md.leftLane.poly)
r_poly = np.array(md.rightLane.poly)
#p_poly = np.array(md.path.poly)
p_poly = np.array([0., 0., 0., 0.])
else:
l_poly = model_polyfit(md.leftLane.points,
self._path_pinv) # left line
r_poly = model_polyfit(md.rightLane.points,
self._path_pinv) # right line
p_poly = model_polyfit(md.path.points,
self._path_pinv) # predicted path
#l_poly[2] *= ((l_poly[3] - CAMERA_OFFSET) / max(0.01, l_poly[3]))
#r_poly[2] *= ((r_poly[3] - CAMERA_OFFSET) / min(-0.01, r_poly[3]))
# only offset left and right lane lines; offsetting p_poly does not make sense
l_poly[3] += CAMERA_OFFSET
r_poly[3] += CAMERA_OFFSET
l_prob = md.leftLane.prob # left line prob
r_prob = md.rightLane.prob # right line prob
if self.l_prob > l_prob:
l_poly = (l_poly * l_prob +
min(1.0 - l_prob, self.l_prob) * self.l_poly) / (
l_prob + min(1.0 - l_prob, self.l_prob) + 0.0001)
l_prob = (l_prob**2 +
min(1.0 - l_prob, 0.9 * self.l_prob) * self.l_prob) / (
l_prob + min(1 - l_prob, 0.9 * self.l_prob) + 0.0001)
if self.r_prob > r_prob:
r_poly = (r_poly *
(r_prob) + min(1.0 - r_prob, self.r_prob) * self.r_poly
) / (r_prob + min(1.0 - r_prob, self.r_prob) + 0.0001)
r_prob = (
r_prob**2 + min(1.0 - r_prob, 0.9 * self.r_prob) * self.r_prob
) / (r_prob + min(1.0 - r_prob, 0.9 * self.r_prob) + 0.0001)
# Find current lanewidth
lr_prob = l_prob * r_prob
self.lane_width_certainty += 0.05 * (lr_prob -
self.lane_width_certainty)
current_lane_width = abs(l_poly[3] - r_poly[3])
self.lane_width_estimate += 0.005 * (current_lane_width -
self.lane_width_estimate)
speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
(1 - self.lane_width_certainty) * speed_lane_width
half_lane_width = self.lane_width / 2.0
l_center = l_prob * (l_poly[3] - half_lane_width)
r_center = r_prob * (r_poly[3] + half_lane_width)
p_prob = 0.0001 + (self.c_prob**2 + self.p_prob**2) / (
self.c_prob + self.p_prob + 0.0001)
self.p_poly[3] = clip(0.0, self.p_poly[3] - 0.005,
self.p_poly[3] + 0.005)
self.p_poly[2] = clip(0.0, self.p_poly[2] - 0.001,
self.p_poly[2] + 0.001)
self.p_poly[1] = clip(0.0, self.p_poly[1] - 0.0001,
self.p_poly[1] + 0.0001)
#self.p_poly[0] = clip(0.0, self.p_poly[0] - 0.00001, self.p_poly[0] + 0.00001)
if l_center < 0.0 or r_center > 0.0:
if l_center > -r_center:
p_poly[3] = (r_center + p_prob * self.p_poly[3]) / (
r_prob + p_prob + 0.0001)
else:
p_poly[3] = (l_center + p_prob * self.p_poly[3]) / (
l_prob + p_prob + 0.0001)
race_line_adjust = interp(abs(p_poly[3]), [0.0, 0.3], [0.0, 1.0])
l_race_poly = (race_line_adjust * l_poly * l_prob +
p_prob * self.p_poly) / (race_line_adjust * l_prob +
p_prob + 0.0001)
r_race_poly = (race_line_adjust * r_poly * r_prob +
p_prob * self.p_poly) / (race_line_adjust * r_prob +
p_prob + 0.0001)
if self.d_poly[1] < 0.0:
p_poly[2] = min(l_race_poly[2], r_race_poly[2], self.p_poly[2])
p_poly[1] = min(l_race_poly[1], r_race_poly[1], self.d_poly[1])
#p_poly[0] = min(l_race_poly[0], r_race_poly[0], self.d_poly[0])
else:
p_poly[2] = max(l_race_poly[2], r_race_poly[2], self.p_poly[2])
p_poly[1] = max(l_race_poly[1], r_race_poly[1], self.d_poly[1])
#p_poly[0] = max(l_race_poly[0], r_race_poly[0], self.d_poly[0])
else:
p_poly[3] = self.p_poly[3]
p_poly[2] = self.p_poly[2]
p_poly[1] = self.d_poly[1]
#p_poly[0] = self.d_poly[0]
self.lead_dist = md.lead.dist
self.lead_prob = md.lead.prob
self.lead_var = md.lead.std**2
# compute target path
self.d_poly, self.c_poly, self.c_prob = calc_desired_path(
l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego,
self.lane_width)
self.r_poly = r_poly
self.r_prob = r_prob
self.l_poly = l_poly
self.l_prob = l_prob
self.p_poly = p_poly
self.p_prob = p_prob
