def get_forward_course(dl):
ax = [0.0, 60.0, 125.0, 50.0, 75.0, 30.0, -10.0]
ay = [0.0, 0.0, 50.0, 65.0, 30.0, 50.0, -20.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=dl)
return cx, cy, cyaw, ck
def get_straight_course2(dl):
ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0]
ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=dl)
return cx, cy, cyaw, ck
def get_straight_course(dl):
ax = [0.0, 5.0, 10.0, 20.0, 30.0, 40.0, 50.0]
ay = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=dl)
return cx, cy, cyaw, ck
def get_switch_back_course(dl):
ax = [0.0, 30.0, 6.0, 20.0, 35.0]
ay = [0.0, 0.0, 20.0, 35.0, 20.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=dl)
ax = [35.0, 10.0, 0.0, 0.0]
ay = [20.0, 30.0, 5.0, 0.0]
cx2, cy2, cyaw2, ck2, s2 = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=dl)
cyaw2 = [i - math.pi for i in cyaw2]
cx.extend(cx2)
cy.extend(cy2)
cyaw.extend(cyaw2)
ck.extend(ck2)
return cx, cy, cyaw, ck
def get_straight_course3(dl):
ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0]
ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0]
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
ay,
ds=dl)
cyaw = [i - math.pi for i in cyaw]
return cx, cy, cyaw, ck
def main():
print(__file__ + " start!!")
dl = 1.0 # course tick
# cx, cy, cyaw, ck = get_straight_course(dl)
# cx, cy, cyaw, ck = get_straight_course2(dl)
# cx, cy, cyaw, ck = get_straight_course3(dl)
# cx, cy, cyaw, ck = get_forward_course(dl)
# cx, cy, cyaw, ck = get_switch_back_course(dl)
x = cubic_spline_planner.x
y = cubic_spline_planner.y
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(x, y, ds=dl)
sp = calc_speed_profile(cx, cy, cyaw, TARGET_SPEED)
initial_state = State(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0)
t, x, y, yaw, v, d, a = do_simulation(cx, cy, cyaw, ck, sp, dl,
initial_state)
if showVehicleTrajectory: # pragma: no cover
plt.close("all")
plt.subplots()
plt.plot(cx, cy, "-r", label="spline")
plt.plot(x, y, "-g", label="tracking")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x (m)")
plt.ylabel("y (m)")
plt.legend()
plt.figure(figsize=(3.5, 3.5 * 0.4)) # 单位英寸， 3.5
plt.rcParams['font.sans-serif'] = ['Times New Roman'
] # 如果要显示中文字体，则在此处设为：SimHei
plt.axes([0.2, 0.35, 0.75, 0.45])
plt.grid(linestyle="--", linewidth=0.2, alpha=1)
# plt.xlim(-100, 500)
# plt.ylim(-100, 500)
font1 = {
'family': 'Times New Roman',
'weight': 'normal',
'size': 10,
}
plt.xlabel('Time (s)', font1)
plt.ylabel('Speed (Km/h)', font1)
plt.xticks(fontproperties='Times New Roman', fontsize=10)
plt.yticks(fontproperties='Times New Roman', fontsize=10)
# plt.legend()
plt.plot(t, v, "-r", label="speed", linewidth=0.5, alpha=0.8)
plt.title("Speed profile of the ego vehicle")
plt.savefig('../SimGraph/MPC_speed.tiff', dpi=600)
plt.show()
def main():
print("LQR steering control tracking start!!")
ax = cubic_spline.x
ay = cubic_spline.y
goal = [ax[-2], ay[-2]]
cx, cy, cyaw, ck, s = cubic_spline.calc_spline_course(ax, ay, ds=0.1)
sp = calc_speed_profile(cx, cy, cyaw, target_speed)
t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, ck, sp, goal)
if show_animation:
plt.close()
flg, _ = plt.subplots(1)
# plt.plot(ax, ay, "xb", label="waypoints")
plt.plot(cx, cy, "-r", label="target course")
plt.plot(x, y, "-g", label="tracking")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
flg, ax = plt.subplots(1)
plt.plot(s, [math.degrees(iyaw) for iyaw in cyaw], "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]")
flg, ax = plt.subplots(1)
plt.plot(s, ck, "-r", label="curvature")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("curvature [1/m]")
plt.show()