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_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_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_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
Exemple #5
0
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
Exemple #7
0
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
Exemple #8
0
def get_lane_change_scenario4(course, dl):

    ax = [-200, -210.0, -220.0, -230.0, -240.0, -250.0, -260, -270, -280]
    ay = [-16, -16, -16.5, -17, -18, -19, -20, -20, -20]
    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
Exemple #9
0
def path_plan(dl):
    # Insert path plan coordinates here:
    ax = [10.0, 20.0]
    ay = [10.0, 20.0]

    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
                                                                  ay,
                                                                  ds=dl)

    return cx, cy, cyaw, ck
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]
    #ax = [15.0, 11.0, 10.5, 6.0, 1.5, -10.0, -15.0]
    #ay = [15.0, 15.0, 12.0, 7.5, 3.0, 0.0, 0.0]
    x_crds = np.array([-8, 1, 10, 19, 28.0, 28, 28, 28, 28])
    y_crds = np.array([1.0, 1.0, 1.0, 1.0, 1, 10, 19, 28, 37])
    u_path = np.array([-1, 2, 2, 2, 2, 3, 3, 3, 3, 0])

    ax = list(x_crds)
    ay = list(y_crds)

    goal = [ax[-1], ay[-1]]

    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
                                                                  ay,
                                                                  ds=0.1)
    primi = lattice_based_planner.Primitives(4.5, 0.1)
    cx, cy, cyaw, ck = lattice_based_planner.calc_lattice_course(
        primi, x_crds, y_crds, u_path)

    target_speed = 10.8 / 3.6  # simulation parameter km/h -> m/s

    sp = calc_speed_profile(cyaw, target_speed)

    t, x, y, yaw, v = do_simulation(cx, cy, cyaw, ck, sp, goal)

    if show_animation:  # pragma: no cover
        plt.close()
        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()

        plt.subplots(1)
        plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
        plt.grid(True)
        plt.legend()
        plt.xlabel("line length[m]")
        plt.ylabel("yaw angle[deg]")

        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 pathCallback(msg):
    global cx, cy, cyaw, ck, s
    global global_path_x, global_path_y
    global x, y, yaw, v, t
    global state
    global last_idx

    global gpsvel
    global state
    # global header_time

    global history_x_mid, history_y_mid
    global updateIdx, updateRate

    ### Compare Header
    # 0821 Add if path is same as prev one, pass

    # start = time.time()

    if updateIdx > updateRate:
        global_path_x = msg.position
        global_path_y = msg.velocity
        updateIdx = 0

    # print("global_path_x: ", global_path_x)

    '''
    if header_time == msg.header.stamp:
        print("************************SAME PATH ")
        #return 0
    '''
    if len(global_path_x) < 30:

        print("ERROR Rogue Path data", len(global_path_x))
        return 0
    dist = np.sqrt(np.square(history_x_mid - global_path_x[4]) + np.square(history_y_mid -global_path_y[4] ))
    # print("************************* DIST", dist)

    history_x_mid = global_path_x[4]
    history_y_mid = global_path_y[4]
    # header_time = msg.header.stamp

    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
        global_path_x, global_path_y, ds=0.1)
    # Initial state
    state = State(x=global_path_x[0], y=global_path_y[0])

    last_idx = len(cx) - 1
    x = [state.x]
    y = [state.y]
    yaw = [state.yaw]
    v = [state.v]
    t = [0.0]
    # target_idx, _ = calc_target_index(state, cx, cy)
    updateIdx = 1 + updateIdx
def get_astar_course(dl):
    df = pd.read_csv('route.csv', delim_whitespace=True)
    ax = df.X
    ay = df.Y
    #ax = [0.0, 1.0, 2.0, 2.5, 3.0, 4.5, 6.0, 7.5, 9.0, 11.0, 12.0, 13.0, 15.0, 17.0]
    #ay = [0.0, 1.0, 2.0, 4.0, 6.0, 8.0, 8.5, 10.5, 12.0, 13.0, 14.0, 15.0, 15.5, 16.0]
    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
                                                                  ay,
                                                                  ds=dl)

    return cx, cy, cyaw, ck, s
