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 calc_next_node(current, steer, direction, config, ox, oy, kdtree): """ 获取按照当前控制策略后到达的情况 :param current: :param steer: :param direction: :param config: :param ox: :param oy: :param kdtree: :return: """ x, y, yaw = current.xlist[-1], current.ylist[-1], current.yawlist[-1] arc_l = XY_GRID_RESOLUTION * 1.5 xlist, ylist, yawlist = [], [], [] for dist in np.arange(0, arc_l, MOTION_RESOLUTION): x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer) xlist.append(x) ylist.append(y) yawlist.append(yaw) if not check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree): return None d = direction == 1 xind = round(x / XY_GRID_RESOLUTION) yind = round(y / XY_GRID_RESOLUTION) yawind = round(yaw / YAW_GRID_RESOLUTION) addedcost = 0.0 if d != current.direction: addedcost += SB_COST # steer penalty addedcost += STEER_COST * abs(steer) # steer change penalty addedcost += STEER_CHANGE_COST * abs(current.steer - steer) cost = current.cost + addedcost + arc_l node = Node(xind, yind, yawind, d, xlist, ylist, yawlist, [d], pind=calc_index(current, config), cost=cost, steer=steer) return node
def calc_next_node(current, steer, direction, config, ox, oy, kd_tree): # pose of current node x, y, yaw = current.x_list[-1], current.y_list[-1], current.yaw_list[-1] arc_l = XY_GRID_RESOLUTION * 1.5 x_list, y_list, yaw_list = [], [], [] for _ in np.arange(0, arc_l, MOTION_RESOLUTION): x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer) x_list.append(x) y_list.append(y) yaw_list.append(yaw) if not check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree): return None d = direction == 1 # pose finally reached x_ind = round(x / XY_GRID_RESOLUTION) y_ind = round(y / XY_GRID_RESOLUTION) yaw_ind = round(yaw / YAW_GRID_RESOLUTION) added_cost = 0.0 # driving forward-backward change penalty if d != current.direction: added_cost += SB_COST # steer penalty added_cost += STEER_COST * abs(steer) # steer change penalty added_cost += STEER_CHANGE_COST * abs(current.steer - steer) # distance(time) penalty added_cost += arc_l cost = current.cost + added_cost node = Node( x_ind, y_ind, yaw_ind, d, x_list, y_list, yaw_list, [d], parent_index=calc_index(current, config), cost=cost, steer=steer) return node
def calc_next_node(current, steer, direction, config, ox, oy, kd_tree): x, y, yaw = current.x_list[-1], current.y_list[-1], current.yaw_list[-1] # todo 弧长arc_l的选择对路径生成的影响很重要,其取值影响车辆状态积分后的节点落在哪个栅格中,取值越大落在其他栅格中的可能性越大 arc_l = XY_GRID_RESOLUTION * 1.5 # 根据当前车辆状态与离散化的输入,积分推断朝s=arc_l的距离前进产生的状态序列[x,y,yaw] x_list, y_list, yaw_list = [], [], [] for _ in np.arange(0, arc_l, MOTION_RESOLUTION): x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer) x_list.append(x) y_list.append(y) yaw_list.append(yaw) # collision check 碰撞检测 if not check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree): return None d = direction == 1 x_ind = round(x / XY_GRID_RESOLUTION) y_ind = round(y / XY_GRID_RESOLUTION) yaw_ind = round(yaw / YAW_GRID_RESOLUTION) # 相对父节点增加的cost added_cost = 0.0 # 调转车头的penalty(最大) if d != current.direction: added_cost += SB_COST # steer penalty added_cost += STEER_COST * abs(steer) # steer change penalty added_cost += STEER_CHANGE_COST * abs(current.steer - steer) # 父节点的cost + 移动距离 + switch back,steer,steer change penalty cost = current.cost + added_cost + arc_l node = Node(x_ind, y_ind, yaw_ind, d, x_list, y_list, yaw_list, [d], parent_index=calc_index(current, config), cost=cost, steer=steer) return node
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 calc_next_node(current, steer, direction, config, ox, oy, kd_tree): x, y, yaw = current.x_list[-1], current.y_list[-1], current.yaw_list[-1] arc_l = XY_GRID_RESOLUTION * 1.5 # arc_l = XY_GRID_RESOLUTION * 0.5 x_list, y_list, yaw_list = [], [], [] for _ in np.arange(0, arc_l, MOTION_RESOLUTION): x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer) # x, y, yaw = self.control_data['target_speed'] x_list.append(x) y_list.append(y) yaw_list.append(yaw) if not check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree): return None d = direction == 1 x_ind = round(x / XY_GRID_RESOLUTION) y_ind = round(y / XY_GRID_RESOLUTION) yaw_ind = round(yaw / YAW_GRID_RESOLUTION) added_cost = 0.0 if d != current.direction: added_cost += SB_COST # steer penalty added_cost += STEER_COST * abs(steer) # steer change penalty added_cost += STEER_CHANGE_COST * abs(current.steer - steer) cost = current.cost + added_cost + arc_l node = Node(x_ind, y_ind, yaw_ind, d, x_list, y_list, yaw_list, [d], parent_index=calc_index(current, config), cost=cost, steer=steer) return node
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 calc_next_node(current, steer, direction, config, ox, oy, kdtree): """ Calculate cost and indices of the node with steer, direction primitives. - current : current node - steer : steer primitives - direction : direction primitives """ x, y, yaw = current.xlist[-1], current.ylist[-1], current.yawlist[-1] arc_l = XY_GRID_RESOLUTION * 1.5 xlist, ylist, yawlist = [], [], [] for dist in np.arange(0, arc_l, MOTION_RESOLUTION): x, y, yaw = move(x, y, yaw, MOTION_RESOLUTION * direction, steer) xlist.append(x) ylist.append(y) yawlist.append(yaw) if not check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree): return None # d = direction == 1 # check whether direction control input is forward of not. xind = round(x / XY_GRID_RESOLUTION) yind = round(y / XY_GRID_RESOLUTION) yawind = round(yaw / YAW_GRID_RESOLUTION) addedcost = 0.0 # # direction switch penalty # if d != current.direction: # addedcost += SB_COST # direction switch penalty if current.direction != direction: addedcost += SB_COST # backward drive penalty if direction != 1: addedcost += BACK_COST # steer penalty addedcost += STEER_COST * abs(steer) # steer change penalty addedcost += STEER_CHANGE_COST * abs(current.steer - steer) cost = current.cost + addedcost + arc_l node = Node(xind, yind, yawind, direction, xlist, ylist, yawlist, [direction], pind=calc_index(current, config), cost=cost, steer=steer) # node = Node(xind, yind, yawind, d, xlist, # ylist, yawlist, [d], # pind=calc_index(current, config), # cost=cost, steer=steer) return node
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