WRC/pure_change_zhixian.py

"""

Path tracking simulation with pure pursuit steering control and PID speed control.

author: Atsushi Sakai (@Atsushi_twi)

"""
import datetime
import time
from loguru import logger
import math
import sqlite3
import threading
import time
import numpy as np

import geartest2
import sped
import tcp4
import test5

k = 0  # look forward gain
Lfc = 5  # look-ahead distance
Kp = 1.0  # speed propotional gain
dt = 0.1  # [s]
L = 2.9  # [m] wheel base of vehicle
tx = 0
ty = 0
fangxp = 0
a = 0
a_trace = 0
brk_open = 0
brk_change=0
e_trace=0

# show_animation = True
#

class State:

    def __init__(self, x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.car_yaw, v=sped.speed):
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v


def update(state, a):
    state.x = tcp4.x_car
    # print('stax', state.x)
    state.y = tcp4.y_car
    # print('stay', state.y)

    state.yaw = tcp4.car_yaw
    # print('stayaw', state.car_yaw)
    state.v = sped.speed + a * dt

    # state.x = tcp4.x_car + sped.speed * math.cos(tcp4.car_yaw) * dt
    # # print("state",state.x)
    # state.y = tcp4.y_car + sped.speed * math.sin(tcp4.car_yaw) * dt
    # state.car_yaw = tcp4.car_yaw + sped.speed / L * math.tan(delta) * dt
    # state.v = sped.speed + a * dt

    return state

    # return state


def PIDControl(target, current):
    a = Kp * (target - current)

    return a


def pure_pursuit_control(state, cx, cy, pind):
    global tx, ty, fangxp
    ind = calc_target_index(state, cx, cy)

    # if pind >= ind:
    #     ind = pind

    if ind < len(cx):
        tx = cx[ind]
        ty = cy[ind]
    else:
        tx = cx[-1]
        ty = cy[-1]
        ind = len(cx) - 1

    alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw

    if state.v < 0:  # back
        alpha = math.pi - alpha

    Lf = k * state.v + Lfc

    delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)
    if delta > np.pi / 6.0:
        delta = np.pi / 6.0
        # print('delta1', delta)

    elif delta < - np.pi / 6.0:
        delta = - np.pi / 6.0
        # print('delta2', delta)
    angle = delta * 57.3
    fangxp = int(angle * 18)
    print('fxp', fangxp)

    db_file = "turndb.db"
    conn = sqlite3.connect(db_file)
    cur = conn.cursor()
    # sql='INSERT INTO turndb(id) VALUES(1);'
    sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
    cur.execute(sql)
    conn.commit()
    cur.close()
    conn.close()
    time.sleep(0.5)

    return delta, ind


def calc_target_index(state, cx, cy):
    # search nearest point index
    dx = [state.x - icx for icx in cx]
    dy = [state.y - icy for icy in cy]
    d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
    ind = d.index(min(d))
    L = 0.0

    Lf = k * state.v + Lfc

    # search look ahead target point index
    while Lf > L and (ind + 1) < len(cx):
        dx = cx[ind + 1] - cx[ind]
        dy = cx[ind + 1] - cx[ind]
        L += math.sqrt(dx ** 2 + dy ** 2)
        ind += 1

    return ind


