PID Controller Trajectory

PID autonomous driving Trajectory

def closed_loop_control(cx, cy, cyaw, ck, speed_value, goal,start):


    T = 500.0  # max simulation time
    # goal_dis = 0.3
    goal_dis = 2.0
    stop_speed = 0.05

    state = State(x=start[0], y=start[1], yaw=start[2], v=start[3])
    # state = State(x=120, y=60.0, yaw=1.57, 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)
    
    global gmapindex
    global gmindist
    global gvehicleyaw
    global gcyaw
    global gdiffyaw
    global gcurvature
    global gspeed
    global gangleout
    global gdelta


    gmapindex =[] 
    gmindist =[] 
    gvehicleyaw =[] 
    gcyaw =[] 
    gdiffyaw =[] 
    gcurvature =[] 
    gspeed =[] 
    gangleout =[] 
    gdelta =[] 

    global gspeedprofile
    global gstatespeed
    global gpidout

    gspeedprofile = []
    gstatespeed = []
    gpidout = []

    global gstatex
    global gstatey
    global gstateyaw
    global gstatev
    gstatex =[]
    gstatey =[]
    gstateyaw =[]
    gstatev =[]

    global gformA
    global gformB
    global gformC

    gformA = []
    gformB = []
    gformC = []

    
    while T >= time:
        di, target_ind = longitudinal_lateral_control(
            state, cx, cy, cyaw, ck, target_ind)
        ai = SpeedControl(speed_value[target_ind], state.v)

        state = positionupdate(state, ai, di)
    
        gstatex.append(state.x)
        gstatey.append(state.y)
        gstateyaw.append(state.yaw)
        gstatev.append(state.v)

        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("Finished")
            break

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

        # print ('target_ind',target_ind)

        if target_ind % 10 == 0 and animation:
            plt.cla()
            plt.plot(cx, cy, "-r", label="course")
            plt.plot(x, y, "ob", label="trajectory",ms=2)
            plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
            plt.axis("equal")
            plt.grid(True)
            tempspeed = state.v*3.6
            plt.title("speed:" + str(round(tempspeed, 2)) +
                      "tind:" + str(target_ind))
            plt.savefig('demomap_png/'+str(target_ind)+'.png')
            plt.pause(0.0001)

    csvparam = 'result.csv'
    if os.path.exists(csvparam):
        os.remove(csvparam)
    

    print ('len(gspeedprofile)',len(gspeedprofile),'len(gmapindex)',len(gmapindex))        

    totaldf = pd.DataFrame({'mapidx':gmapindex,'mindist':gmindist,'vehicleyaw':gvehicleyaw\
        ,'cyaw':gcyaw,'diffyaw':gdiffyaw,'curvature':gcurvature,'vehiclespeed':gspeed\
        ,'angleout':gangleout,'delta':gdelta,'speedprofile':gspeedprofile,'PIDSpeedIn':gstatespeed\
        ,'pidout':gpidout,'statex':gstatex,'statey':gstatey,'stateyaw':gstateyaw,'statev':gstatev\
        ,'gformA':gformA,'gformB':gformB,'gformC':gformC})    
    
    # totaldf['radius'] = 1.0/totaldf['curvature']

    totaldf['deltadegree'] = totaldf['delta']*57.3
    totaldf.to_csv('result.csv')

    return t, x, y, yaw, v

def demo_main():
    global mapx
    global mapy
    global maptheta

    LoadData()

    print("demo_main !!")
    # 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, 6.0, 6.0, 12.0]
    # ay = [0.0, 0.0, 6.0, 6.0]
    ax = mapx#[]
    ay = mapy#[]

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

    # start = [ax[0]+4.0,ay[0]+0.1,1.57,0]
    start = [ax[0]+4.0,ay[0]+0.1,10/57.3,0]
    cx, cy, cyaw, ck, s = gen_spline(ax, ay, ds=0.1)
    # radius = 1/ck
    target_speed = 10.0 / 3.6 #kph -> mps

    print ('len(ax),len(cx)',len(ax),len(cx))

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

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

    cnt = 0
    refvehiclecurvature = []
    refvehicleradius = []
    for cnt in range(len(x)):
        if cnt + 2 >= len(x):
            refvehiclecurvature.append(refvehiclecurvature[-1])
            refvehicleradius.append(refvehicleradius[-1])
        else:    
            curvatureval = curvature([[x[cnt],y[cnt]],[x[cnt+1],y[cnt+1]],[x[cnt+2],y[cnt+2]]])
            if curvatureval != 0.0:
                refvehiclecurvature.append(1/curvatureval)
            else:
                refvehiclecurvature.append(0.01)
            refvehicleradius.append(curvatureval)
        cnt += 1

    print ('len(refvehicleradius)',len(refvehicleradius),'len(x)',len(x))    

    csvparam = 'vehicletrace.csv'
    if os.path.exists(csvparam):
        os.remove(csvparam)
    spliedf = pd.DataFrame({'x':x,'y':y,'yaw':yaw,'refvehiclecurvature':refvehiclecurvature\
        ,'refvehicleradius':refvehicleradius})
    spliedf.to_csv('vehicletrace.csv')


    flg, _ = plt.subplots(1)
    print(len(ax), len(ay))
    plt.plot(ax, ay, "xb", label="input",ms=3.0)
    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()

    global filterdelta

    flg, ax = plt.subplots(1)
    plt.plot(gdelta, "-r", label="delta")
    plt.plot(filterdelta, "-b", label="filterdelta")
    
    plt.grid(True)
    plt.legend()
    plt.ylabel("delta degree")

    flg, ax = plt.subplots(1)
    plt.plot(yaw, "-r", label="vehicle state yaw")
    
    plt.grid(True)
    plt.legend()
    plt.ylabel("state yaw")
    plt.show()