LSX183
/
WRC
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: LSX183:ae7ef1bf8e9f17262e8c92ff8e5927030660118f
Branches Tags
LSX183:master
LSX183:master_pure
...
compare: LSX183:4710fac35d629893de30bd70230b5418cacd1025
Branches Tags
LSX183:master
LSX183:master_pure

3 Commits
ae7ef1bf8e ... 4710fac35d

Author SHA1 Message Date
liu 4710fac35d V0.1 2 years ago
liu 072e766429 V0.2 2 years ago
liu b9c1eea48e V0.2 2 years ago
2 changed files with 183 additions and 25 deletions
Show Stats

82
pure_test.py
Unescape Escape View File

@ -161,7 +161,9 @@ def pure_pursuit_control(state, cx, cy, pind):
# print('A',A) # print('A',A)
if len(TX_TY)>20: if len(TX_TY)>20:
Lfc=Lfc*curvature Lfc=Lfc*curvature*0.5
if Lfc==0:
Lfc=2
print('lcf',Lfc) print('lcf',Lfc)
alpta_1=math.atan2(ty - state.y, tx - state.x) alpta_1=math.atan2(ty - state.y, tx - state.x)
@ -239,25 +241,44 @@ def main():
global x1,y1 global x1,y1
# 设置目标路点 # 设置目标路点
cx = [ 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, 96.048, 96.078, 96.109, 96.139, 96.17, 96.203, 96.234, 96.266, 96.299, 96.33, 96.364, 96.395, 96.424,
96.456, 96.487, 96.514, 96.546, 96.576, 96.609, 96.638, 96.667, 96.701, 96.735, 96.765, 96.795, 96.825,
96.855, 96.888, 96.918, 96.953, 96.982, 97.013, 97.045, 97.095, 97.123, 97.157, 97.179, 97.208, 97.243,
97.277,
97.308, 97.342, 97.376, 97.407, 97.435, 97.47, 97.501, 97.534, 97.565, 97.595, 97.624, 97.649, 97.684, 97.716,
97.749,
97.776, 97.811, 97.84, 97.869, 97.901, 97.931, 97.96, 97.984, 98.016, 98.042, 98.07, 98.1, 98.128, 98.155,
98.188, 98.218, 98.244, 98.273, 98.304, 98.339, 98.366, 98.397, 98.42, 98.465, 98.495, 98.523, 98.549, 98.578,
98.61,
98.637, 98.667, 98.7, 98.729, 98.757, 98.789, 98.819, 98.843, 98.872, 98.892, 98.921, 98.955, 98.984, 99.01,
99.04, 99.06, 99.09, 99.121, 99.152, 99.178, 99.208, 99.233, 99.26, 99.29, 99.32, 99.349, 99.376, 99.4,
99.428,
99.456, 99.486, 99.514, 99.544, 99.571, 99.598, 99.627, 99.654, 99.682, 99.714, 99.741, 99.769, 99.799,
99.831,
99.859, 99.888, 99.932, 99.957, 99.987, 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.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.38, 101.404, 101.433, 101.468, 101.495, 101.521, 101.551, 101.575, 101.604, 101.632, 101.66, 101.683,
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.716,
101.744, 101.744, 101.776, 101.808, 101.83, 101.854, 101.889, 101.918, 101.945, 101.967, 101.99, 102.013, 102.033,
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.057,
102.082, 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.102, 102.116, 102.134, 102.149, 102.164, 102.176, 102.184, 102.187, 102.185, 102.183, 102.176, 102.165, 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, 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.606, 101.522, 101.44, 101.353, 101.272, 101.185, 101.09, 100.999, 100.9, 100.804, 100.706, 100.611,
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.505,
100.406, 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,
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.83,
98.623, 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,
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.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, 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, 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, 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,
@ -265,12 +286,24 @@ def main():
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, 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, 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, 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.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,
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.937,
79.785, 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,
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.856,
77.741, 77.625, 77.511, 77.403, 77.298, 77.199, 77.065, 76.938, 76.826, 76.707] 77.741, 77.625, 77.511, 77.403, 77.298, 77.199, 77.065, 76.938, 76.826, 76.707]
cy = [ cy = [
9.935, 9.788, 9.642, 9.494, 9.342, 9.194, 9.046, 8.896, 8.747, 8.598, 8.451, 8.302, 8.158, 8.012, 7.864,
7.718, 7.569, 7.425, 7.28, 7.132, 6.986, 6.841, 6.693, 6.543, 6.396, 6.246, 6.099, 5.95, 5.801, 5.653, 5.5,
5.351, 5.2, 4.974, 4.821, 4.669, 4.522, 4.373, 4.222, 4.068, 3.918, 3.77, 3.623, 3.472, 3.322, 3.173, 3.025,
2.882, 2.733, 2.583, 2.436, 2.289, 2.144, 1.994, 1.845, 1.695, 1.546, 1.397, 1.245, 1.091, 0.939, 0.791,
0.641, 0.493, 0.346, 0.197, 0.051, -0.102, -0.254, -0.404, -0.553, -0.708, -0.859, -1.012, -1.166, -1.321,
-1.474, -1.628, -1.858, -2.012, -2.164, -2.32, -2.473, -2.626, -2.776, -2.927, -3.078, -3.231, -3.384, -3.535,
-3.686, -3.836, -3.987, -4.136, -4.287, -4.43, -4.58, -4.728, -4.872, -5.013, -5.158, -5.302, -5.442, -5.585,
-5.728, -5.873, -6.015, -6.158, -6.305, -6.447, -6.59, -6.733, -6.877, -7.022, -7.165, -7.307, -7.449, -7.59,
-7.732, -7.871, -8.012, -8.151, -8.29, -8.429, -8.576, -8.718, -8.863, -9.006, -9.153, -9.371, -9.514, -9.657,
-9.789, -9.937, -10.083, -10.229, -10.378, -10.523, -10.668, -10.812, -10.958, -11.103, -11.247, -11.395, -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, -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, -13.3, -13.447, -13.593, -13.741, -13.889, -14.033, -14.182, -14.326, -14.474, -14.607, -14.754, -14.903,
@ -295,15 +328,14 @@ def main():
-28.984, -29.007, -29.039, -29.063, -29.097, -29.129, -29.158, -29.18, -29.206, -29.227, -29.251, -29.28, -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] -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] target_speed = 2 / 3.6 # [m/s]
T = 500.0 # 最大模拟时间 T = 500.0 # 最大模拟时间
# 设置车辆的出事状态 # 设置车辆的出事状态
yaw=math.radians(450-(347+180-360)) yaw=math.radians(450-(347+180-360))
x_car = np.cos(yaw) * 2.3 + float(100) x_car = np.cos(yaw) * 2.3 + float(96)
# print('x',x_car) # print('x',x_car)
# north = beidou[15] # 北向位置坐标：以基站为原点的地理坐标系下的北向位置，单位：米，小数点后 3 位 # north = beidou[15] # 北向位置坐标：以基站为原点的地理坐标系下的北向位置，单位：米，小数点后 3 位
y_car = -np.sin(yaw) * 2.3 + float(-9.78) y_car = -np.sin(yaw) * 2.3 + float(9.9)
# print('yaw_1',yaw) # print('yaw_1',yaw)

