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
