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 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 plot_model(m, VM, v_ego, curvature, imgw, calibration, top_down, top_down_color=216): # Draw bar representing position and distribution of lead car from unfiltered vision model if top_down is not None: _, _ = to_lid_pt(m.lead, 0) _, py_top = to_lid_pt(m.lead + m.lead_std, 0) px, py_bottom = to_lid_pt(m.lead - m.lead_std, 0) top_down[1][int(round(px - 4)):int(round(px + 4)), py_top:py_bottom] = top_down_color if calibration is None: return if m.cpath.valid: draw_path(m.cpath.y, _PATH_XD, YELLOW, imgw, calibration, top_down, YELLOW) draw_var(m.cpath.y, _PATH_XD, m.cpath.std, YELLOW, imgw, calibration, top_down) dpath_poly, _, _ = calc_desired_path(m.lpath.poly, m.rpath.poly, m.cpath.poly, m.lpath.prob, m.rpath.prob, m.cpath.prob, v_ego) dpath_poly = np.array(dpath_poly) dpath_y = np.polyval(dpath_poly, _PATH_X) draw_path(dpath_y, _PATH_X, RED, imgw, calibration, top_down, RED) if m.lpath.valid: color = (0, int(255 * m.lpath.prob), 0) draw_path(m.lpath.y, _PATH_XD, color, imgw, calibration, top_down, YELLOW) draw_var(m.lpath.y, _PATH_XD, m.lpath.std, color, imgw, calibration, top_down) if m.rpath.valid: color = (0, int(255 * m.rpath.prob), 0) draw_path(m.rpath.y, _PATH_XD, color, imgw, calibration, top_down, YELLOW) draw_var(m.rpath.y, _PATH_XD, m.rpath.std, color, imgw, calibration, top_down) if len(m.freepath) > 0: for i, p in enumerate(m.freepath): d = i*2 px, py = to_lid_pt(d, 0) cols = [36, 73, 109, 146, 182, 219, 255] if p >= 0.4: top_down[1][int(round(px - 4)):int(round(px + 4)), int(round(py - 4)):int(round(py + 4))] = find_color(top_down[0], (0, cols[int((p-0.4)*10)], 0)) elif p <= 0.2: top_down[1][int(round(px - 4)):int(round(px + 4)), int(round(py - 4)):int(round(py + 4))] = 192 #find_color(top_down[0], (192, 0, 0)) # draw user path from curvature draw_steer_path(v_ego, curvature, BLUE, imgw, calibration, top_down, VM, BLUE)
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