Exemple #13
0
def get_path():
    waypoints_x = []
    waypoints_y = []
    x_path, y_path = Path.generate_path(True)
    waypoints_x = x_path
    waypoints_y = y_path
    final_goal = [waypoints_x[-1], waypoints_y[-1]]
    course_x, course_y, course_yaw, course_k, course_s = cubic_spline_planner.calc_spline_course(
        waypoints_x, waypoints_y, ds=0.1)
    target_velocity = 10.0 / 3.6
    return waypoints_x, waypoints_y, final_goal, course_x, course_y, course_yaw, course_k, course_s, target_velocity
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 = cubic_spline_planner.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, eigVals = closed_loop_prediction(cx, cy, cyaw, ck, sp,
                                                      goal)

    if show_animation:  # pragma: no cover
        plt.close()
        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()

        plt.subplots(1)
        plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
        plt.grid(True)
        plt.legend()
        plt.xlabel("line length[m]")
        plt.ylabel("yaw angle[deg]")

        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.subplots(1)
        plt.plot(t, eigVals[:, 0], "xb", label="lateral error eigval")
        plt.plot(t, eigVals[:, 1], "cs", label="lateral error rate eigval")
        plt.plot(t, eigVals[:, 2], "mo", label="heading error eigval")
        plt.plot(t, eigVals[:, 3], "r*", label="heading error rate eigval")
        plt.grid(True)
        plt.legend()
        plt.xlabel("time")
        plt.ylabel("eigvalues")

        plt.show()
Exemple #15
0
def get_straight_course2(dl, y_coords, x_coords):
    #import pandas as pd
    #df = pd.read_csv("route.csv")
    #ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0,-80]
    #ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0,-1.0]
    ax = y_coords
    ay = x_coords
    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
                                                                  ay,
                                                                  ds=dl)

    return cx, cy, cyaw, ck
Exemple #16
0
def main():
    print("LQR steering control tracking start!!")
    df = pd.read_csv('route.csv', delim_whitespace=True)
    ax = df.X
    ay = df.Y
    #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 = [df["X"].iloc[-1], df["Y"].iloc[-1]]

    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
                                                                  ay,
                                                                  ds=0.1)
    target_speed = 7.2 / 3.6  # simulation parameter km/h -> m/s

    sp = calc_speed_profile(cyaw, target_speed)

    t, x, y, yaw, v = do_simulation(cx, cy, cyaw, ck, sp, goal)

    if show_animation:  # pragma: no cover
        plt.close()
        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()

        plt.subplots(1)
        #plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
        plt.plot(t, np.rad2deg(yaw), "-r", label="yaw")
        plt.grid(True)
        plt.legend()
        plt.xlabel("Time [s]")
        plt.ylabel("Yaw angle[deg]")

        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.subplots()
        plt.plot(t, v, "-r", label="speed")
        plt.grid(True)
        plt.xlabel("Time [s]")
        plt.ylabel("Speed [kmh]")

        plt.show()
def main():
    print("LQR steering control tracking start!!")
    ax = [0.0, 6.0, 12.5, 10.0, 10.5, 3.0, -1.0]
    ay = [0.0, -3.0, -5.0, 6.5, 3.0, 10.0, -2.0]
    goal = [ax[-1], ay[-1]]

    cx, cy, cyaw, ck, s = cubic_spline_planner.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)
Exemple #18
0
def get_circular_course(dl):
    x = np.array([0])
    y = np.array([0])
    theta = np.array([0])
    for t in np.linspace(0, 2 * math.pi, 50):
        x = np.append(x, 5 * np.sin(t))
        y = np.append(y, 5 * np.sin(t + (math.pi / 2)))
        theta = np.append(theta, t)

    ax = x[1:]
    ay = y[1:]
    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
                                                                  ay,
                                                                  ds=dl)

    return cx, cy, cyaw, ck