def main():
    global a,e_trace
    #  target course
    cx = [70.733, 70.703, 70.681, 70.661, 70.631, 70.608, 70.579, 70.554, 70.533, 70.509, 70.482, 70.457, 70.426,
          70.402, 70.38, 70.352, 70.322, 70.298, 70.277, 70.253, 70.23, 70.205, 70.18, 70.151, 70.121, 70.093, 70.072,
          70.046, 70.025, 70.0, 69.975, 69.95, 69.919, 69.895, 69.87, 69.84, 69.808, 69.778, 69.753, 69.722, 69.693,
          69.668, 69.642, 69.605, 69.573, 69.543, 69.524, 69.5, 69.47, 69.442, 69.414, 69.389, 69.367, 69.344, 69.323,
          69.297, 69.28, 69.256, 69.231, 69.21, 69.192, 69.166, 69.147, 69.127, 69.105, 69.087, 69.064, 69.04, 69.014,
          68.987, 68.957, 68.926, 68.895, 68.866, 68.837, 68.804, 68.776, 68.746, 68.72, 68.696, 68.663, 68.636, 68.606,
          68.573, 68.545, 68.519, 68.488, 68.46, 68.436, 68.406, 68.374, 68.343, 68.317, 68.29, 68.26, 68.228, 68.198,
          68.17, 68.147, 68.121, 68.099, 68.075, 68.052, 68.024, 67.995, 67.969, 67.946, 67.912, 67.885, 67.864, 67.832,
          67.799, 67.777, 67.748, 67.725, 67.689, 67.668, 67.64, 67.613, 67.584, 67.557, 67.532, 67.506, 67.475, 67.448,
          67.415, 67.382, 67.347, 67.314, 67.287, 67.25, 67.221, 67.189, 67.165, 67.14, 67.12, 67.091, 67.064, 67.022,
          66.999]
    cy = [33.88, 33.985, 34.086, 34.188, 34.298, 34.409, 34.516, 34.633, 34.736, 34.847, 34.962, 35.077, 35.195, 35.318,
          35.441, 35.563, 35.688, 35.812, 35.91, 36.011, 36.117, 36.227, 36.343, 36.506, 36.632, 36.758, 36.885, 37.013,
          37.147, 37.275, 37.408, 37.542, 37.681, 37.818, 37.956, 38.1, 38.241, 38.382, 38.527, 38.666, 38.804, 38.95,
          39.096, 39.24, 39.383, 39.526, 39.626, 39.765, 39.902, 40.033, 40.176, 40.31, 40.432, 40.561, 40.679, 40.793,
          40.904, 41.011, 41.116, 41.215, 41.314, 41.442, 41.544, 41.649, 41.758, 41.867, 41.98, 42.1, 42.22, 42.345,
          42.467, 42.592, 42.719, 42.839, 42.964, 43.079, 43.189, 43.305, 43.414, 43.517, 43.644, 43.744, 43.871,
          43.996, 44.111, 44.229, 44.355, 44.462, 44.564, 44.672, 44.777, 44.88, 44.982, 45.083, 45.18, 45.302, 45.409,
          45.505, 45.606, 45.713, 45.815, 45.919, 46.026, 46.127, 46.24, 46.348, 46.448, 46.565, 46.686, 46.786, 46.884,
          47.015, 47.122, 47.221, 47.321, 47.448, 47.554, 47.66, 47.761, 47.862, 47.971, 48.082, 48.188, 48.295, 48.411,
          48.525, 48.641, 48.76, 48.875, 48.991, 49.111, 49.218, 49.327, 49.433, 49.545, 49.645, 49.748, 49.845, 49.997,
          50.097]

    target_speed = 10.0 / 3.6  # [m/s]

    # initial state
    state = State(x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.car_yaw, v=sped.speed)

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

    while a == 1:
        ai = PIDControl(target_speed, state.v)
        di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
        state = update(state, ai)

        time = time + dt

        x.append(state.x)
        x1 = round(x[-1], 3)
        # print('x1', x[-1])
        y.append(state.y)
        y1 = round(y[-1], 3)
        yaw.append(state.yaw)
        v.append(state.v)
        t.append(time)
        shijian = datetime.datetime.now()
        print('纯跟踪运行')
        e_trace = abs(math.sqrt(pow((float(tcp4.east) - 67.872), 2) + pow((float(tcp4.north) - 45.89), 2)))
        print('e_trace', e_trace)
        gear_d()
        if e_trace < 1 and 160 < (float(tcp4.fang_xiang)) < 180:
            a = 2
            print('退出纯跟踪')
            shache()
            print('刹车')
        # logger.add(
        #     '预瞄点记录%s-%s-%s,%s:%s.log' % (shijian.year, shijian.month, shijian.day, shijian.hour, shijian.minute))
        # logger.debug('y {},x {},tx {}，ty {},fxp{},car_yaw{},北斗x {}，北斗y {},x_car{},y_car{},方位角{}，转换角{}', str(y1),
        #              str(x1), str(tx), str(ty), str(fangxp),
        #              str(state.yaw), str(tcp4.east), str(tcp4.north), str(tcp4.x_car),
        #              str(tcp4.y_car), str(tcp4.beidou[5]), str(tcp4.heading))
    #     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()


def shache():
    global brk_open,brk_change
    brk_open = 10
    brk_change = 1
    db_file = "geardb.db"
    conn = sqlite3.connect(db_file)
    cur = conn.cursor()
    # sql='INSERT INTO geardb(id) VALUES(1);'
    sql = "update geardb set brk_open='%s',brk_change='%s' WHERE id = 1" % (brk_open, brk_change)
    cur.execute(sql)
    conn.commit()
    cur.close()
    conn.close()


