def collision_risk(father_node, son_node, obstacle): """ :param father_node: :param son_node: :param obstacle: :return: """ begin = [father_node.x, father_node.y, 0.0 * math.pi / 180.0] end = [son_node.x, son_node.y, 0.0 * math.pi / 180.0] s, rho, theta = cubic.Polynomial(begin, end) dis = math.inf for i in range(0, len(s), 5): for j in range(len(obstacle)): tmp_dis = math.sqrt((s[i] - obstacle[j][0]) ** 2 + (rho[i] - obstacle[j][1]) ** 2) if tmp_dis < r_circle + obstacle_inflation: if dis > tmp_dis: dis = tmp_dis break cost = 1/(dis + 0.0001) return cost
def ToPathOptimizer2(efficients):
closed_set = dijkstra_planning(start_SRho, longi_step, latera_step, lateral_num, efficients)
# print('----------------------------------')
# print('closed_set:', len(closed_set))
goal = determine_goal(closed_set)
rx, ry = calc_final_path(goal, closed_set)
# print("rx, ry: %s" % rx, ry)
tmp_s = []
tmp_rho = []
tmp_thetaRho = []
for i in range(len(rx) - 1):
point_s = [rx[-(i + 1)], ry[-(i + 1)], 0.0 * math.pi / 180.0]
point_e = [rx[-(i + 2)], ry[-(i + 2)], 0.0 * math.pi / 180.0]
s, rho, thetaRho = cubic.Polynomial(point_s, point_e)
tmp_s.extend(s)
tmp_rho.extend(rho)
tmp_thetaRho.extend(thetaRho)
x = []
y = []
theta = []
if show_rough_path:
# print(s)
for j in range(len(tmp_s)):
tmpX, tmpY, tmpTheta = ftc.frenetToXY(tmp_s[j], tmp_rho[j], tmp_thetaRho[j], efficients)
x.append(tmpX)
y.append(tmpY)
theta.append(tmpTheta)
# plt.scatter(end_set[0, i][0], end_set[0, i][1], color='b', s=2, alpha=0.8)
plt.plot(x, y, 'magenta', linewidth=0.4, alpha=1)
return tmp_s, tmp_rho, tmp_thetaRho
def plotkappaGraph():
efficients = cubicSpline.saveEfficients()
closed_set = dijkstra_planning(begin, longi_step, latera_step, lateral_num,
efficients)
# print('----------------------------------')
# print('closed_set:', len(closed_set))
goal = determine_goal(closed_set)
rx, ry = calc_final_path(goal, closed_set)
# print("rx, ry: %s" % rx, ry)
tmp_s = []
tmp_rho = []
tmp_theta = []
for i in range(len(rx) - 1):
point_s = [rx[-(i + 1)], ry[-(i + 1)], 0.0 * math.pi / 180.0]
point_e = [rx[-(i + 2)], ry[-(i + 2)], 0.0 * math.pi / 180.0]
s, rho, theta = cubic.Polynomial(point_s, point_e)
tmp_s.extend(s)
tmp_rho.extend(rho)
tmp_theta.extend(theta)
tmp_kappa = trajectory_kappa(tmp_rho, tmp_s)
plt.plot(tmp_s[0:-2],
tmp_kappa,
c='blue',
linestyle="-",
linewidth=0.8,
alpha=1,
label='Rough Path')
def trajectory_kappa(father_node, son_node, efficients):
begin = [father_node.x, father_node.y, 0.0 * math.pi / 180.0]
end = [son_node.x, son_node.y, 0.0 * math.pi / 180.0]
s, rho, thetaRho = cubic.Polynomial(begin, end)
tmp_kappa = []
for i in range(0, len(s), 5):
x0, y0, theta0 = ftc.frenetToXY(s[i], rho[i], thetaRho[i], efficients)
x1, y1, theta1 = ftc.frenetToXY(s[i + 1], rho[i + 1], thetaRho[i + 1],
efficients)
x2, y2, theta2 = ftc.frenetToXY(s[i + 2], rho[i + 2], thetaRho[i + 2],
efficients)
k1 = (x1 - x0) * (y2 - 2 * y1 + y0)
k2 = (y1 - y0) * (x2 - 2 * x1 + x0)
k3 = ((x1 - x0)**2 + (y1 - y0)**2)**(3.0 / 2.0)
if (k3 == 0.0):
tmp_kappa.append(0)
else:
tmp_kappa.append((k1 - k2) / k3)
sum_kappa = 0
for i in range(len(tmp_kappa)):
sum_kappa = sum_kappa + tmp_kappa[i]**2
mean_kappa = sum_kappa / len(tmp_kappa)
return mean_kappa
def generate_lattice(efficients):
end_set = sampling(longitudinal_num, lateral_num, lateral_step,
longitudinal_step)
# print(end_set)
end_size = end_set.shape
# print("end_size:")
# print(end_size)
# 生成车辆起点到第一列采样点的图
for i in range(end_size[1]):
s, rho, thetaRho = cubic.Polynomial(start_SRho, end_set[0, i])
x = []
y = []
# print(s)
for j in range(len(s)):
tmpX, tmpY, tmpTheta = ftc.frenetToXY(s[j], rho[j], thetaRho[j],
efficients)
x.append(tmpX)
y.append(tmpY)
# plt.scatter(end_set[0, i][0], end_set[0, i][1], color='b', s=2, alpha=0.8)
plt.plot(x, y, c='b', linewidth=0.2, alpha=1.0)
# 采样点之间的图
for i in range(end_size[0] - 1):
for j in range(end_size[1]):
# print([i, j])
for q in range(end_size[1]):
# mptg.test_optimize_trajectory(end_set[1, 0], end_set[0, 1])
s, rho, thetaRho = cubic.Polynomial(end_set[i, q],
end_set[i + 1, j])
x = []
y = []
# print(s)
for q in range(len(s)):
tmpX, tmpY, tmpTheta = ftc.frenetToXY(
s[q], rho[q], thetaRho[q], efficients)
x.append(tmpX)
y.append(tmpY)
# plt.scatter(end_set[i + 1, j][0], end_set[i + 1, j][1], color='b', s=2, alpha=0.8)
plt.plot(x, y, c='b', linewidth=0.1, alpha=0.80)
return None
def generate_lattice():
end_set = sampling(longi_num, lateral_num, lateral_step, longi_step)
# print(end_set)
end_size = end_set.shape
# print("end_size:")
# print(end_size)
# 生成车辆起点到第一列采样点的图
for i in range(end_size[1]):
s, rho, theta = cubicp.Polynomial(start, end_set[0, i])
plt.scatter(end_set[0, i][0],
end_set[0, i][1],
color='b',
s=2,
alpha=0.8,
label='sampling points')
plt.plot(s,
rho,
'r',
linewidth=0.4,
alpha=0.8,
label='cubic polynomials')
# 采样点之间的图
for i in range(end_size[0] - 1):
for j in range(end_size[1]):
# print([i, j])
for q in range(end_size[1]):
# mptg.test_optimize_trajectory(end_set[1, 0], end_set[0, 1])
s, rho, theta = cubicp.Polynomial(end_set[i, q], end_set[i + 1,
j])
plt.scatter(end_set[i + 1, j][0],
end_set[i + 1, j][1],
color='b',
s=2,
alpha=0.8)
plt.plot(s, rho, 'r', linewidth=0.4, alpha=0.8)
def plotGraph():
if showLane:
lane.lane()
if showObstacle:
for i in range(len(obstacle)):
car.simVehicle([obstacle[i][0], obstacle[i][1]], 0 * math.pi / 180,
'r', 1)
if showRefLine:
x = [i for i in range(100)]
refline = [refLineRho for i in range(100)]
plt.plot(x,
refline,
c='green',
linestyle="--",
linewidth=1.2,
alpha=1,
label='Reference Line')
# plt.legend(loc = 1)
# sampling_point = PathLattice.sampling(longitudinal_num, lateral_num, latera_step, longitudinal_step)
# 用来显示lattice图像
# PathLattice.generate_lattice()
efficients = cubicSpline.saveEfficients()
closed_set = dijkstra_planning(begin, longi_step, latera_step, lateral_num,
efficients)
# print('----------------------------------')
# print('closed_set:', len(closed_set))
goal = determine_goal(closed_set)
rx, ry = calc_final_path(goal, closed_set)
# print("rx, ry: %s" % rx, ry)
tmp_s = []
tmp_rho = []
tmp_theta = []
for i in range(len(rx) - 1):
point_s = [rx[-(i + 1)], ry[-(i + 1)], 0.0 * math.pi / 180.0]
point_e = [rx[-(i + 2)], ry[-(i + 2)], 0.0 * math.pi / 180.0]
s, rho, theta = cubic.Polynomial(point_s, point_e)
tmp_s.extend(s)
tmp_rho.extend(rho)
tmp_theta.extend(theta)
plt.plot(tmp_s, tmp_rho, c='b', linewidth=0.8, label='Rough Path')
# plt.legend(loc=2)
if showVehicleAnimation:
car.simVehicle([begin[0], begin[1]], 0 * math.pi / 180, 'g', 1)
time_stamp = 0
for j in range(0, len(tmp_s), 30):
time_stamp = tmp_s[j] / s_max
car.simVehicle([tmp_s[j], tmp_rho[j]], tmp_theta[j], 'b',
time_stamp)
plt.pause(0.001)
font1 = {
'family': 'Times New Roman',
'weight': 'normal',
'size': 10,
}
plt.xlabel("x (m)", font1)
plt.ylabel('y (m)', font1)
plt.xticks(fontproperties='Times New Roman', fontsize=10)
plt.yticks(fontproperties='Times New Roman', fontsize=10)
plt.xlim(-1, 110)
plt.ylim(-4, 20)
plt.axis("equal")
plt.savefig('../SimGraph/pathPlanner060502.tiff', dpi=600)
def plotGraph():
print(__file__ + " start!!")
plt.figure(figsize=(3.5, 3.0)) # 单位英寸， 3.5
p1 = [0.15, 0.15, 0.80, 0.8]
plt.axes(p1)
plt.axis("equal")
plt.grid(linestyle="--", linewidth=0.5, alpha=1)
# # 计算多车道环境的弧长参数曲线的系数
# efficients = multiLane.saveEfficients()
# 计算回环环境的弧长参数曲线的系数
efficients = cubicSpline.saveEfficients()
# if show_lane:
# # show multilane
# mlane.curvePath()
if show_obstacle:
for i in range(len(obstacle)):
x, y, theta = ftc.frenetToXY(obstacle[i][0], obstacle[i][1], obstacleHeading, efficients)
car.simVehicle([x, y], theta, 'r', 0.8)
# sampling_point = PathLattice.sampling(longitudinal_num, lateral_num, latera_step, longitudinal_step)
# 用来显示lattice图像
# PathLattice.generate_lattice()
closed_set = dijkstra_planning(start_SRho, longi_step, latera_step, lateral_num, efficients)
# print('----------------------------------')
# print('closed_set:', len(closed_set))
goal = determine_goal(closed_set)
rx, ry = calc_final_path(goal, closed_set)
# print("rx, ry: %s" % rx, ry)
tmp_s = []
tmp_rho = []
tmp_thetaRho = []
for i in range(len(rx) - 1):
point_s = [rx[-(i + 1)], ry[-(i + 1)], 0.0 * math.pi / 180.0]
point_e = [rx[-(i + 2)], ry[-(i + 2)], 0.0 * math.pi / 180.0]
s, rho, thetaRho = cubic.Polynomial(point_s, point_e)
tmp_s.extend(s)
tmp_rho.extend(rho)
tmp_thetaRho.extend(thetaRho)
if showPathLattice:
pathlattice.ToPathPlanner2(efficients)
x = []
y = []
theta = []
if show_rough_path:
# print(s)
for j in range(len(tmp_s)):
tmpX, tmpY, tmpTheta = ftc.frenetToXY(tmp_s[j], tmp_rho[j], tmp_thetaRho[j], efficients)
x.append(tmpX)
y.append(tmpY)
theta.append(tmpTheta)
# plt.scatter(end_set[0, i][0], end_set[0, i][1], color='b', s=2, alpha=0.8)
plt.plot(x, y, 'magenta', linewidth=0.5, alpha=1)
if show_vehicle_animation:
x0, y0, theta0 = ftc.frenetToXY(start_SRho[0], start_SRho[1], start_SRho[2], efficients)
car.simVehicle([x0, y0], theta0, 'g', 1)
time_stamp = 0
for j in range(0, len(tmp_s), 50):
time_stamp = tmp_s[j] / s_max
car.simVehicle([x[j], y[j]], theta[j], 'b', time_stamp)
plt.pause(0.01)
font1 = {'family': 'Times New Roman',
'weight': 'normal',
'size': 10,
}
plt.xlabel("x (m)", font1)
plt.ylabel('y (m)', font1)
plt.xticks(fontproperties='Times New Roman', fontsize=10)
plt.yticks(fontproperties='Times New Roman', fontsize=10)
plt.xlim(-10, 30)
plt.ylim(-5, 80)
# plt.savefig('/home/ming/桌面/PTPSim/SimGraph/pathPlannerXY_5_30_8_48.svg')
plt.savefig('../SimGraph/pathPlanner2_061101.tiff', dpi=600)
plt.show()
def plotGraph():
print(__file__ + " start!!")
plt.figure(figsize=(3.5, 3.5 * 0.62)) # 单位英寸， 3.5
plt.axes([0.2, 0.2, 0.7, 0.7])
plt.axis("equal")
plt.grid(linestyle="--", linewidth=0.5, alpha=1)
# 计算回环环境的弧长参数曲线的系数
efficients = cubicSpline.saveEfficients()
pathlattice.ToPathPlanner(efficients)
if show_obstacle:
c = ['r', 'r', 'r', 'darkorange']
for i in range(len(obstacle)):
tmpx, tmpy, tmptheta = ftc.frenetToXY(obstacle[i][0],
obstacle[i][1],
obstacleHeading, efficients)
car.simVehicle([tmpx, tmpy], tmptheta, c[i], 0.8)
closed_set = dijkstra_planning(start_SRho, longi_step, latera_step,
lateral_num, efficients)
# print('----------------------------------')
# print('closed_set:', len(closed_set))
goal = determine_goal(closed_set)
rx, ry = calc_final_path(goal, closed_set)
# print("rx, ry: %s" % rx, ry)
tmp_s = []
tmp_rho = []
tmp_thetaRho = []
for i in range(len(rx) - 1):
point_s = [rx[-(i + 1)], ry[-(i + 1)], 0.0 * math.pi / 180.0]
point_e = [rx[-(i + 2)], ry[-(i + 2)], 0.0 * math.pi / 180.0]
s, rho, thetaRho = cubic.Polynomial(point_s, point_e)
tmp_s.extend(s)
tmp_rho.extend(rho)
tmp_thetaRho.extend(thetaRho)
x = []
y = []
theta = []
if show_rough_path:
# print(s)
for j in range(len(tmp_s)):
tmpX, tmpY, tmpTheta = ftc.frenetToXY(tmp_s[j], tmp_rho[j],
tmp_thetaRho[j], efficients)
x.append(tmpX)
y.append(tmpY)
theta.append(tmpTheta)
# plt.scatter(end_set[0, i][0], end_set[0, i][1], color='b', s=2, alpha=0.8)
plt.plot(x, y, 'magenta', linewidth=0.6, alpha=1)
if show_vehicle_animation:
x0, y0, theta0 = ftc.frenetToXY(start_SRho[0], start_SRho[1],
start_SRho[2], efficients)
car.simVehicle([x0, y0], theta0, 'b', 1)
for j in range(0, len(tmp_s), 50):
time_stamp = tmp_s[j] / s_end
car.simVehicle([x[j], y[j]], theta[j], 'b', time_stamp)
plt.pause(0.001)
font1 = {
'family': 'Times New Roman',
'weight': 'normal',
'size': 10,
}
plt.xlabel("x (m)", font1)
plt.ylabel('y (m)', font1)
plt.xticks(fontproperties='Times New Roman', fontsize=10)
plt.yticks(fontproperties='Times New Roman', fontsize=10)
plt.xlim(120, 200)
plt.ylim(90, 140)
# plt.savefig('/home/ming/桌面/PTPSim/SimGraph/pathPlannerXY_5_30_8_48.svg')
plt.savefig('../SimGraph/pathPlanner4_061102.tiff', dpi=600)
plt.show()
closed_set = dijkstra_planning(start_SRho, longi_step, latera_step, lateral_num, efficients)
# print('----------------------------------')
# print('closed_set:', len(closed_set))
goal = determine_goal(closed_set)
rx, ry = calc_final_path(goal, closed_set)
# print("rx, ry: %s" % rx, ry)
tmp_s = []
tmp_rho = []
tmp_thetaRho = []
for i in range(len(rx) - 1):
point_s = [rx[-(i + 1)], ry[-(i + 1)], 0.0 * math.pi / 180.0]
point_e = [rx[-(i + 2)], ry[-(i + 2)], 0.0 * math.pi / 180.0]
s, rho, thetaRho = cubic.Polynomial(point_s, point_e)
tmp_s.extend(s)
tmp_rho.extend(rho)
tmp_thetaRho.extend(thetaRho)
x = []
y = []
theta = []
if show_rough_path:
# print(s)
for j in range(len(tmp_s)):
tmpX, tmpY, tmpTheta = mlane.frenetToXY(tmp_s[j],tmp_rho[j], tmp_thetaRho[j], efficients)
x.append(tmpX)
y.append(tmpY)
theta.append(tmpTheta)