Exemple #19
0
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]
    ax = [0.0, 100.0, 100.0, 50.0, 60.0]
    ay = [0.0, 0.0, -30.0, -20.0, 0.0]
    goal = [ax[-1], ay[-1]]

    cx, cy, cyaw, ck, s = cubic_spline_planner.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:  # pragma: no cover
        plt.close()
        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()

        plt.subplots(1)
        plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
        plt.grid(True)
        plt.legend()
        plt.xlabel("line length[m]")
        plt.ylabel("yaw angle[deg]")

        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()
Exemple #20
0
def cars_planners_setup(cord_cars_xy_cords):
    """

    """
    car_num = len(cord_cars_xy_cords)
    cars_plannars = {}

    for i_c in range(car_num):
        cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
            cord_cars_xy_cords[i_c][0], cord_cars_xy_cords[i_c][1], ds=0.1)

        target_speed = 3  # simulation parameter km/h -> m/s

        sp = lqr_speed_steer_control.calc_speed_profile(cyaw, target_speed)
        cars_plannars[i_c] = Planner(cx, cy, cyaw, ck, s, sp)

    return cars_plannars
Exemple #21
0
def main():
    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 = cubic_spline_planner.calc_spline_course(ax, ay, ds=0.1)
    
    veh_state =  VehicleState(x=0.5, y=0.0, yaw=0.0, v=0.0)

    e, th_e = 0.0, 0.0
    
    steering_lqr_controller = SteeringLQR()

    dl, target_ind, e, th_e = steering_lqr_controller.lqr_steering_control(veh_state, 
            cx, cy, cyaw, ck, e, th_e)

    print(np.rad2deg(dl))
def main():
    print("LQR steering control tracking start!!")
    ax = [7.5, 12.836819813353078, 58]
    #
    #
    ay = [2, 26.423725233465163, 38.0]
    goal = [ax[-1], ay[-1]]

    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
                                                                  ay,
                                                                  ds=0.1)
    target_speed = 20.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, ax[0],
                                             ay[0])

    if show_animation:  # pragma: no cover
        plt.close()
        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()

        plt.subplots(1)
        plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
        plt.grid(True)
        plt.legend()
        plt.xlabel("line length[m]")
        plt.ylabel("yaw angle[deg]")

        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()
Exemple #23
0
def main():
    print(__file__ + " start!!")

    # Chose course by global variable
    if LANE_CHANGE:
        ax, ay, course_name = getLaneChange()
    elif FIGURE_EIGHT:
        ax, ay, course_name = getFigureEight()
    elif ROAD:
        ax, ay, course_name = getRoadPath()

    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax, ay, ds=DL)

    sp = calc_speed_profile(cx, cy, cyaw, TARGET_SPEED)

    initial_state = State(x=cx[0], y=0.5, yaw=cyaw[0], v=TARGET_SPEED)

    t, x, y, yaw, v, d, a, station, e_ct_arr = do_simulation(
        cx, cy, cyaw, ck, sp, initial_state, course_name)

    if SHOW_PLOTS:  # pragma: no cover
        # plt.close("all")
        plt.figure(1)
        plt.plot(cx, cy, "-r", label="spline")
        # plt.scatter(cx, cy, "r", s=0.9, 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.subplots()
        # plt.plot(t, v, "-r", label="speed")
        # plt.grid(True)
        # plt.xlabel("Time [s]")
        # plt.ylabel("Speed [kmh]")

        # Plot relevant errors
        plt.figure(0)
        plt.plot(station, e_ct_arr, c=COLOR, label="Q={}".format(Q[0, 0]))
        plt.xlabel("Station[m]")
        plt.ylabel("Cross-Track Error[m]")
        plt.title("MPC at {}m/s on {}".format(TARGET_SPEED, course_name))
        plt.grid(True)
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 = cubic_spline_planner.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:  # pragma: no cover
        plt.close()
        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()

        plt.subplots(1)
        plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
        plt.grid(True)
        plt.legend()
        plt.xlabel("line length[m]")
        plt.ylabel("yaw angle[deg]")

        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()
