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