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