Exemple #25
0
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]
    ax = [4.368, -0.2, -3.564, -4.310]
    ay = [0, 1.189, 5.362, 9.536]
    goal = [ax[-1], ay[-1]]

    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
                                                                  ay,
                                                                  ds=0.1)
    target_speed = 3  # 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:  # pragma: no cover
        plt.close()
        plt.subplots(1)
        #plt.plot(ax, ay, "xb", label="input")
        plt.plot(cx, cy, "-r", label="reference path")
        plt.plot(x, y, "-g", label="tracking")
        plt.grid(False)
        plt.axis("equal")
        plt.xlabel("x[m]")
        plt.ylabel("y[m]")
        #plt.legend()
        plt.savefig('path.png', transparent=True)

        plt.subplots(1)
        plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
        plt.grid(True)
        plt.legend()
        plt.xlabel("line length[m]")
        plt.ylabel("yaw angle[deg]")

        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()
Exemple #26
0
def generate_path(is_default=True):
    x_path = []
    y_path = []
    if is_default == True:
        path_obj = Path()
        x_raw, y_raw = path_obj.default_raw_path()
        x_refacto, y_refacto = path_obj.default_refactor()
        path_properties = path_obj.path_props(x_refacto, y_refacto)
        x_path, y_path = path_obj.remove_singular(x_refacto, y_refacto,
                                                  path_properties)
        waypoints_x = x_path
        waypoints_y = y_path
        final_goal = path_properties[1]
        course_x, course_y, course_yaw, course_k, course_s = cubic_spline_planner.calc_spline_course(
            waypoints_x, waypoints_y, ds=0.1)
    else:
        print "No Other than default"
    return x_path, y_path, course_x, course_y, course_yaw, course_k, course_s, final_goal
