|
|
import time
|
|
|
# import txt2
|
|
|
import numpy as np
|
|
|
import math
|
|
|
import matplotlib.pyplot as plt
|
|
|
from loguru import logger
|
|
|
# import tcp4
|
|
|
import sqlite3
|
|
|
import threading
|
|
|
import CUR
|
|
|
ag = [0,0]
|
|
|
a = []
|
|
|
|
|
|
# import test5
|
|
|
k = 0 # 前视距离系数
|
|
|
Lfc =3# 前视距离
|
|
|
Kp = 1.0 # 速度P控制器系数
|
|
|
dt = 0.1 # 时间间隔，单位：s
|
|
|
L = 2.7 # 车辆轴距，单位：m
|
|
|
x1=[]
|
|
|
y1=[]
|
|
|
# yaw1 = math.radians(414)
|
|
|
# print('y',yaw1)
|
|
|
# arc=math.atan2(1, 1)
|
|
|
# print('acr',arc)
|
|
|
yaw = math.radians(450 - (344 + 180 - 360))
|
|
|
f=0
|
|
|
T = []
|
|
|
TX = []
|
|
|
curvature=0
|
|
|
A=0
|
|
|
B=0
|
|
|
C=0
|
|
|
def calculate_curvature(point_a, point_b, point_c):
|
|
|
global curvature
|
|
|
# 将输入点转换为 NumPy 数组
|
|
|
a = np.array(point_a)
|
|
|
b = np.array(point_b)
|
|
|
c = np.array(point_c)
|
|
|
|
|
|
# 计算各边长
|
|
|
ab = np.linalg.norm(a - b)
|
|
|
bc = np.linalg.norm(b - c)
|
|
|
ca = np.linalg.norm(c - a)
|
|
|
|
|
|
# 使用海伦公式计算三角形面积
|
|
|
s = (ab + bc + ca) / 2
|
|
|
area = np.sqrt(s * (s - ab) * (s - bc) * (s - ca))
|
|
|
|
|
|
# 计算曲率
|
|
|
curvature = 4 * area / (ab * bc * ca)
|
|
|
print('cur',curvature)
|
|
|
# i=0
|
|
|
# # a = path[i]
|
|
|
# # b = path[i + 10]
|
|
|
# # c = path[i + 20]
|
|
|
# # curvature = calculate_curvature(a, b, c)
|
|
|
# i=i+1
|
|
|
# print(i)
|
|
|
# return curvature
|
|
|
|
|
|
class VehicleState:
|
|
|
# state = VehicleState(x = tcp4.x_car, y =tcp4.y_car, yaw = tcp4.yaw, v = 0)
|
|
|
|
|
|
def __init__(self, x=88, y=47, yaw=yaw, v=0.0):
|
|
|
self.x = x
|
|
|
self.y = y
|
|
|
self.yaw = yaw
|
|
|
self.v = v
|
|
|
def update(state, a, delta):
|
|
|
state.x =state.x + state.v * math.cos(state.yaw) * dt
|
|
|
# print("state.x",state.x)
|
|
|
state.y = state.y + state.v * math.sin(state.yaw) * dt
|
|
|
# print("state.y",state.y)
|
|
|
|
|
|
state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
|
|
|
# print('state.yaw',state.yaw)
|
|
|
state.v = state.v + a * dt
|
|
|
return state
|
|
|
def PControl(target, current):
|
|
|
a = Kp * (target - current)
|
|
|
return a
|
|
|
def remove_adjacent_duplicates(coords):
|
|
|
result = []
|
|
|
for coord in coords:
|
|
|
if not result or coord != result[-1]:
|
|
|
result.append(coord)
|
|
|
print('coor',result)
|
|
|
def pure_pursuit_control(state, cx, cy, pind):
|
|
|
global ag,f, Lfc, delta, curvature, A, B, C
|
|
|
ind = calc_target_index(state, cx, cy)
|
|
|
# if pind >= ind:
|
|
|
# ind = pind
|
|
|
if ind < len(cx):
|
|
|
tx = cx[ind]
|
|
|
# print('tx1',tx)
|
|
|
|
|
|
ty = cy[ind]
|
|
|
# print('ty1',ty)
|
|
|
TX.append((tx, ty))
|
|
|
# print(TX)
|
|
|
else:
|
|
|
tx = cx[-1]
|
|
|
print('tx2',tx)
|
|
|
|
|
|
ty = cy[-1]
|
|
|
print('ty2',ty)
|
|
|
ind = len(cx) - 1
|
|
|
|
|
|
T.append(TX)
|
|
|
# print('T0',T[0][0])
|
|
|
# print('t',TX)
|
|
|
# A=0
|
|
|
# if len(T)>20:
|
|
|
# remove_adjacent_duplicates(T)
|
|
|
# seen = set()
|
|
|
# coords_without_duplicates = []
|
|
|
# for coord in T:
|
|
|
# if coord not in seen:
|
|
|
# coords_without_duplicates.append(coord)
|
|
|
# seen.add(coord)
|
|
|
# else:
|
|
|
# coords_without_duplicates.remove(coord)
|
|
|
#
|
|
|
# print(coords_without_duplicates)
|
|
|
# return result
|
|
|
TX_TY = [x for i, x in enumerate(T[0]) if i == 0 or x != T[0][i - 1]]
|
|
|
# print('txty',TX_TY)
|
|
|
if len(TX_TY)>16:
|
|
|
A=TX_TY[-1]
|
|
|
B=TX_TY[-4]
|
|
|
C=TX_TY[-8]
|
|
|
calculate_curvature(A,B,C)
|
|
|
if curvature>0:
|
|
|
|
|
|
a.append(curvature)
|
|
|
|
|
|
|
|
|
print('a', a)
|
|
|
# if len(T)>300:
|
|
|
# for i in range(len(T)):
|
|
|
# global curvature
|
|
|
# curvature=calculate_curvature(T[0][i],T[0][i+10],T[0][i+20])
|
|
|
# print('cur,1',curvature)
|
|
|
# print('to',T[0][i])
|
|
|
# print('t1',T[0][i+10])
|
|
|
# print('t2',T[0][i+20])
|
|
|
if curvature>=0.18:
|
|
|
Lfc=0.05
|
|
|
# print('cur', curvature)
|
|
|
# print('lfc', Lfc)
|
|
|
if 0.1<=curvature< 0.18:
|
|
|
Lfc = 0.2
|
|
|
# print('cur', curvature)
|
|
|
# print('lfc', Lfc)
|
|
|
|
|
|
else:
|
|
|
Lfc = 1.5
|
|
|
# print('cur', curvature)
|
|
|
# print('lfc', Lfc)
|
|
|
|
|
|
# print('A',A)
|
|
|
alpta_1=math.atan2(ty - state.y, tx - state.x)
|
|
|
|
|
|
alpha = alpta_1- state.yaw
|
|
|
# print('alpha',alpha)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
delta = math.atan2(2.0 * L * math.sin(alpha) / Lfc, 1.0)
|
|
|
# print('delta',delta)
|
|
|
|
|
|
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)
|
|
|
|
|
|
# print('deltal1',delta1)
|
|
|
angle = delta * 57.3
|
|
|
|
|
|
#
|
|
|
# fangxp = int((angle) *18)
|
|
|
# # fxp=int(fangxp)
|
|
|
# d=(fangxp-f)
|
|
|
# print('d',d)
|
|
|
# if d>4:
|
|
|
# d=4
|
|
|
# fangxp = f + d
|
|
|
# fangxp=f+d
|
|
|
#
|
|
|
# if d<-4:
|
|
|
# d=-4
|
|
|
# fangxp=f+d
|
|
|
# f = fangxp
|
|
|
# print('f', f)
|
|
|
#
|
|
|
#
|
|
|
# # print('fangx[',fangxp)
|
|
|
#
|
|
|
#
|
|
|
# ag.append(fangxp)
|
|
|
# print('ag',ag)
|
|
|
#
|
|
|
# print('fxp',fangxp)
|
|
|
|
|
|
|
|
|
|
|
|
time.sleep(0.04)
|
|
|
|
|
|
|
|
|
# print(delta)
|
|
|
return delta, ind
|
|
|
def calc_target_index(state, cx, cy):
|
|
|
# 搜索最临近的路点
|
|
|
dx = [state.x - icx for icx in cx]
|
|
|
# print('dx',dx)
|
|
|
dy = [state.y - icy for icy in cy]
|
|
|
# print('dy',dy)
|
|
|
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
|
|
|
# print('d',d)
|
|
|
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
|
|
|
return ind
|
|
|
def main():
|
|
|
global x1,y1
|
|
|
# 设置目标路点
|
|
|
cx = [
|
|
|
100.012, 100.043, 100.076, 100.108, 100.135, 100.167, 100.191, 100.221,
|
|
|
100.252,
|
|
|
100.281, 100.311, 100.342, 100.371, 100.394, 100.427, 100.455, 100.49, 100.522, 100.553, 100.58, 100.614,
|
|
|
100.641,
|
|
|
100.672, 100.702, 100.728, 100.761, 100.792, 100.824, 100.849, 100.876, 100.909, 100.942, 100.967, 100.999,
|
|
|
101.031, 101.059, 101.09, 101.121, 101.15, 101.175, 101.204, 101.235, 101.264, 101.288, 101.32, 101.348,
|
|
|
101.38,
|
|
|
101.404, 101.433, 101.468, 101.495, 101.521, 101.551, 101.575, 101.604, 101.632, 101.66, 101.683, 101.716,
|
|
|
101.744,
|
|
|
101.776, 101.808, 101.83, 101.854, 101.889, 101.918, 101.945, 101.967, 101.99, 102.013, 102.033, 102.057,
|
|
|
102.082,
|
|
|
102.102, 102.116, 102.134, 102.149, 102.164, 102.176, 102.184, 102.187, 102.185, 102.183, 102.176, 102.165,
|
|
|
102.141, 102.112, 102.086, 102.058, 102.031, 101.989, 101.955, 101.912, 101.861, 101.805, 101.748, 101.68,
|
|
|
101.606,
|
|
|
101.522, 101.44, 101.353, 101.272, 101.185, 101.09, 100.999, 100.9, 100.804, 100.706, 100.611, 100.505,
|
|
|
100.406,
|
|
|
100.299, 100.195, 100.085, 99.979, 99.867, 99.739, 99.62, 99.493, 99.363, 99.231, 99.102, 98.965, 98.83,
|
|
|
98.623,
|
|
|
98.48, 98.339, 98.198, 98.06, 97.927, 97.794, 97.659, 97.534, 97.402, 97.268, 97.143, 97.023, 96.896, 96.773,
|
|
|
96.645, 96.518, 96.393, 96.26, 96.127, 95.988, 95.845, 95.706, 95.558, 95.409, 95.258, 95.102, 94.945, 94.788,
|
|
|
94.633, 94.475, 94.315, 94.16, 94.002, 93.903, 93.75, 93.589, 93.434, 93.274, 93.118, 92.963, 92.807, 92.648,
|
|
|
92.487, 92.333, 92.18, 92.022, 91.864, 91.711, 91.553, 91.407, 91.252, 91.102, 90.954, 90.806, 90.652, 90.511,
|
|
|
90.354, 90.203, 90.049, 89.89, 89.737, 89.581, 89.422, 89.26, 89.108, 88.949, 88.793, 88.636, 88.48, 88.326,
|
|
|
88.168, 88.01, 87.855, 87.7, 87.548, 87.395, 87.243, 87.088, 86.939, 86.785, 86.635, 86.482, 86.334, 86.185,
|
|
|
86.038, 85.886, 85.666, 85.516, 85.367, 85.217, 85.067, 84.913, 84.766, 84.62, 84.473, 84.322, 84.171, 84.022,
|
|
|
83.871, 83.722, 83.576, 83.429, 83.282, 83.132, 82.981, 82.834, 82.687, 82.524, 82.375, 82.225, 82.072,
|
|
|
81.918,
|
|
|
81.772, 81.618, 81.457, 81.309, 81.154, 80.998, 80.843, 80.694, 80.543, 80.385, 80.237, 80.086, 79.937,
|
|
|
79.785,
|
|
|
79.639, 79.493, 79.355, 79.205, 79.063, 78.922, 78.784, 78.647, 78.517, 78.384, 78.25, 78.119, 77.986, 77.856,
|
|
|
77.741, 77.625, 77.511, 77.403, 77.298, 77.199, 77.065, 76.938, 76.826, 76.707]
|
|
|
cy = [
|
|
|
-9.789, -9.937, -10.083, -10.229, -10.378, -10.523, -10.668, -10.812, -10.958, -11.103, -11.247, -11.395,
|
|
|
-11.54, -11.689, -11.832, -11.978, -12.121, -12.267, -12.412, -12.559, -12.705, -12.85, -13.006, -13.15,
|
|
|
-13.3, -13.447, -13.593, -13.741, -13.889, -14.033, -14.182, -14.326, -14.474, -14.607, -14.754, -14.903,
|
|
|
-15.051, -15.195, -15.346, -15.491, -15.642, -15.794, -15.945, -16.091, -16.249, -16.401, -16.546, -16.696,
|
|
|
-16.843, -16.986, -17.132, -17.272, -17.413, -17.553, -17.694, -17.839, -17.984, -18.128, -18.271, -18.413,
|
|
|
-18.558, -18.705, -18.857, -19.008, -19.149, -19.293, -19.441, -19.586, -19.73, -19.876, -20.023, -20.169,
|
|
|
-20.317, -20.462, -20.61, -20.763, -20.912, -21.067, -21.225, -21.373, -21.528, -21.675, -21.818, -21.953,
|
|
|
-22.076, -22.203, -22.372, -22.481, -22.601, -22.718, -22.832, -22.95, -23.07, -23.192, -23.315, -23.439,
|
|
|
-23.56, -23.68, -23.799, -23.912, -24.022, -24.132, -24.242, -24.34, -24.435, -24.518, -24.605, -24.686,
|
|
|
-24.761, -24.838, -24.911, -24.978, -25.039, -25.1, -25.154, -25.213, -25.253, -25.307, -25.361, -25.408,
|
|
|
-25.451, -25.493, -25.53, -25.573, -25.62, -25.642, -25.665, -25.683, -25.703, -25.718, -25.732, -25.747,
|
|
|
-25.758, -25.759, -25.769, -25.778, -25.789, -25.799, -25.799, -25.8, -25.8, -25.8, -25.8, -25.807, -25.816,
|
|
|
-25.823, -25.836, -25.841, -25.861, -25.884, -25.908, -25.936, -25.961, -25.99, -26.017, -26.048, -26.084,
|
|
|
-26.118, -26.141, -26.176, -26.213, -26.247, -26.274, -26.299, -26.34, -26.373, -26.403, -26.435, -26.467,
|
|
|
-26.5, -26.532, -26.563, -26.599, -26.624, -26.661, -26.683, -26.715, -26.748, -26.78, -26.813, -26.842,
|
|
|
-26.873, -26.908, -26.943, -26.977, -27.008, -27.043, -27.068, -27.095, -27.133, -27.164, -27.193, -27.224,
|
|
|
-27.255, -27.287, -27.319, -27.348, -27.381, -27.407, -27.448, -27.483, -27.518, -27.55, -27.587, -27.615,
|
|
|
-27.643, -27.673, -27.697, -27.725, -27.753, -27.782, -27.823, -27.849, -27.876, -27.901, -27.931, -27.955,
|
|
|
-27.982, -28.009, -28.036, -28.065, -28.088, -28.117, -28.142, -28.172, -28.201, -28.232, -28.251, -28.278,
|
|
|
-28.304, -28.333, -28.361, -28.38, -28.403, -28.436, -28.464, -28.495, -28.531, -28.558, -28.581, -28.618,
|
|
|
-28.649, -28.679, -28.711, -28.74, -28.769, -28.792, -28.825, -28.851, -28.877, -28.901, -28.931, -28.955,
|
|
|
-28.984, -29.007, -29.039, -29.063, -29.097, -29.129, -29.158, -29.18, -29.206, -29.227, -29.251, -29.28,
|
|
|
-29.304, -29.328, -29.346, -29.369, -29.384, -29.409, -29.426, -29.458, -29.479, -29.499]
|
|
|
|
|
|
|
|
|
target_speed = 2 / 3.6 # [m/s]
|
|
|
T = 500.0 # 最大模拟时间
|
|
|
# 设置车辆的出事状态
|
|
|
yaw=math.radians(450-(347+180-360))
|
|
|
x_car = np.cos(yaw) * 2.3 + float(100)
|
|
|
# print('x',x_car)
|
|
|
# north = beidou[15] # 北向位置坐标：以基站为原点的地理坐标系下的北向位置，单位：米，小数点后 3 位
|
|
|
y_car = -np.sin(yaw) * 2.3 + float(-9.78)
|
|
|
|
|
|
# print('yaw_1',yaw)
|
|
|
|
|
|
state = VehicleState(x =x_car, y=y_car, yaw = float(yaw), v = 0)
|
|
|
# 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 lastIndex > target_ind:
|
|
|
|
|
|
ai = PControl(target_speed, state.v)
|
|
|
di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
|
|
|
fxp=int(di*57.3*18)
|
|
|
# print('fxp1',fxp)
|
|
|
state = update(state, ai, di)
|
|
|
time = time + dt
|
|
|
x.append(state.x)
|
|
|
x1=x[-1]
|
|
|
# print('x1',x[-1])
|
|
|
y.append(state.y)
|
|
|
y1=y[-1]
|
|
|
# print('y1',y[-1])
|
|
|
yaw.append(state.yaw)
|
|
|
yaw1=yaw[-1]
|
|
|
# print('yaw',yaw[-1])
|
|
|
v.append(state.v)
|
|
|
# print('v',v[-1])
|
|
|
t.append(time)
|
|
|
# logger.add('file.log')
|
|
|
# logger.debug('角度 {},y {},x {}',str(yaw1),str(y1),str(x1))
|
|
|
# f = open('data1,txt', 'a')
|
|
|
# # print('fff')
|
|
|
# f.write("角度"+str(yaw1))
|
|
|
# f.write('\n')
|
|
|
# f.write("\n")
|
|
|
# f.close()
|
|
|
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)
|
|
|
|
|
|
plt.ioff()
|
|
|
plt.show()
|
|
|
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)
|
|
|
t8=threading.Thread(target=CUR.calculate_curvature)
|
|
|
# t6 = threading.Thread(target=tcp4.zuo_biao)
|
|
|
# t7=threading.Thread(target=txt2.txt)
|
|
|
#
|
|
|
# # 启动线程
|
|
|
# t7.start()
|
|
|
t5.start()
|
|
|
|
|
|
# t6.start()
|
|
|
# t1.start()
|
|
|
# t2.start()
|
|
|
# t3.start()
|
|
|
# t4.start() |