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