Exemple #27
0
def main():
    show_animation = True
    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]
    ax = [0.0, 100.0, 100.0, 50.0, 60.0]
    ay = [0.0, 0.0, -30.0, -20.0, 0.0]
    goal = [ax[-1], ay[-1]]

    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
        ax, ay, ds=1.0)
    target_speed = 30.0 / 3.6  # simulation parameter km/h -> m/s

    sp = calc_speed_profile(cx, cy, cyaw, target_speed)

    t, x, y, v, yaw = control(cx, cy, cyaw, ck, sp, 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, 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 = cubic_spline_planner.calc_spline_course(ax,
                                                                  ay,
                                                                  ds=0.1)
    target_speed = 10.0 / 3.6  # simulation parameter km/h -> m/s

    sp = calc_speed_profile(cyaw, target_speed)

    t, x, y, yaw, v = do_simulation(cx, cy, cyaw, ck, sp, goal)

    if show_animation:  # pragma: no cover
        plt.close()
        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()

        plt.subplots(1)
        plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
        plt.grid(True)
        plt.legend()
        plt.xlabel("line length[m]")
        plt.ylabel("yaw angle[deg]")

        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 = cubic_spline_planner.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:  # pragma: no cover
        plt.close()
        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()

        plt.subplots(1)
        plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")
        plt.grid(True)
        plt.legend()
        plt.xlabel("line length[m]")
        plt.ylabel("yaw angle[deg]")

        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()
Exemple #30
0
def closed_loop_prediction(ax, ay):
    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax, ay, ds=0.1)
    rospy.init_node("path_tracking")

    listener = tf.TransformListener()
    listener.waitForTransform('/map', '/base_link', rospy.Time(), rospy.Duration(20.0))

    pub = rospy.Publisher('drive_parameters', drive_param, queue_size=10)
    index = 0
    pe = 0
    pth_e = 0

    gazebo_pose = Pose_listener()

    while not rospy.is_shutdown():
        t = listener.getLatestCommonTime('/map', '/base_link')
        if listener.canTransform('/map', '/base_link', t):
            position, quaternion = listener.lookupTransform('/map', '/base_link', t)
            rpy = tf.transformations.euler_from_quaternion(quaternion)
            x = position[0]
            y = position[1]
            yaw = rpy[2]
            #x = gazebo_pose.pose.x
            #y = gazebo_pose.pose.y
            #yaw = gazebo_pose.pose.theta
            delta, e, th_e, index = lqr_steering_control(x, y, yaw, cx, cy, cyaw, ck, pe, pth_e)
            if delta > max_steer:
                delta = max_steer
            elif delta < -max_steer:
                delta = -max_steer
            pe = e
            pth_e = th_e

            msg = drive_param()
            if np.linalg.norm([x - waypoints[-1][0], y - waypoints[-1][1]]) < 0.3:
                msg.velocity = 0
                rospy.signal_shutdown("the car has reach the goal")
            else:
                msg.velocity = velocity
            msg.angle = -delta
            pub.publish(msg)
    def set_path(self, pose, vel, ax=[0, 1], ay=[0, 1]):
        self.state.update(pose.x, pose.y, pose.theta, vel)
        self.start_th = pose.theta

        self.cx, self.cy, self.cyaw, ck, s = cubic_spline_planner.calc_spline_course(
            ax, ay, ds=0.1)

        #  target course
        # self.cx = ax
        # self.cy = ay

        # self.cx.append(self.cx[len(self.cx)-1])
        # self.cy.append(self.cy[len(self.cy)-1])
        # self.cyaw.append(self.cyaw[len(self.cyaw)-1]

        self.last_idx = len(self.cx) - 1

        #  target course
        self.target_course = TargetCourse(self.cx, self.cy)
        self.target_idx, _ = self.target_course.search_target_index(
            self.state, self.k, self.linear_tolerance_outer, self.look_ahead)
    def __init__(self, initialState):
        self.k = 0.5  # control gain
        self.Kp = 1.0  # speed proportional gain
        self.dt = 0.1  # [s] time difference
        self.L = 0.29  # [m] Wheel base of vehicle
        self.max_steer = np.radians(30.0)  # [rad] max steering angle
        self.show_animation = True
        gpsMath = GpsMath()

        ax = [4300738.591950053, 4300735.762529756, 4300731.881422988, 4300725.061867644, 4300716.428532064]
        ay = [687530.1153441871, 687527.1996543248, 687525.1935662497, 687519.4075912425, 687510.02154688]
        ax = list(map(lambda x: gpsMath.locationToPoint(x, 9.083533, 480)["x"], [47.031944, 47.031745, 47.031807, 47.031965, 47.031944]))
        ay = list(map(lambda x: gpsMath.locationToPoint(47.031944, x, 480)["y"], [9.083533, 9.083629, 9.083816, 9.083692, 9.083533]))
        #ax = [4300796.650611158, 4300812.637012742, 4300812.637012742, 4300794.963600908, 4300796.650611158]
        #ay = [687608.2953105057, 687615.5013634935, 687615.5013634935, 687620.2303347178, 687608.2953105057]
        print(ax)
        print(ay)

        ds = 0.1

        self.cx, self.cy, self.cyaw, self.ck, self.s = cubic_spline_planner.calc_spline_course(
            ax, ay, ds=ds)

        self.target_speed = 3.0 / 3.6  # [m/s]

        self.max_simulation_time = 100.0

        # Initial state
        self.state = initialState

        self.last_idx = len(self.cx) - 1
        self.target_idx, _ = self.calc_target_index(self.state, self.cx, self.cy)
        self.time = 0.0
        self.x = [self.state.x]
        self.y = [self.state.y]
        self.yaw = [self.state.yaw]
        self.v = [self.state.v]
        self.t = [0.0]
        self.LOG.info("on {}/{}".format(self.x, self.y))
