def ToPathPlanner(efficients): # show sampling path # show sampling path s0 = 400 offset = 0.3 refLineRho = lane_width * 0.5 laneChaneRefLine = lane_width * 1.5 refline = refLineRho start_SRho, static_obs=parameters(s0, offset, refline) generate_lattice(efficients, refLineRho, start_SRho) generate_lattice(efficients, laneChaneRefLine, start_SRho) if showVehicleStart: tmpx, tmpy, tmptheta = ftc.frenetToXY(start_SRho[0], start_SRho[1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, 'b', 1) if show_obstacle: c = ['lightseagreen', 'orange', 'green'] for i in range(len(static_obs)): tmpx, tmpy, tmptheta = ftc.frenetToXY(static_obs[i][0], static_obs[i][1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, c[i], 1) s0 = 480 offset = 0.0 refline = laneChaneRefLine start_SRho, static_obs=parameters(s0, offset, refline) generate_lattice(efficients, refLineRho, start_SRho) generate_lattice(efficients, laneChaneRefLine, start_SRho) return None
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 boundValue(efficients):
# 等式约束
# 注意，这里生成的坐标位置是最右侧车道线的，不是参考线的
xr0, yr0, thetar0 = ftc.search_in_coefficients(s0, efficients)
s1 = s0 + reso_s
# xr1, yr1, thetar1 = ftc.search_in_coefficients(s1, efficients)
s2 = s0 + 2 * reso_s
xr2, yr2, thetar2 = ftc.frenetToXY(s2, refLineRho, 0, efficients)
x0, y0, theta0 = ftc.frenetToXY(start_SRho[0], start_SRho[1],
start_SRho[2], efficients)
print("x0, y0, theta0:", x0, y0, theta0)
rho_1 = reso_s * (math.tan(theta0 - thetar0)) + start_SRho[1]
x1, y1, theta1 = ftc.frenetToXY(s1, rho_1, 0, efficients)
print("x1, y1, theta1:", x1, y1, theta1)
m = kappa0 * ((x1 - x0)**2 + (y1 - y0)**2)**(3 / 2)
n = (x1 - x0) * (yr2 - 2 * y1 + y0) + (y1 - y0) * (xr2 - 2 * x1 + x0)
q = (x1 - x0) * math.sin(thetar2 + math.pi / 2.0) - (
y1 - y0) * math.cos(thetar2 + math.pi / 2.0)
rho_2 = (m - n) / q # 本来求出的是到参考线的距离
# 加上参考线到最右侧车道线的距离，是真正的偏移量，
# 这个问题debug了很久，一定要将方程设在参考线处，可以免去很多麻烦
rho_2 = rho_2 + refLineRho
print("rho1, rho2:", rho_1, rho_2)
return rho_1, rho_2
def plotGraph():
# plt.style.use('ggplot')
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)
# plt.rcParams['font.sans-serif'] = ['Times New Roman'] # 如果要显示中文字体，则在此处设为：SimHei
# plt.rcParams['axes.unicode_minus'] = False # 显示负号
# # show multilane
# multiLane.curvePath()
# # 计算多车道环境的弧长参数曲线的系数
# efficients = multiLane.saveEfficients()
# 计算回环环境的弧长参数曲线的系数
efficients = cubicSpline.saveEfficients()
# show sampling path
generate_lattice(efficients)
if show_obstacle:
c = ['r', 'r', 'r', 'darkorange']
for i in range(len(static_obs)):
tmpx, tmpy, tmptheta = ftc.frenetToXY(static_obs[i][0], static_obs[i][1], obstacleHeading,
efficients)
car.simVehicle([tmpx, tmpy], tmptheta, c[i], 1)
if showVehicleStart:
tmpx, tmpy, tmptheta = ftc.frenetToXY(start_SRho[0], start_SRho[1], obstacleHeading, efficients)
car.simVehicle([tmpx, tmpy], tmptheta, 'b', 1)
font1 = {'family': 'Times New Roman',
'weight': 'normal',
'size': 10,
}
plt.xlabel("x (m)", font1)
plt.ylabel('y (m)', font1)
# 设置坐标刻度值的大小以及刻度值的字体
plt.tick_params(labelsize=10)
# x = np.array([0.0, 10.0, 20.0, 30.0, 40.0, 50.0])
# x = np.array([0.0, 20.0, 40.0, 60.0, 80.0, 100.0])
# y = np.array([-2.0, -1, 0.0, 1.0, 2.0])
# y = np.array([-2.0, -1, 0.0, 1.0, 2.0])
# xgroup_labels = ['0.0', '20.0', '40.0', '60.0', '80.0', '100.0'] # x轴刻度的标识
# ygroup_labels = ['-2.0', '-1.0', '0.0', '1.0', '2.0'] # y轴刻度的标识
# plt.xticks(x, xgroup_labels, fontproperties='Times New Roman', fontsize=10) # 默认字体大小为10
# plt.yticks(y, ygroup_labels, fontproperties='Times New Roman', fontsize=10)
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('../SimGraph/pathLatticeXY4_061202.tiff', dpi=600)
plt.show()
def inequality_cons(rho, s, optimization, efficients):
# 曲率约束
for i in range(n_s):
x0, y0, theta0 = ftc.frenetToXY(s[i], rho[i], 0, efficients)
x1, y1, theta1 = ftc.frenetToXY(s[i + 1], rho[i + 1], 0, efficients)
x2, y2, theta2 = ftc.frenetToXY(s[i + 2], rho[i + 2], 0, 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)
kappa1 = -k1 + k2 + k3 * kappa_max
kappa2 = k1 - k2 + k3 * kappa_max
optimization.subject_to(kappa1 >= 0)
optimization.subject_to(kappa2 >= 0)
# 障碍物约束
for i in range(n_s + 3):
# the first obs
if ((s[i] - static_obs[0][0])**2 < 9):
c1_obs = (s[i] - static_obs[0][0])**2 + (rho[i] -
static_obs[0][1])**2
c2_obs = (s[i] + d_circle -
static_obs[0][0])**2 + (rho[i] - static_obs[0][1])**2
c3_obs = (s[i] + 2 * d_circle -
static_obs[0][0])**2 + (rho[i] - static_obs[0][1])**2
optimization.subject_to(c1_obs >= safe_distance**2)
optimization.subject_to(c2_obs >= safe_distance**2)
optimization.subject_to(c3_obs >= safe_distance**2)
# the second obs
if ((s[i] - static_obs[1][0])**2 < 9):
c1_obs2 = (s[i] - static_obs[1][0])**2 + (rho[i] -
static_obs[1][1])**2
c2_obs2 = (s[i] + d_circle -
static_obs[1][0])**2 + (rho[i] - static_obs[1][1])**2
c3_obs2 = (s[i] + 2 * d_circle -
static_obs[1][0])**2 + (rho[i] - static_obs[1][1])**2
optimization.subject_to(c1_obs2 >= safe_distance**2)
optimization.subject_to(c2_obs2 >= safe_distance**2)
optimization.subject_to(c3_obs2 >= safe_distance**2)
# the second obs
if ((s[i] - static_obs[2][0])**2 < 9):
c1_obs3 = (s[i] - static_obs[2][0])**2 + (rho[i] -
static_obs[2][1])**2
c2_obs3 = (s[i] + d_circle -
static_obs[2][0])**2 + (rho[i] - static_obs[2][1])**2
c3_obs3 = (s[i] + 2 * d_circle -
static_obs[2][0])**2 + (rho[i] - static_obs[2][1])**2
optimization.subject_to(c1_obs3 >= safe_distance**2)
optimization.subject_to(c2_obs3 >= safe_distance**2)
optimization.subject_to(c3_obs3 >= safe_distance**2)
def plotGraph():
# plt.rcParams['font.sans-serif'] = ['Times New Roman'] # 如果要显示中文字体，则在此处设为：SimHei
# plt.rcParams['axes.unicode_minus'] = False # 显示负号
# # show multilane
# multiLane.curvePath()
# # 计算多车道环境的弧长参数曲线的系数
# efficients = multiLane.saveEfficients()
# 计算回环环境的弧长参数曲线的系数
efficients = cubicSpline.saveEfficients()
# show sampling path
generate_lattice(efficients)
if show_obstacle:
c = ['r', 'gold', 'darkorange']
for i in range(len(static_obs)):
tmpx, tmpy, tmptheta = ftc.frenetToXY(static_obs[-(i + 1)][0],
static_obs[-(i + 1)][1],
obstacleHeading, efficients)
car.simVehicle([tmpx, tmpy], tmptheta, c[i], 0.8)
if showVehicleStart:
tmpx, tmpy, tmptheta = ftc.frenetToXY(start_SRho[0], start_SRho[1],
obstacleHeading, efficients)
car.simVehicle([tmpx, tmpy], tmptheta, 'b', 0.8)
font1 = {
'family': 'Times New Roman',
'weight': 'normal',
'size': 10,
}
plt.xlabel("x (m)", font1)
plt.ylabel('y (m)', font1)
# 设置坐标刻度值的大小以及刻度值的字体
plt.tick_params(labelsize=10)
# x = np.array([0.0, 10.0, 20.0, 30.0, 40.0, 50.0])
# x = np.array([0.0, 20.0, 40.0, 60.0, 80.0, 100.0])
# y = np.array([-2.0, -1, 0.0, 1.0, 2.0])
# y = np.array([-2.0, -1, 0.0, 1.0, 2.0])
# xgroup_labels = ['0.0', '20.0', '40.0', '60.0', '80.0', '100.0'] # x轴刻度的标识
# ygroup_labels = ['-2.0', '-1.0', '0.0', '1.0', '2.0'] # y轴刻度的标识
# plt.xticks(x, xgroup_labels, fontproperties='Times New Roman', fontsize=10) # 默认字体大小为10
# plt.yticks(y, ygroup_labels, fontproperties='Times New Roman', fontsize=10)
plt.xticks(fontproperties='Times New Roman', fontsize=10)
plt.yticks(fontproperties='Times New Roman', fontsize=10)
plt.xlim(30, 140)
plt.ylim(80, 130)
plt.savefig('../SimGraph/pathLatticeXY3_053002.tiff', dpi=600)
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 ToPathPlanner2(efficients): # show sampling path generate_lattice(efficients) if showVehicleStart: tmpx, tmpy, tmptheta = ftc.frenetToXY(start_SRho[0], start_SRho[1], obstacleHeading, efficients) car.simVehicle([tmpx, tmpy], tmptheta, 'b', 1)
def path_kappa(rho, s, efficients):
kappaVec = []
for i in range(len(rho) - 2):
x0, y0, theta0 = ftc.frenetToXY(s[i], rho[i], 0, efficients)
x1, y1, theta1 = ftc.frenetToXY(s[i + 1], rho[i + 1], 0, efficients)
x2, y2, theta2 = ftc.frenetToXY(s[i + 2], rho[i + 2], 0, 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):
kappaVec.append(0)
else:
kappaVec.append((k1 - k2) / k3)
return kappaVec
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 plotGraph():
efficients = cubicSpline.saveEfficients()
# show sampling path
generate_lattice(efficients)
if show_obstacle:
for i in range(len(obstacle)):
tmpx, tmpy, tmptheta = ftc.frenetToXY(obstacle[i][0], obstacle[i][1], obstacleHeading, efficients)
car.simVehicle([tmpx, tmpy], tmptheta, 'r', 1)
if showVehicleStart:
tmpx, tmpy, tmptheta = ftc.frenetToXY(start_SRho[0], start_SRho[1], obstacleHeading, efficients)
car.simVehicle([tmpx, tmpy], tmptheta, 'b', 1)
font1 = {'family': 'Times New Roman',
'weight': 'normal',
'size': 10,
}
plt.xlabel("x (m)", font1)
plt.ylabel('y (m)', font1)
# 设置坐标刻度值的大小以及刻度值的字体
plt.tick_params(labelsize=10)
# x = np.array([0.0, 10.0, 20.0, 30.0, 40.0, 50.0])
# x = np.array([0.0, 20.0, 40.0, 60.0, 80.0, 100.0])
# y = np.array([-2.0, -1, 0.0, 1.0, 2.0])
# y = np.array([-2.0, -1, 0.0, 1.0, 2.0])
# xgroup_labels = ['0.0', '20.0', '40.0', '60.0', '80.0', '100.0'] # x轴刻度的标识
# ygroup_labels = ['-2.0', '-1.0', '0.0', '1.0', '2.0'] # y轴刻度的标识
# plt.xticks(x, xgroup_labels, fontproperties='Times New Roman', fontsize=10) # 默认字体大小为10
# plt.yticks(y, ygroup_labels, fontproperties='Times New Roman', fontsize=10)
plt.xticks(fontproperties='Times New Roman', fontsize=10)
plt.yticks(fontproperties='Times New Roman', fontsize=10)
plt.xlim(-10, 30)
plt.ylim(-5, 80)
def ToMPC(): efficients = cubicSpline.saveEfficients() rho_vector = ipoptSolver(efficients) theta_rho = [] for i in range(len(s) - 1): state0 = [s[i], rho_vector[i]] state1 = [s[i + 1], rho_vector[i + 1]] tmp_theta = heading.calHeadingFrenet(state0, state1) theta_rho.append(tmp_theta) theta_rho.append(theta_rho[-1]) x = [] y = [] theta = [] # print(s) for j in range(len(s)): tmpX, tmpY, tmpTheta = ftc.frenetToXY(s[j], rho_vector[j], theta_rho[j], efficients) x.append(tmpX) y.append(tmpY) theta.append(tmpTheta) kappa_list = calKappa.path_kappa(rho_vector, s, efficients) return x[0:n_s + 1], y[0:n_s + 1], theta[0:n_s + 1], kappa_list
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()
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():
# plot graph
plt.figure(figsize=(3.5, 3.5 * 0.62)) # 单位英寸， 3.5
font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10}
plt.rcParams['font.sans-serif'] = ['Times New Roman'
] # 如果要显示中文字体，则在此处设为：SimHei
p1 = [0.2, 0.2, 0.7, 0.7]
plt.axes(p1)
efficients = cubicSpline.saveEfficients()
rho_vector = ipoptSolver(efficients)
theta_rho = []
for i in range(len(s) - 1):
state0 = [s[i], rho_vector[i]]
state1 = [s[i + 1], rho_vector[i + 1]]
tmp_theta = heading.calHeadingFrenet(state0, state1)
theta_rho.append(tmp_theta)
theta_rho.append(theta_rho[-1])
x = []
y = []
theta = []
# print(s)
for j in range(len(s)):
tmpX, tmpY, tmpTheta = ftc.frenetToXY(s[j], rho_vector[j],
theta_rho[j], efficients)
x.append(tmpX)
y.append(tmpY)
theta.append(tmpTheta)
tmp_s = []
tmp_rho = []
tmp_thetaRho = []
if showRoughPath:
tmp_s, tmp_rho, tmp_thetaRho = pathPlanner.ToPathOptimizer3(efficients)
if showRefinedPath:
plt.plot(x, y, c='cyan', linewidth=0.6, alpha=1, label='Refined path')
if showVehicleStart:
tmpx, tmpy, tmptheta = ftc.frenetToXY(start_SRho[0], start_SRho[1],
obstacleHeading, efficients)
car.simVehicle([tmpx, tmpy], tmptheta, 'b', 0.8)
# print(len(x), len(y), len(theta))
# print(x[0], y[0], theta[0])
# plot obstacles
# if show_obstacle:
# c=['r', 'gold', 'darkorange']
# for i in range(len(dyn_obs)):
# obs_s = [j for j in np.arange(dyn_obs[-(i+1)][0], dyn_obs[-(i+1)][0]+ 50, 1)]
# obs_rho = [dyn_obs[-(i+1)][1] for q in np.arange(dyn_obs[-(i+1)][0], dyn_obs[-(i+1)][0]+ 50, 1)]
# for m in range(0, len(obs_s), 1):
# time_stamp = m / len(obs_s) + 0.08
# if time_stamp > 1.0:
# time_stamp = 1.0
# tmpx, tmpy, tmptheta = ftc.frenetToXY(obs_s[m], obs_rho[m], obstacleHeading, efficients)
# car.simVehicle([tmpx, tmpy], tmptheta, c[i], time_stamp)
if show_obstacle:
c = ['r', 'gold', 'darkorange']
for i in range(len(static_obs)):
tmpx, tmpy, tmptheta = ftc.frenetToXY(static_obs[-(i + 1)][0],
static_obs[-(i + 1)][1],
obstacleHeading, efficients)
car.simVehicle([tmpx, tmpy], tmptheta, c[i], 0.8)
if show_vehicle_animation:
for i in range(0, n_s, 2):
time_stamp = i / n_s + 0.1
if time_stamp > 1.0:
time_stamp = 1.0
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
# plt.pause(0.001)
plt.grid(linestyle="--", linewidth=0.5, alpha=1)
# plt.title('x-y Graph', font1)
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.legend(loc=0) # 图例位置自动
plt.xlim(30, 140)
plt.ylim(80, 130)
# plt.axis('equal')
plt.savefig('../SimGraph/pathOptimization3Path060404.svg')
plt.savefig('../SimGraph/pathOptimization3Path060404.tiff', dpi=600)
plt.figure(figsize=(3.5, 1.8)) # 单位英寸， 3.5
font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10}
plt.rcParams['font.sans-serif'] = ['Times New Roman'
] # 如果要显示中文字体，则在此处设为：SimHei
p2 = [0.2, 0.25, 0.7, 0.6]
plt.axes(p2)
if showRoughKappa:
rough_kappa = calKappa.path_kappa(tmp_rho, tmp_s, efficients)
print("--------------")
print(len(rough_kappa))
plt.plot(tmp_s[0:-2],
rough_kappa,
c='magenta',
linestyle="-",
linewidth=0.5,
alpha=1,
label='Rough Path Curvature Profile')
if showRefinedKappa:
kappa_list = calKappa.path_kappa(rho_vector, s, efficients)
plt.plot(s[0:n_s + 1],
kappa_list,
c='cyan',
linestyle="-",
linewidth=0.5,
alpha=1,
label='Refined Path Curvature Profile')
y = [max(kappa_list) for i in range(n_s + 1)]
y2 = [min(kappa_list) for i in range(n_s + 1)]
plt.plot(s[0:n_s + 1],
y,
c='k',
linestyle="--",
linewidth=0.5,
alpha=1)
plt.plot(s[0:n_s + 1],
y2,
c='k',
linestyle="--",
linewidth=0.5,
alpha=1)
plt.title('Curvature Profile', font1)
plt.grid(linestyle="--", linewidth=0.5, alpha=1)
plt.xlabel('s (m)', font1)
plt.ylabel('kappa (1/m)', font1)
plt.xlim(90, 210)
plt.ylim(-0.1, 0.3)
plt.legend(loc=0) # 图例位置自动
plt.savefig('../SimGraph/pathOptimization3Kappa060404.svg')
plt.savefig('../SimGraph/pathOptimization3Kappa060404.tiff', dpi=600)
plt.show()
def plotGraph():
# plot graph
plt.figure(figsize=(3.5, 3.0)) # 单位英寸， 3.5
font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10}
plt.rcParams['font.sans-serif'] = ['Times New Roman'] # 如果要显示中文字体，则在此处设为：SimHei
p1 = [0.15, 0.15, 0.80, 0.8]
plt.axes(p1)
efficients = cubicSpline.saveEfficients()
rho_vector = ipoptSolver(efficients)
theta_rho = []
for i in range(len(s) - 1):
state0 = [s[i], rho_vector[i]]
state1 = [s[i + 1], rho_vector[i + 1]]
tmp_theta = heading.calHeadingFrenet(state0, state1)
theta_rho.append(tmp_theta)
theta_rho.append(theta_rho[-1])
x = []
y = []
theta = []
# print(s)
for j in range(len(s)):
tmpX, tmpY, tmpTheta = ftc.frenetToXY(s[j], rho_vector[j], theta_rho[j], efficients)
x.append(tmpX)
y.append(tmpY)
theta.append(tmpTheta)
tmp_s = []
tmp_rho = []
tmp_thetaRho = []
if showRoughPath:
tmp_s, tmp_rho, tmp_thetaRho = pathPlanner.ToPathOptimizer2(efficients)
if showRefinedPath:
plt.plot(x, y, c='cyan', linewidth=0.6, alpha=1, label='Refined path')
# plot obstacles
if show_obstacle:
for i in range(len(static_obs)):
tmpx, tmpy, tmptheta = ftc.frenetToXY(static_obs[i][0], static_obs[i][1], obstacleHeading, efficients)
car.simVehicle([tmpx, tmpy], tmptheta, 'r', 1)
if show_vehicle_start:
x0, y0, theta0 = ftc.frenetToXY(start_SRho[0], start_SRho[1], start_SRho[2], efficients)
car.simVehicle([x0, y0], theta0, 'blue', 1)
if show_vehicle_animation:
for i in range(0, n_s, 1):
time_stamp = i / n_s + 0.3
if time_stamp > 1.0:
time_stamp = 1.0
if i <= 5:
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
if 5 <= i <= 10:
if (i % 2) == 0:
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
if 10 <= i <= n_s:
if (i % 3) == 0:
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
# plt.pause(0.001)
plt.grid(linestyle="--", linewidth=0.5, alpha=1)
# plt.title('x-y Graph', font1)
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.legend(loc=0) # 图例位置自动
plt.xlim(-10, 30)
plt.ylim(-5, 80)
plt.savefig('../SimGraph/pathOptimization2Path060406.svg')
plt.savefig('../SimGraph/pathOptimization2Path060406.tiff', dpi=600)
plt.figure(figsize=(3.5, 1.8)) # 单位英寸， 3.5
font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10}
plt.rcParams['font.sans-serif'] = ['Times New Roman'] # 如果要显示中文字体，则在此处设为：SimHei
p2 = [0.2, 0.25, 0.75, 0.60]
plt.axes(p2)
if showRoughKappa:
rough_kappa = calKappa.path_kappa(tmp_rho, tmp_s, efficients)
print("--------------")
print(len(rough_kappa))
print(len(tmp_s))
plt.plot(tmp_s[0:-2], rough_kappa, c='magenta', linestyle="-", linewidth=0.5, alpha=1,label='Rough Path Curvature Profile')
if showRefinedKappa:
kappa_refined = calKappa.path_kappa(rho_vector, s, efficients)
plt.plot(s[0:n_s + 1], kappa_refined, c='cyan', linestyle="-", linewidth=0.5, alpha=1,
label='Refined Path Curvature Profile')
y = [max(kappa_refined) for i in range(n_s + 1)]
y2 = [min(kappa_refined) for i in range(n_s + 1)]
# plt.plot(s[0:n_s + 1], y, c='k', linestyle="--", linewidth=0.5, alpha=1)
# plt.plot(s[0:n_s + 1], y2, c='k', linestyle="--", linewidth=0.5, alpha=1)
plt.title('Curvature Profile', font1)
plt.grid(linestyle="--", linewidth=0.5, alpha=1)
plt.xlabel('s (m)', font1)
plt.ylabel('kappa (1/m)', font1)
plt.xlim(-1, 110)
plt.ylim(-0.1, 0.2)
plt.legend(loc=0) # 图例位置自动
plt.savefig('../SimGraph/pathOptimization2Kappa060406.svg')
plt.savefig('../SimGraph/pathOptimization2Kappa060406.tiff', dpi=600)
plt.show()
return None
def plotGraph():
# plot graph
plt.figure(figsize=(3.5, 3.5 * 0.62)) # 单位英寸， 3.5
font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10}
plt.rcParams['font.sans-serif'] = ['Times New Roman'
] # 如果要显示中文字体，则在此处设为：SimHei
p1 = [0.2, 0.2, 0.7, 0.7]
plt.axes(p1)
efficients = cubicSpline.saveEfficients()
rho_vector = ipoptSolver(efficients)
theta_rho = []
for i in range(len(s) - 1):
state0 = [s[i], rho_vector[i]]
state1 = [s[i + 1], rho_vector[i + 1]]
tmp_theta = heading.calHeadingFrenet(state0, state1)
theta_rho.append(tmp_theta)
theta_rho.append(theta_rho[-1])
x = []
y = []
theta = []
# print(s)
for j in range(len(s)):
tmpX, tmpY, tmpTheta = ftc.frenetToXY(s[j], rho_vector[j],
theta_rho[j], efficients)
x.append(tmpX)
y.append(tmpY)
theta.append(tmpTheta)
tmp_s = []
tmp_rho = []
tmp_thetaRho = []
if showRoughPath:
tmp_s, tmp_rho, tmp_thetaRho = pathPlanner.ToPathOptimizer5(efficients)
if showRefinedPath:
plt.plot(x[0:-3],
y[0:-3],
c='cyan',
linewidth=0.6,
alpha=1,
label='Refined path')
if showVehicleStart:
tmpx, tmpy, tmptheta = ftc.frenetToXY(start_SRho[0], start_SRho[1],
obstacleHeading, efficients)
car.simVehicle([tmpx, tmpy], tmptheta, 'b', 1)
# print(len(x), len(y), len(theta))
# print(x[0], y[0], theta[0])
# plot obstacles
if showObsDyn:
c = ['r', 'gold', 'lightseagreen']
for i in range(len(static_obs)):
obs_s = [
j for j in np.arange(static_obs[-(i + 1)][0],
static_obs[-(i + 1)][0] + 100, 1)
]
obs_rho = [
static_obs[-(i + 1)][1] for q in np.arange(
static_obs[-(i + 1)][0], static_obs[-(i + 1)][0] + 100, 1)
]
for m in range(0, len(obs_s), 2):
time_stamp = m / len(obs_s) + 0.05
if time_stamp > 1.0:
time_stamp = 1.0
tmpx, tmpy, tmptheta = ftc.frenetToXY(obs_s[m], obs_rho[m],
obstacleHeading,
efficients)
car.simVehicle([tmpx, tmpy], tmptheta, c[i], time_stamp)
if showStaticObstacle:
for i in range(len(static_obs)):
c = ['lightseagreen', 'orange', 'green']
tmpx, tmpy, tmptheta = ftc.frenetToXY(static_obs[i][0],
static_obs[i][1],
obstacleHeading, efficients)
car.simVehicle([tmpx, tmpy], tmptheta, c[i], 1)
if showVehicleAnimation:
for i in range(0, 6, 1):
time_stamp = i / n_s + 0.1
if time_stamp > 1.0:
time_stamp = 1.0
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
for i in range(6, 30, 2):
time_stamp = i / n_s + 0.1
if time_stamp > 1.0:
time_stamp = 1.0
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
for i in range(30, 60, 4):
time_stamp = i / n_s + 0.1
if time_stamp > 1.0:
time_stamp = 1.0
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
# plt.pause(0.001)
for i in range(60, 80, 5):
time_stamp = i / n_s + 0.1
if time_stamp > 1.0:
time_stamp = 1.0
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
for i in range(80, 120, 4):
time_stamp = i / n_s + 0.1
if time_stamp > 1.0:
time_stamp = 1.0
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
for i in range(120, 160, 3):
time_stamp = i / n_s + 0.1
if time_stamp > 1.0:
time_stamp = 1.0
car.simVehicle([x[i], y[i]], theta[i], 'b', time_stamp)
plt.grid(linestyle="--", linewidth=0.5, alpha=1)
# plt.title('x-y Graph', font1)
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.legend(loc=0) # 图例位置自动
plt.xlim(300, 500)
plt.ylim(60, 190)
# plt.axis('equal')
plt.savefig('../SimGraph/pathOptimizer_5Path060406.svg')
plt.savefig('../SimGraph/pathOptimizer_5Path060406.tiff', dpi=600)
plt.figure(figsize=(3.5, 1.8)) # 单位英寸， 3.5
font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10}
plt.rcParams['font.sans-serif'] = ['Times New Roman'
] # 如果要显示中文字体，则在此处设为：SimHei
p2 = [0.2, 0.25, 0.7, 0.6]
plt.axes(p2)
if showRoughKappa:
print("==================")
print("len:tmp_rho,tmp_s", len(tmp_rho), len(tmp_s))
rough_kappa = calKappa.path_kappa(tmp_rho, tmp_s, efficients)
print("--------------")
print(len(rough_kappa))
plt.plot(tmp_s[0:-2],
rough_kappa,
c='magenta',
linestyle="-",
linewidth=0.5,
alpha=1,
label='Rough Path Curvature Profile')
if showRefinedKappa:
kappa_refined = calKappa.path_kappa(rho_vector, s, efficients)
plt.plot(s[0:-2],
kappa_refined,
c='cyan',
linestyle="-",
linewidth=0.5,
alpha=1,
label='Refined Path Curvature Profile')
# y = [kappa_max for i in range(n_s + 1)]
# y2 = [-kappa_max for i in range(n_s + 1)]
# plt.plot(s[0:n_s + 1], y, c= 'k', linestyle="--", linewidth=0.5, alpha=1)
# plt.plot(s[0:n_s + 1], y2, c= 'k', linestyle="--", linewidth=0.5, alpha=1)
plt.title('Curvature Profile', font1)
plt.grid(linestyle="--", linewidth=0.5, alpha=1)
plt.xlabel('s (m)', font1)
plt.ylabel('kappa (1/m)', font1)
plt.xlim(400, 580)
plt.ylim(-0.1, 0.3)
plt.legend(loc=0) # 图例位置自动
plt.savefig('../SimGraph/pathOptimization5Kappa060406.svg')
plt.savefig('../SimGraph/pathOptimization5Kappa060406.tiff', dpi=600)
plt.show()