def analytic_expansion(current, goal, ox, oy, kd_tree): start_x = current.x_list[-1] start_y = current.y_list[-1] start_yaw = current.yaw_list[-1] goal_x = goal.x_list[-1] goal_y = goal.y_list[-1] goal_yaw = goal.yaw_list[-1] max_curvature = math.tan(MAX_STEER) / WB paths = rs.calc_paths(start_x, start_y, start_yaw, goal_x, goal_y, goal_yaw, max_curvature, step_size=MOTION_RESOLUTION) if not paths: return None best_path, best = None, None for path in paths: if check_car_collision(path.x, path.y, path.yaw, ox, oy, kd_tree): cost = calc_rs_path_cost(path) if not best or best > cost: best = cost best_path = path return best_path
def analytic_expantion(current, goal, c, ox, oy, kdtree): sx = current.xlist[-1] sy = current.ylist[-1] syaw = current.yawlist[-1] gx = goal.xlist[-1] gy = goal.ylist[-1] gyaw = goal.yawlist[-1] max_curvature = math.tan(MAX_STEER) / WB paths = rs.calc_paths(sx, sy, syaw, gx, gy, gyaw, max_curvature, step_size=MOTION_RESOLUTION) if not paths: return None best_path, best = None, None for path in paths: if check_uav_collision(path.x, path.y, path.yaw, ox, oy, kdtree): cost = calc_rs_path_cost(path) if not best or best > cost: best = cost best_path = path return best_path
def analytic_expantion(current, goal, c, ox, oy, kdtree): """ 获取一条最优的rs路径 :param current: :param goal: :param c: :param ox: :param oy: :param kdtree: :return: """ sx = current.xlist[-1] sy = current.ylist[-1] syaw = current.yawlist[-1] gx = goal.xlist[-1] gy = goal.ylist[-1] gyaw = goal.yawlist[-1] max_curvature = math.tan(MAX_STEER) / WB # 最大曲率 paths = rs.calc_paths(sx, sy, syaw, gx, gy, gyaw, max_curvature, step_size=MOTION_RESOLUTION) # 获取rs曲线路径 if not paths: return None best_path, best = None, None for path in paths: # 筛选路径 if check_car_collision(path.x, path.y, path.yaw, ox, oy, kdtree): cost = calc_rs_path_cost(path) if not best or best > cost: best = cost best_path = path return best_path
def analytic_expansion(current, goal, ox, oy, kd_tree): """ Obtain the optimal path that meets physical feasibility, no collision, and the lowest cost. """ start_x = current.x_list[-1] start_y = current.y_list[-1] start_yaw = current.yaw_list[-1] goal_x = goal.x_list[-1] goal_y = goal.y_list[-1] goal_yaw = goal.yaw_list[-1] max_curvature = math.tan(MAX_STEER) / WB # Only consider curvature limit(due to physical feasiblity) # Obstacles are not considered here. paths = rs.calc_paths( start_x, start_y, start_yaw, goal_x, goal_y, goal_yaw, max_curvature, step_size=MOTION_RESOLUTION) if not paths: return None best_path, best = None, None for path in paths: if check_car_collision(path.x, path.y, path.yaw, ox, oy, kd_tree): cost = calc_rs_path_cost(path) if not best or best > cost: best = cost best_path = path return best_path
def analytic_expansion(current, goal, ox, oy, kd_tree, closedList, c): start_x = current.x_list[-1] start_y = current.y_list[-1] start_yaw = current.yaw_list[-1] goal_x = goal.x_list[-1] goal_y = goal.y_list[-1] goal_yaw = goal.yaw_list[-1] max_curvature = math.tan(MAX_STEER) / WB paths = rs.calc_paths(start_x, start_y, start_yaw, goal_x, goal_y, goal_yaw, max_curvature, step_size=MOTION_RESOLUTION) #print(len(paths)) if not paths: return None best_path, best = None, None vel = 0 actual_path = None for path in paths: if check_car_collision(path.x, path.y, path.yaw, ox, oy, kd_tree): cost = calc_rs_path_cost(path) #+cost from heightmap #print((cost,start_x,start_y,path.x[0],path.y[0],current.x_list[0],current.y_list[0])) #path is getting modified...? f_x = path.x[1:] f_y = path.y[1:] f_yaw = path.yaw[1:] f_cost = current.cost + cost f_parent_index = calc_index(current, c) #current.parent_index fd = [] for d in path.directions[1:]: fd.append(d >= 0) f_steer = 0.0 f_path = Node(current.x_index, current.y_index, current.yaw_index, current.direction, f_x, f_y, f_yaw, fd, cost=f_cost, parent_index=f_parent_index, steer=f_steer) temp_path = get_path(closedList, f_path) # print(len(temp_path.x_list)) # plt.plot(temp_path.x_list,temp_path.y_list,'.b') # plt.ylim(0,450) # plt.xlim(0,450) # global SAVE_INDEX # plt.savefig('debug/' + str(SAVE_INDEX)+ '.png') # plt.clf() # SAVE_INDEX +=1 m_cost, vel = infer_cost_from_model(temp_path) cost += m_cost #cost += infer_cost_height(temp_path) #just to validate that temp_path starts at start and ends at goal before running cost evaluation #print((cost,start_x,start_y,temp_path.x_list[0],temp_path.y_list[0],temp_path.x_list[-1],temp_path.y_list[-1])) #2400 # if cost < 1000 and (not best or best > cost): # print('\n\n\n\n') # best = cost # best_path = path if not best or best > cost: #print('\n\n\n\n') best = cost best_path = path actual_path = temp_path global MIN_BEST_COST, MIN_BEST_PATH, MIN_BEST_VEL, ACTUAL_BEST_PATH if best is not None: if MIN_BEST_COST is None: MIN_BEST_COST = best MIN_BEST_PATH = best_path MIN_BEST_VEL = vel ACTUAL_BEST_PATH = actual_path elif MIN_BEST_COST > best: MIN_BEST_COST = best MIN_BEST_PATH = best_path MIN_BEST_VEL = vel ACTUAL_BEST_PATH = actual_path # time_diff = time.perf_counter() - cur_time time_diff = time.clock() - cur_time #print(time_diff) if time_diff > MAXTIME: #print(type(MIN_BEST_PATH)) return MIN_BEST_PATH, MIN_BEST_VEL # print(cost) if best is not None and best < MIN_COST and USE_MIN_COST: return best_path, vel return None, None