Exemple #33
0
def generate_path(is_default=True):
    x_path = []
    y_path = []
    # csp = imp.load_source('cubic_spline_planner.py', '/home/oks/catkin_ws/src/framework_sim/clrn_gplanner/scripts/cubic_spline_planner.py')
    if is_default == True:
        path_obj = Path()
        x_raw, y_raw = path_obj.default_raw_path()
        path_properties = path_obj.path_props(x_raw, y_raw)
        x_path, y_path = path_obj.remove_singular(x_raw, y_raw,
                                                  path_properties)
        x_refacto, y_refacto = path_obj.remove_initpoint(x_path, y_path)
        path_properties = path_obj.path_props(x_refacto, y_refacto)
        waypoints_x = x_path
        waypoints_y = y_path
        final_goal = path_properties[1]
        course_x, course_y, course_yaw, course_k, course_s = csp.calc_spline_course(
            waypoints_x, waypoints_y, ds=0.1)
        ref_speed = path_obj.get_ref_speed()
        speed_profile = calc_speed_profile(course_x, course_y, course_yaw,
                                           ref_speed)
    else:
        print "No Other than default"
    return x_path, y_path, course_x, course_y, course_yaw, course_k, course_s, final_goal, speed_profile
def main():
    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 = cubic_spline_planner.calc_spline_course(ax,
                                                                  ay,
                                                                  ds=0.1)

    target_speed = 30.0 / 3.6  # [m/s]

    max_simulation_time = 100.0

    # Initial state
    state = State(x=-0.0, y=5.0, yaw=np.radians(0.0), v=2.0)

    last_idx = len(cx) - 1
    time = 0.0
    x = [state.x]
    y = [state.y]
    yaw = [state.yaw]
    v = [state.v]
    t = [0.0]
    target_idx, _ = calc_target_index(state, cx, cy)
    show_animation = True
    dt = 0.1
    while max_simulation_time >= time and last_idx > target_idx:
        ai = longitude_control(target_speed, state.v)
        di, target_idx = latitude_control(state, cx, cy, cyaw, target_idx)
        state.update(ai, di)

        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_idx], cy[target_idx], "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 last_idx >= target_idx, "Cannot reach 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()
Exemple #35
0
    cv2.circle(traj, (draw_kalman_x, draw_kalman_y), 1, (255, 255, 255), 1)

    cv2.rectangle(traj, (10, 20), (600, 60), (0, 0, 0), -1)
    text = "Coordinates: x=%2fm y=%2fm z=%2fm" % (x, y, z)
    cv2.putText(traj, text, (20, 40), cv2.FONT_HERSHEY_PLAIN, 1,
                (255, 255, 255), 1, 8)

    cv2.imshow('Road facing camera', img)
    cv2.imshow('Trajectory', traj)
    cv2.waitKey(1)
    print(kalman_y.x)

    if counter > N_STEPS:
        break
    counter += 1

trajectory = np.array(trajectory)
cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
    list(trajectory[:, 0][::2]), list(trajectory[:, 1][::2]), ds=0.1)
plt.scatter(trajectory[:, 0], trajectory[:, 1], label='Filtered', s=0.5)
plt.scatter(trajectory[:, 2], trajectory[:, 3], label='Bicycle Model', s=0.5)
plt.scatter(trajectory[:, 4], trajectory[:, 5], label='GNSS', s=0.5)
plt.scatter(trajectory[:, 6],
            trajectory[:, 7],
            label='Visual Odometery',
            s=0.5)
