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()