def gear_p():
    print('P')
    gear_cmd = 1
    auto_acc = 0
    db_file = "geardb.db"
    conn = sqlite3.connect(db_file)
    cur = conn.cursor()
    # sql='INSERT INTO geardb(id) VALUES(1);'
    sql = "update geardb set gear_cmd='%s',auto_acc='%s' WHERE id = 1" % (gear_cmd, auto_acc)
    cur.execute(sql)
    conn.commit()
    cur.close()
    conn.close()


def gear_r():
    gear_cmd = 2  # r
    print('r')
    auto_acc = 70
    db_file = "geardb.db"
    conn = sqlite3.connect(db_file)
    cur = conn.cursor()
    # sql='INSERT INTO geardb(id) VALUES(1);'
    sql = "update geardb set gear_cmd='%s',auto_acc='%s' WHERE id = 1" % (gear_cmd, auto_acc)
    cur.execute(sql)
    conn.commit()
    cur.close()
    conn.close()

def gear_d():
    gear_cmd = 4  # r
    # print('d')
    auto_acc = 48
    db_file = "geardb.db"
    conn = sqlite3.connect(db_file)
    cur = conn.cursor()
    # sql='INSERT INTO geardb(id) VALUES(1);'
    sql = "update geardb set gear_cmd='%s',auto_acc='%s' WHERE id = 1"%(gear_cmd,auto_acc)
    cur.execute(sql)
    conn.commit()
    cur.close()
    conn.close()
def car_back():
    print('进入倒库')
    while True:
        global fangxp, a_trace

        db_file = "turndb.db"
        conn = sqlite3.connect(db_file)
        cur = conn.cursor()
        # sql='INSERT INTO turndb(id) VALUES(1);'
        sql = "update turndb set angle='%s' WHERE id = 1" % fangxp
        cur.execute(sql)
        conn.commit()
        cur.close()
        conn.close()
        # d = math.sqrt(pow((east - x), 2) + pow((north - y), 2))

        a_trace = abs(math.sqrt(pow((float(tcp4.east) - 68.35), 2) + pow((float(tcp4.north) - 43.85), 2)))
        print('a_trace', a_trace)
        b_trace = abs(math.sqrt((float(tcp4.east) - 70.15) ** 2 + (float(tcp4.north) - 38.6) ** 2))
        c_trace = abs(math.sqrt((float(tcp4.east) - 70.7) ** 2 + (float(tcp4.north) - 38.4) ** 2))
        d_trace = abs(math.sqrt((float(tcp4.east) - 72) ** 2 + (float(tcp4.north) - 37.8) ** 2))
        # print("tcp4.east",tcp4.east)
        print('tcp_fangxiang', tcp4.fang_xiang)

        if 140.0 < (float(tcp4.fang_xiang)) < 180.0 and a_trace < 1:
            fangxp = -540

            print('右打死', fangxp)

        elif 70 < float(tcp4.fang_xiang) < 110 and b_trace < 1:
            fangxp = 0

            print('回正', fangxp)

        elif 70 < float(tcp4.fang_xiang) < 110 and c_trace < 1:
            fangxp = -540
            print('右打死2', fangxp)


        elif 60 < float(tcp4.fang_xiang) < 90 and d_trace < 1.2:
            fangxp = 0
            print('回正2', fangxp)
        else:
            print('lose')
        time.sleep(0.5)


def car_back_comply():
    global a, brk_open,brk_change,e_trace

    a = 1
    print('开始跟踪')

    main()
    while True:


        if e_trace < 1 and sped.speed == 0:
            gear_p()
            brk_open = 0
            brk_change=0
            print('松刹车')
            gear_r()
            print('挂倒挡')
            car_back()
        time.sleep(0.05)


if __name__ == '__main__':
    t1 = threading.Thread(target=test5.can165)  # 读取eps状态
    t2 = threading.Thread(target=test5.can0a5)  # 读取目前方向盘角度
    t3 = threading.Thread(target=test5.main)
    t4 = threading.Thread(target=test5.update)
    t5 = threading.Thread(target=car_back_comply)
    t6 = threading.Thread(target=tcp4.zuo_biao)
    t7 = threading.Thread(target=sped.sped)

    t8 = threading.Thread(target=geartest2.brk_open_option)
    t9 = threading.Thread(target=geartest2.can4b8)
    t10 = threading.Thread(target=geartest2.main)
    t11 = threading.Thread(target=geartest2.update)

    # 启动线程
    t5.start()
    t6.start()
    t7.start()
    t1.start()
    t2.start()
    t3.start()
    t8.start()
    t9.start()
    t10.start()
    t11.start()