plt.scatter(cx, cy, label='Spline', s=0.5)
plt.xticks([-3, -2, -1, 0, 1, 2, 3])
plt.legend(loc='best')
plt.show()
Exemple #36
0
def get_Switch_back_course2(dl):
    ax = [24.899999618530273, 24.900005340576172, 24.90001106262207,\
    24.900014877319336, 24.900020599365234, 24.900026321411133,\
    24.90003204345703, 24.89870262145996, 24.889429092407227,\
    24.864276885986328, 24.815370559692383, 24.735021591186523,\
    24.615943908691406, 24.29094886779785, 23.830982208251953,\
    23.252336502075195, 22.575511932373047, 21.824485778808594,\
    21.385210037231445, 20.937171936035156, 20.484573364257812,\
    20.03000259399414, 19.574806213378906, 19.1195068359375,\
    18.233142852783203, 17.346778869628906, 16.46041488647461,\
    15.574051856994629, 14.687687873840332, 13.801324844360352,\
    12.914960861206055, 12.028596878051758, 11.142233848571777,\
    10.25586986541748, 9.369505882263184, 8.93588638305664,\
    8.50233268737793, 8.069173812866211, 7.637269020080566,\
    7.208261489868164, 6.784831523895264, 6.3709259033203125,\
    6.020498275756836, 5.684054851531982, 5.282763481140137,\
    4.910910606384277, 4.570590019226074, 4.262977123260498,\
    3.9884414672851562, 3.7466609477996826, 3.5367300510406494,\
    3.357264518737793, 3.2064950466156006, 3.082350254058838,\
    2.9825286865234375, 2.9045591354370117, 2.8458502292633057,\
    2.803729772567749, 2.775472640991211, 2.75832462310791,\
    2.7495148181915283, 2.7462656497955322, 2.7457969188690186,\
    2.745792865753174, 2.745788812637329, 2.7457847595214844,\
    2.747112989425659, 2.7563867568969727, 2.7815396785736084,\
    2.8304450511932373, 2.910794734954834, 3.0298728942871094,\
    3.354867458343506, 3.8148345947265625, 4.393479824066162,\
    5.0703043937683105, 5.82133150100708, 6.260606288909912,\
    6.708644866943359, 7.161242961883545, 7.615814685821533,\
    8.07101058959961, 8.526309967041016, 9.351309776306152,\
    10.176310539245605, 11.001310348510742, 11.826310157775879,\
    12.651309967041016, 13.476309776306152, 14.411310195922852,\
    15.34631061553955, 16.28131103515625, 17.216310501098633,\
    18.151309967041016, 19.0863094329834, 20.021310806274414,\
    20.956310272216797, 21.89130973815918, 22.826311111450195]
    ay = [14.199999809265137, 15.033333778381348, 15.866666793823242,\
    16.700000762939453, 17.53333282470703, 18.366666793823242,\
    19.200000762939453, 19.65530014038086, 20.110496520996094,\
    20.565067291259766, 21.01766586303711, 21.46570587158203,\
    21.90498161315918, 22.65601348876953, 23.332843780517578,\
    23.911497116088867, 24.371471405029297, 24.696475982666016,\
    24.81555938720703, 24.895915985107422, 24.944826126098633,\
    24.969985961914062, 24.979265213012695, 24.98059844970703,\
    24.98060417175293, 24.980609893798828, 24.980615615844727,\
    24.980621337890625, 24.980627059936523, 24.980632781982422,\
    24.98063850402832, 24.98064422607422, 24.980648040771484,\
    24.980653762817383, 24.98065948486328, 24.97962760925293,\
    24.9724178314209, 24.95285987854004, 24.914812088012695,\
    24.852230072021484, 24.75927734375, 24.630525588989258,\
    24.485376358032227, 24.310237884521484, 24.048620223999023,\
    23.74658203125, 23.409381866455078, 23.042070388793945,\
    22.649396896362305, 22.235727310180664, 21.80500030517578,\
    21.36069679260254, 20.905839920043945, 20.442995071411133,\
    19.974287033081055, 19.50144386291504, 19.025814056396484,\
    18.54842185974121, 18.070003509521484, 17.59105110168457,\
    17.11186981201172, 16.63261604309082, 16.153350830078125,\
    15.52001667022705, 14.886683464050293, 14.253350257873535,\
    13.798049926757812, 13.342854499816895, 12.88828182220459,\
    12.435683250427246, 11.98764419555664, 11.548367500305176,\
    10.79733657836914, 10.120506286621094, 9.541853904724121,\
    9.081877708435059, 8.756874084472656, 8.637789726257324,\
    8.557435035705566, 8.508523941040039, 8.483365058898926,\
    8.474085807800293, 8.47275161743164, 8.472745895385742,\
    8.47274112701416, 8.472735404968262, 8.47273063659668,\
    8.472725868225098, 8.4727201461792, 8.4727144241333,\
    8.472708702087402, 8.472702026367188, 8.472696304321289,\
    8.47269058227539, 8.472684860229492, 8.472679138183594,\
    8.472672462463379, 8.47266674041748, 8.472661018371582]
    cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(ax,
                                                                  ay,
                                                                  ds=dl)

    ax = [22.826311111450195, 22.418312072753906, 22.010366439819336,\
    21.602720260620117, 21.196029663085938, 20.791549682617188,\
    20.39132308959961, 19.998374938964844, 19.213205337524414,\
    18.5056095123291, 17.90065574645996, 17.419771194458008,\
    17.079994201660156, 16.961217880249023, 16.878517150878906,\
    16.82509994506836, 16.793806076049805, 16.77730941772461,\
    16.768220901489258, 16.762981414794922, 16.75774383544922]
    ay = [8.472661018371582, 8.471787452697754, 8.465682029724121,\
    8.449118614196777, 8.416891098022461, 8.363858222961426,\
    8.285024642944336, 8.175670623779297, 7.835904121398926,\
    7.355029106140137, 6.750082015991211, 6.042492389678955,\
    5.257328033447266, 4.823906421661377, 4.38212776184082,\
    3.9358205795288086, 3.4873881340026855, 3.0381479263305664,\
    2.58868670463562, 2.2887322902679443, 1.9887781143188477]
    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
