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 = pycubicspline.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 = pycubicspline.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 main():
print("rear wheel feedback tracking start!!")
ax = [0.0, 6.0, 12.5, 5.0, 7.5, 3.0, -1.0]
ay = [0.0, 0.0, 5.0, 6.5, 3.0, 5.0, -2.0]
goal = [ax[-1], ay[-1]]
cx, cy, cyaw, ck, s = pycubicspline.calc_spline_course(ax, ay, ds=0.1)
target_speed = 10.0 / 3.6
sp = calc_speed_profile(cx, cy, cyaw, target_speed)
t, x, y, yaw, v, goal_flag = closed_loop_prediction(
cx, cy, cyaw, ck, sp, goal)
# Test
assert goal_flag, "Cannot goal"
if show_animation:
plt.close()
flg, _ = plt.subplots(1)
plt.plot(ax, ay, "xb", label="input")
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()
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()
def main():
print("LQR steering control tracking start!!")
ax = [0.0, 6.0, 12.5, 10.0, 7.5, 3.0, -1.0]
ay = [0.0, -3.0, -5.0, 6.5, 3.0, 5.0, -2.0]
goal = [ax[-1], ay[-1]]
cx, cy, cyaw, ck, s = pycubicspline.calc_spline_course(ax, ay, ds=0.1)
target_speed = 10.0 / 3.6
sp = calc_speed_profile(cx, cy, cyaw, target_speed)
t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, ck, sp, goal)
if animation:
matplotrecorder.save_movie("animation.gif", 0.1) # gif is ok.
flg, _ = plt.subplots(1)
plt.plot(ax, ay, "xb", label="input")
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()
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()
def main():
print("LQR steering control tracking start!!")
ax = [0.0, 6.0, 12.5, 10.0, 17.5, 20.0, 25.0]
ay = [0.0, -3.0, -5.0, 6.5, 3.0, 0.0, 0.0]
goal = [ax[-1], ay[-1]]
cx, cy, cyaw, ck, s = pycubicspline.calc_spline_course(ax, ay, ds=0.1)
target_speed = 10.0 / 3.6 # simulation parameter km/h -> m/s
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()
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 = pycubicspline.calc_spline_course(ax, ay, ds=dl)
return cx, cy, cyaw, ck
def main():
# target course
ax = [0.0, 100.0, 100.0, 50.0, 60.0]
ay = [0.0, 0.0, -30.0, -20.0, 0.0]
cx, cy, cyaw, ck, s = pycubicspline.calc_spline_course(ax, ay, ds=0.1)
target_speed = 30.0 / 3.6 # [m/s]
T = 100.0 # max simulation time
# initial state
state = State(x=-0.0, y=5.0, yaw=math.radians(20.0), v=0.0)
lastIndex = len(cx) - 1
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind, mind = calc_target_index(state, cx, cy)
while T >= time and lastIndex > target_ind:
ai = PIDControl(target_speed, state.v)
di, target_ind = stanley_control(state, cx, cy, cyaw, target_ind)
state = update(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)
if show_animation:
plt.cla()
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
plt.pause(0.001)
# Test
assert lastIndex >= target_ind, "Cannot goal"
if show_animation:
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.legend()
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.axis("equal")
plt.grid(True)
flg, ax = plt.subplots(1)
plt.plot(t, [iv * 3.6 for iv in v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.grid(True)
plt.show()