126
tcp4.py
Unescape Escape View File

@ -0,0 +1,126 @@
import datetime
import math
import socket
import numpy as np
from loguru import logger
clientSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
clientSocket.connect(('192.168.43.100', 4002))
# # 发
# sendData = input('>>')
# clientSocket.send(sendData.encode('gbk'))
# 收
# jishi=0
# while jishi<= 30:
x = 0
y = 0
yaw = 0
x_car = 0
y_car = 0
heading = 0
east = 0
north = 0
beidou = 0
def zuo_biao():
global yaw, x_car, y_car, east, north, beidou, heading
while True:
recvData = clientSocket.recv(1024)
tcpdata = recvData.decode('gbk')
# print(tcpdata)
beidou = tcpdata.split(',')
# print(beidou)
# print(len(beidou))
# KSXT = beidou[0]
# if KSXT=='$KSXT':
KSXT = beidou[0] # 帧头
time0 = beidou[1] # 时间
lon = beidou[2] # 经度
lat = beidou[3] # 纬度
height = beidou[4] # 海拔
heading = beidou[5] # 方位角
heading = float(heading)
heading = heading + 180
if heading > 360:
heading = heading - 360
if 0 < heading <= 90:
yaw = math.radians(90 - float(heading))
yaw = round(yaw, 3)
else:
yaw = math.radians(450 - float(heading))
yaw = round(yaw, 3)
print("yaw", yaw)
# pitch = beidou[6] # 俯仰角
# tracktrue = beidou[7] # 速度角
# vel = beidou[8] # 水平速度
# roll = beidou[9] # 横滚 空
# pos_qual = beidou[10] # GNSS定位质量指示符0 = 定位不可用或无效 1 = 单点定位 2 = RTK 浮点解 3 = RTK 固定解
# heading_qual = beidou[11] # GNSS定向质量指示符 0 = 定位不可用或无效 1 = 单点定位 2 = RTK 浮点解 3 = RTK 固定解
# solnsvsh = beidou[12] # 前天线使用卫星数 前天线当前参与解算的卫星数量
# solnsvsb = beidou[13] # 后天线使用卫星数 前天线当前参与解算的卫星数量
east = beidou[14] # 东向位置坐标：以基站为原点的地理坐标系下的东向位置，单位：米，小数点后 3 位
x_car = np.cos(yaw) * 2.3 + float(east)
x_car = round(x_car, 3)
# print('x',x_car)
north = beidou[15] # 北向位置坐标：以基站为原点的地理坐标系下的北向位置，单位：米，小数点后 3 位
y_car = -np.sin(yaw) * 2.3 + float(north)
y_car = round(y_car, 3)
shijian = datetime.datetime.now()
# logger.add(
# 'x,y记录%s-%s-%s,%s:%s.log' % (shijian.year, shijian.month, shijian.day, shijian.hour, shijian.minute))
# logger.debug('北斗x {}，北斗y {},x_car{},y_car{},方位角{}，转换角{}', str(east), str(north), str(x_car),
# str(y_car), str(beidou[5]), str(heading))
# up = beidou[16] # 天向位置坐标：以基站为原点的地理坐标系下的天顶向位置，单位：米，小数点后 3 位
# eastvel = beidou[17] # 东向速度：地理坐标系下的东向速度，小数点后 3位，单位：Km/h(如无为空)
# northvel = beidou[18] # 北向速度：地理坐标系下的北向速度，小数点后 3位，单位：Km/h(如无为空)
# uphvel = beidou[19] # 天向速度：地理坐标系下的天顶向速度，小数点后3 位，单位：Km/h(如无为空)
# unknown = beidou[20]
# check = beidou[21] # 异或校验（十六进制字符串，从帧头开始校验）
# east = float(east)
# north = float(north)
# print('接收时间为:',datetime.datetime.now())
# print('方位角：%s; 东向位置坐标：%s; 北向位置坐标：%s; ' % ( heading, east, north))
# print('\n')
# shijian=datetime.datetime.now()
# file = open('1.txt', 'a')
# file.write(str('方位角：%s; 东向位置坐标：%s; 北向位置坐标：%s; ' % (heading, east, north)))
# file.write('\n')
# jishi=jishi+1
# time.sleep(0.05)
# else:
# print('未接收到定位消息，重新连接')
# clientSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# clientSocket.connect(('192.168.43.100', 4002))
# # clientSocket.close()
# d = math.sqrt(pow((east - x), 2) + pow((north - y), 2))
# if 0.2>=d >= 0.1:
# print('大于', d)
# print('\n')
# file = open('1.txt', 'a')
# file.write(str('方位角：%s; 东向位置坐标：%s; 北向位置坐标：%s;与上一点的距离为%s ' % (heading, x, y,d)))
# file.write('\n')
# x = east
# y = north
# print(x, y)
# else:
# print('不在范围', d)
# print('\n')
# if x==0 and y==0:
# x = east
# y = north
# time.sleep(0.05)
# zuo_biao()
Write Preview
Loading…
Cancel
Save