Exemple #37
0
def main():
    """Plot an example of Stanley steering control on a cubic spline."""
    #  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 = cubic_spline_planner.calc_spline_course(ax,
                                                                  ay,
                                                                  ds=0.1)

    target_speed = 30.0 / 3.6  # [m/s]

    max_simulation_time = 100.0

    # Initial state
    state = State(x=-0.0, y=5.0, yaw=np.radians(20.0), v=0.0)

    last_idx = len(cx) - 1
    time = 0.0
    x = [state.x]
    y = [state.y]
    yaw = [state.yaw]
    v = [state.v]
    t = [0.0]
    target_idx, _ = calc_target_index(state, cx, cy)

    while max_simulation_time >= time and last_idx > target_idx:
        ai = pid_control(target_speed, state.v)
        di, target_idx = stanley_control(state, cx, cy, cyaw, target_idx)
        state.update(ai, di)

        time += dt

        x.append(state.x)
        y.append(state.y)
        yaw.append(state.yaw)
        v.append(state.v)
        t.append(time)

        if show_animation:  # pragma: no cover
            plt.cla()
            # for stopping simulation with the esc key.
            plt.gcf().canvas.mpl_connect(
                'key_release_event',
                lambda event: [exit(0) if event.key == 'escape' else None])
            plt.plot(cx, cy, ".r", label="course")
            plt.plot(x, y, "-b", label="trajectory")
            plt.plot(cx[target_idx], cy[target_idx], "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 last_idx >= target_idx, "Cannot reach goal"

    if show_animation:  # pragma: no cover
        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)

        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()
def main():
    """Plot an example of Stanley steering control on a cubic spline."""
    #  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 = cubic_spline_planner.calc_spline_course(
        ax, ay, ds=0.1)

    target_speed = 30.0 / 3.6  # [m/s]

    max_simulation_time = 100.0

    # Initial state
    state = State(x=-0.0, y=5.0, yaw=np.radians(20.0), v=0.0)

    last_idx = len(cx) - 1
    time = 0.0
    x = [state.x]
    y = [state.y]
    yaw = [state.yaw]
    v = [state.v]
    t = [0.0]
    target_idx, _ = calc_target_index(state, cx, cy)

    while max_simulation_time >= time and last_idx > target_idx:
        ai = pid_control(target_speed, state.v)
        di, target_idx = stanley_control(state, cx, cy, cyaw, target_idx)
        state.update(ai, di)

        time += dt

        x.append(state.x)
        y.append(state.y)
        yaw.append(state.yaw)
        v.append(state.v)
        t.append(time)

        if show_animation:  # pragma: no cover
            plt.cla()
            plt.plot(cx, cy, ".r", label="course")
            plt.plot(x, y, "-b", label="trajectory")
            plt.plot(cx[target_idx], cy[target_idx], "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 last_idx >= target_idx, "Cannot reach goal"

    if show_animation:  # pragma: no cover
        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)

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