wrc_back_GXK/pure5.1.py

import datetime
import time
from loguru import logger
import numpy as np
import math
# import matplotlib.pyplot as plt
import tcp4
import sqlite3
import threading
import test5
import sped

k = 0  # 前视距离系数
Lfc = 6# 前视距离
Kp = 1.0  # 速度P控制器系数
dt = 0.1  # 时间间隔，单位：s
L = 2.7  # 车辆轴距，单位：m
tx=0
ty=0
fangxp=0

class VehicleState:
    # state = VehicleState(x = tcp4.x_car, y =tcp4.y_car, yaw = tcp4.yaw, v = 0)

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


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


def PControl(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]
        print('tx', tx)
        ty = cy[ind]
        print('ty', ty)
    else:
        tx = cx[-1]
        # print('tx1',tx)

        ty = cy[-1]

        # print('ty1',ty)

        ind = len(cx) - 1

    alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw
    # print('alpha',alpha)
    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)

    file = open('1.txt', 'a')
    file.write(str('北向位置坐标：%s; ' % (fangxp)))
    file.write('\n')

    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.04)
    # print(delta)
    return delta, ind


def calc_target_index(state, cx, cy):
    # 搜索最临近的路点
    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))
    # print('ind',ind)
    L = 0.0
    Lf = k * state.v + Lfc
    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
        # print('ind1', ind)

    return ind


def main():
    #  设置目标路点
    cx = [75.527, 75.629, 75.727, 75.826, 75.924, 76.026, 76.136, 76.235, 76.338, 76.438, 76.548, 76.652, 76.771,
          76.891, 76.996, 77.108, 77.227, 77.339, 77.442, 77.551, 77.655, 77.761, 77.872, 77.981, 78.087, 78.197, 78.32,
          78.428, 78.539, 78.647, 78.753, 78.849, 78.968, 79.078, 79.186, 79.285, 79.387, 79.487, 79.592, 79.694,
          79.795, 79.892, 79.988, 80.092, 80.189, 80.272, 80.361, 80.447, 80.53, 80.611, 80.695, 80.785, 80.871, 80.957,
          81.041, 81.117, 81.197, 81.267, 81.345, 81.42, 81.489, 81.554, 81.605, 81.648, 81.685, 81.709, 81.738, 81.772,
          81.802, 81.854, 81.907, 81.966, 82.025, 82.081, 82.138, 82.195, 82.248, 82.299, 82.355, 82.408, 82.46, 82.51,
          82.558, 82.609, 82.657, 82.702, 82.749, 82.793, 82.83, 82.865, 82.896, 82.938, 82.971, 83.001, 83.03, 83.057,
          83.084, 83.111, 83.127, 83.136, 83.128, 83.111, 83.086, 83.061, 83.034, 82.998, 82.958, 82.918, 82.886, 82.853,
          82.819, 82.789, 82.755, 82.71, 82.678, 82.647, 82.614, 82.576, 82.548, 82.521, 82.503]
    cy = [-20.969, -20.977, -21.0, -21.023, -21.046, -21.062, -21.08, -21.097, -21.108, -21.122, -21.136, -21.148,
          -21.151, -21.155, -21.162, -21.159, -21.159, -21.158, -21.153, -21.145, -21.134, -21.131, -21.115, -21.101,
          -21.083, -21.066, -21.054, -21.032, -21.001, -20.973, -20.942, -20.907, -20.873, -20.837, -20.8, -20.755,
          -20.717, -20.675, -20.626, -20.578, -20.528, -20.48, -20.428, -20.371, -20.311, -20.247, -20.192, -20.132,
          -20.073, -20.013, -19.952, -19.886, -19.812, -19.74, -19.671, -19.597, -19.522, -19.45, -19.369, -19.293,
          -19.217, -19.127, -19.034, -18.94, -18.836, -18.737, -18.632, -18.529, -18.428, -18.329, -18.24, -18.155,
          -18.066, -17.978, -17.886, -17.799, -17.707, -17.619, -17.524, -17.435, -17.342, -17.254, -17.161, -17.061,
          -16.965, -16.865, -16.76, -16.656, -16.561, -16.462, -16.365, -16.25, -16.142, -16.036, -15.923, -15.824,
          -15.723, -15.612, -15.507, -15.398, -15.297, -15.189, -15.076, -14.977, -14.871, -14.763, -14.656, -14.556,
          -14.46, -14.36, -14.26, -14.161, -14.051, -13.941, -13.841, -13.741, -13.639, -13.531, -13.433, -13.328, -13.228]
    target_speed = 5 / 3.6  # [m/s]
    T = 500.0  # 最大模拟时间
    # 设置车辆的出事状态
    state = VehicleState(x=tcp4.x_car, y=tcp4.y_car, yaw=tcp4.yaw, v=sped.speed)
    # state = VehicleState(x=89, y=20, yaw=0.0, v=0.0)
    lastIndex = len(cx) - 1
    time = 0.0
    x = [state.x]
    # print('x',x)
    y = [state.y]
    # print('y',y)
    yaw = [state.yaw]
    v = [state.v]
    t = [0.0]
    target_ind = calc_target_index(state, cx, cy)
    while True:
        # print('heading',tcp4.heading)
        ai = PControl(target_speed, state.v)
        di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
        state = update(state, ai, di)
        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)
        print('y1', y[-1])
        yaw.append(state.yaw)
        yaw1=y[-1]*57.3
        print('yaw', yaw[-1])
        t.append(time)
        # logger.add('file.log')
        # logger.debug('角度 {},y {},x {},北斗角度 {}，北斗x {}，北斗y {}', str(yaw1), str(y1), str(x1),str(tcp4.heading),str(tcp4.east),str(tcp4.north))
        v.append(state.v)
        # print('v', v[-1])

        t.append(time)
        # plt.cla()
        # plt.plot(cx, cy, ".r", label="course")
        # plt.plot(x, y, "-b", label="trajectory")
        # plt.plot(cx[target_ind], cy[target_ind], "go", label="target")
        # plt.axis("equal")
        # plt.grid(True)
        # plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
        # plt.pause(0.001)
        shijian = datetime.datetime.now()
        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{},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 __name__ == '__main__':
    # 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=main)
    t6 = threading.Thread(target=tcp4.zuo_biao)
    t7 = threading.Thread(target=sped.sped)
    # 启动线程
    t5.start()
    t6.start()
    t7.start()
    t1.start()
    t2.start()
    t3.start()
    # t4.start()