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 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 closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
T = 500.0 # max simulation time
goal_dis = 0.3
stop_speed = 0.05
state = State(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0)
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = calc_nearest_index(state, cx, cy, cyaw)
e, e_th = 0.0, 0.0
while T >= time:
dl, target_ind, e, e_th, ai = lqr_steering_control(
state, cx, cy, cyaw, ck, e, e_th, speed_profile)
state = update(state, ai, dl)
if abs(state.v) <= stop_speed:
target_ind += 1
time = time + dt
# check goal
dx = state.x - goal[0]
dy = state.y - goal[1]
if math.sqrt(dx**2 + dy**2) <= goal_dis:
print("Goal")
break
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
if target_ind % 1 == 0 and show_animation:
plt.cla()
plt.plot(cx, cy, "-r", label="course")
plt.plot(x, y, c="b", ls='-', label="trajectory")
simModel.simVehicle(center_point=[x[-1], y[-1]],
heading=yaw[-1],
color='b',
timestamp=1)
# plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
plt.axis("equal")
plt.grid(True)
# plt.xlim(-10, 100)
# plt.ylim(-10, 100)
plt.title("speed[km/h]:" + str(round(state.v * 3.6, 2)) +
",target index:" + str(target_ind))
plt.pause(0.0001)
return t, x, y, yaw, v
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 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 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 plotGraph():
# plot graph
plt.figure(figsize=(3.5, 3.5 * 0.618)) # 单位英寸， 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.6]
plt.axes(p1)
rho_vector = ipoptSolver()
# plot trajectory
plt.plot(s, rho_vector, c='b', linestyle="-", linewidth=0.5, alpha=1)
# plot obstacles
car.simVehicle([static_obs[0][0], static_obs[0][1]], 0 * math.pi / 180, 'r', 1)
car.simVehicle([static_obs[1][0], static_obs[1][1]], 0 * math.pi / 180, 'r', 1)
car.simVehicle([static_obs[2][0], static_obs[2][1]], 0 * math.pi / 180, 'r', 1)
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.xlim(-1, 110)
# plt.ylim(-4, 4)
plt.savefig('../SimGraph/pathOptimizationPath060414.svg')
plt.figure(figsize=(3.5, 3.5 * 0.618)) # 单位英寸， 3.5
font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10}
plt.rcParams['font.sans-serif'] = ['Times New Roman'] # 如果要显示中文字体，则在此处设为：SimHei
p2 = [0.2, 0.2, 0.7, 0.6]
plt.axes(p2)
kappa_list = trajectory_kappa(rho_vector, s)
plt.plot(s[0:n_s + 1], kappa_list, c= 'b', linestyle="-", linewidth=0.5, alpha=1)
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= 'r', linestyle="--", linewidth=0.5, alpha=1)
plt.plot(s[0:n_s + 1], y2, c= 'r', 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.2, 0.2)
plt.savefig('../SimGraph/pathOptimizationKappa060413.svg')
plt.show()
def do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state):
"""
Simulation
cx: course x position list
cy: course y position list
cy: course yaw position list
ck: course curvature list
sp: speed profile
dl: course tick [m]
"""
goal = [cx[-1], cy[-1]]
state = initial_state
# initial yaw compensation
if state.yaw - cyaw[0] >= math.pi:
state.yaw -= math.pi * 2.0
elif state.yaw - cyaw[0] <= -math.pi:
state.yaw += math.pi * 2.0
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
d = [0.0]
a = [0.0]
target_ind, _ = calc_nearest_index(state, cx, cy, cyaw, 0)
odelta, oa = None, None
cyaw = smooth_yaw(cyaw)
while MAX_TIME >= time:
xref, target_ind, dref = calc_ref_trajectory(state, cx, cy, cyaw, ck,
sp, dl, target_ind)
x0 = [state.x, state.y, state.v, state.yaw] # current state
oa, odelta, ox, oy, oyaw, ov = iterative_linear_mpc_control(
xref, x0, dref, oa, odelta)
if odelta is not None:
di, ai = odelta[0], oa[0]
state = update_state(state, ai, di)
time = time + DT
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
d.append(di)
a.append(ai)
if check_goal(state, goal, target_ind, len(cx)):
print("Goal")
break
if showVehicle: # pragma: no cover
plt.cla()
time_stamp = time / 100 + 0.3
plt.plot(cx, cy, "-r", label="course")
# plt.plot(x, y, c="b", label="trajectory", ls = '-')
# plt.xlim(-10, 80)
plt.axis("equal")
plt.grid(True)
# plt.ylim(-10, 80)
simModel.simVehicle(center_point=[x[-1], y[-1]],
heading=yaw[-1],
color='b',
timestamp=1)
# plt.plot(xref[0, :], xref[1, :], "xk", label="xref")
# plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
# plot_car(state.x, state.y, state.yaw, steer=di)
# plt.xlim(-10, 100)
# plt.ylim(-10, 100)
plt.title("Time[s]:" + str(round(time, 2)) + ", speed[km/h]:" +
str(round(state.v * 3.6, 2)))
plt.pause(0.0001)
return t, x, y, yaw, v, d, a
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 dynamic_vehicle():
for station in range(100):
time_stamp = 0.0 + station / 100.0
vehicle.simVehicle([station, 3.75 / 2.0], 0, 'b', time_stamp)
plt.pause(0.01)
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()
x3 = a + (r + 3.75) * np.cos(theta)
y3 = b + (r + 3.75) * np.sin(theta)
# =====================================
# 画出移动障碍物图
heading = np.arange(-np.pi, -np.pi * 3 / 2, -0.01)
center_x = a + (r + 3.75 / 2) * np.cos(heading)
center_y = b + (r + 3.75 / 2) * np.sin(heading)
real_heading = heading + math.pi * 3 / 2
fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)
ax.plot(x, y, color='black', linewidth=2, linestyle='--')
# axes.plot(x2_center, y2_center, color='r', linewidth='0.5', linestyle='--')
ax.plot(x2, y2, color='black', linewidth=1.5, linestyle='-')
# axes.plot(x3_center, y3_center, color='r', linewidth='0.5', linestyle='--')
ax.plot(x3, y3, color='black', linewidth=2, linestyle='-')
for i in range(len(real_heading)):
time_stamp = 0.0 + i / len(real_heading)
vehicle.simVehicle([center_x[i], center_y[i]], real_heading[i], 'b',
time_stamp)
plt.pause(0.001)
if __name__ == '__main__':
plt.axis("equal")
plt.grid(True)
plt.show()
def plotGraph():
# plot graph
plt.figure(figsize=(3.5, 1.2)) # 单位英寸， 3.5
font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10}
plt.rcParams['font.sans-serif'] = ['Times New Roman'
] # 如果要显示中文字体，则在此处设为：SimHei
p1 = [0.15, 0.4, 0.8, 0.4]
plt.axes(p1)
# 调用pathplanner的环境
pp.plotGraph()
rho_vector = ipoptSolver()
# plot trajectory
plt.plot(s,
rho_vector,
c='cyan',
linestyle="-",
linewidth=1,
alpha=1,
label='Refined path')
car.simVehicle([x_init[2], x_init[3]], 0 * math.pi / 180, 'b', 1)
# plot obstacles
car.simVehicle([static_obs[0][0], static_obs[0][1]], 0 * math.pi / 180,
'r', 1)
car.simVehicle([static_obs[1][0], static_obs[1][1]], 0 * math.pi / 180,
'r', 1)
car.simVehicle([static_obs[2][0], static_obs[2][1]], 0 * math.pi / 180,
'r', 1)
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.xlim(-1, 102)
plt.ylim(-4, 4)
# plt.legend(loc=0) # 图例位置自动
plt.savefig('../SimGraph/pathOptimizationTest_060404.svg')
plt.savefig('../SimGraph/pathOptimizationTest_060404.tiff', dpi=600)
plt.figure(figsize=(3.5, 1.2)) # 单位英寸， 3.5
font1 = {'family': 'Times New Roman', 'weight': 'normal', 'size': 10}
plt.rcParams['font.sans-serif'] = ['Times New Roman'
] # 如果要显示中文字体，则在此处设为：SimHei
p2 = [0.2, 0.35, 0.75, 0.6]
plt.axes(p2)
kappa_list = trajectory_kappa(rho_vector, s)
plt.plot(s[0:n_s + 1],
kappa_list,
c='cyan',
linestyle="-",
linewidth=0.8,
alpha=1,
label='Refined Path')
# plot rough path kappa
pp.plotkappaGraph()
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= 'r', linestyle="--", linewidth=0.5, alpha=1, label='Maximum Curvature Limit')
# plt.plot(s[0:n_s + 1], y2, c= 'orange', linestyle="--", linewidth=0.5, alpha=1, label='Minimum Curvature Limit')
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, 102)
plt.ylim(-0.12, 0.12)
# plt.legend(loc=0) # 图例位置自动
plt.savefig('../SimGraph/pathOptimizationTestKappa_060404.svg')
plt.savefig('../SimGraph/pathOptimizationTestKappa_060404.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()
if __name__ == '__main__':
print(__file__ + " start!!")
plt.figure(figsize=(3.5, 3.5 * 0.9)) # 单位英寸， 3.5
plt.axes([0.15, 0.15, 0.75, 0.75])
plt.axis("equal")
plt.grid(linestyle="--", linewidth=0.5, alpha=1)
efficients = mlane.saveEfficients()
if show_lane:
# show multilane
mlane.curvePath()
if show_obstacle:
for i in range(len(obstacle)):
x, y, theta = mlane.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 = []
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():
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():
# 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()
