def gnss_route_xy():
# Loading Parameters
cur_path = os.path.dirname(__file__)
with open(os.path.join(cur_path, '../params/param.yaml'), 'r') as f:
param = yaml.load(f)
globalMapName = param["map"]["globalMapName"]
latitudeAll = ()
longitudeAll = ()
GPS_all = ((0, 0, 0), ) #GPS_all = ()#在此处定义，取所有txt文件数据
GPSfile = open(os.path.join(cur_path, ''.join(['../map/', globalMapName])),
"r")
# GPSfile = open(os.path.join(cur_path,'../map/M30_dynamic_qianjin_20171103.txt'), "r")
while True:
line = GPSfile.readline()
if line:
line = line.strip()
if line.startswith('$GPGGA'):
GPGGA = line.split(',')
latitude = float(GPGGA[2])
longitude = float(GPGGA[4])
altitude = float(GPGGA[9])
# statusTemp1 = GPGGA[12]
# statusTemp2 = statusTemp1.split('*')
# ins_status = statusTemp2[0]
route1_x, route1_y = gps_2_xy(longitude, latitude)
GPS_all += ((route1_x, route1_y, altitude), )
latitudeAll += (latitude, )
longitudeAll += (longitude, )
else:
continue
else:
break
x1 = GPS_all[0][0] #GPS起点x,y换算值
y1 = GPS_all[0][1]
x2 = GPS_all[-1][0] #GPS终点x,y换算值
y2 = GPS_all[-1][1]
# return x1,y1
# result = np.array([x1,y1,x2,y2])
return x1, y1, x2, y2
def M30_pub():
global xLast,yLast,flag
rate = rospy.Rate(50) # hz
# cmd = bytes([2, 0, 100, 13, 0, 0, 0, 3 ,0, 1, 0, 7, 4, 6, 0, 1, 0, 135, 3])
#ser.write(cmd)
while not rospy.is_shutdown():
try:
line = ser.readline()
except :
ser.close()
line = 'no info'
if line:
line = line.strip()
if line.startswith('$GPGGA'):
GPG = line.split(',')
msg = INSPVAX()
try:
latitude = float(GPG[2])
longitude = float(GPG[4])
msg.latitude = int(latitude/100)+(latitude - int(latitude/100)*100.)/60.
msg.longitude = int(longitude/100)+(longitude - int(longitude/100)*100.)/60.
msg.x, msg.y = gps_2_xy(msg.longitude, msg.latitude)
msg.header.stamp = rospy.Time.now()
print msg.header.stamp
dist = np.sqrt((msg.x-xLast)**2 + (msg.y-yLast)**2)
# print dist
if dist>5. and flag==1:
pass
else:
flag = 1
xLast = msg.x
yLast = msg.y
GPGGA_pub.publish(msg)
print(latitude,longitude,msg.latitude,msg.longitude )
except Exception:
ser.close()
print(line)
rate.sleep()
ser.close()
def findMapEndPoint(numAllLane,distOffsetStep=1.5,flagPlot=0):
# Loading Parameters
cur_path = os.path.dirname(__file__)
with open(os.path.join(cur_path,'../params/param.yaml'),'r') as f:
param = yaml.load(f)
globalMapName = param["map"]["globalMapName"]
simu = param["map"]["simu"]
GPS_all = ()
GPSfile = open(os.path.join(cur_path,''.join(['../map/',globalMapName])), "r")
xLast = 0
yLast = 0
flag = 0
while True:
line = GPSfile.readline()
if line:
line = line.strip()
if vehicle_model.name == 'A60':
if line.startswith('%INSPVASA'):
try:
INSPV = line.split(',')
latitude = float(INSPV[4])
longitude = float(INSPV[5])
altitude = float(INSPV[6])
latitude_deg = latitude
longitude_deg = longitude
x_offset, y_offset = gps_2_xy(longitude_deg, latitude_deg) #x指向正北 #y指向正东
dist = np.sqrt((x_offset-xLast)**2 + (y_offset-yLast)**2)
if flag == 0:
flag = 1
xLast = x_offset
yLast = y_offset
else:
if dist > 5. : # 剔除异常数据
pass
elif dist > 0.2 : # 间隔0.2m取一个点
xLast = x_offset
yLast = y_offset
GPS_all += ((x_offset,y_offset),)
else:
pass
except Exception:
# print(line)
pass
else:
continue
else:
if line.startswith('$GPGGA'):
GPGGA = line.split(',')
try:
latitude = float(GPGGA[2])
longitude = float(GPGGA[4])
# altitude = float(GPGGA[9])
altitude = 0.
latitude_deg=int(latitude/100)+(latitude - int(latitude/100)*100)/60.
longitude_deg=int(longitude/100)+(longitude - int(longitude/100)*100)/60.
x_offset, y_offset = gps_2_xy(longitude_deg, latitude_deg) #x指向正北 #y指向正东
dist = np.sqrt((x_offset-xLast)**2 + (y_offset-yLast)**2)
if dist>5. and flag==1:
pass
else:
flag = 1
xLast = x_offset
yLast = y_offset
GPS_all += ((x_offset,y_offset),)
except Exception:
# print(line)
pass
else:
continue
else:
break
GPSfile.close()
if simu == True:
GPS_all = np.load(os.path.join(cur_path,'../log/cal/map_gen/simuMap.npy')) # 用仿真生成的覆盖实际地图
GPS_all_arr = np.array(GPS_all)
numGPSallPoint = len(GPS_all)
np.save(os.path.join(cur_path,'../map/rawMap.npy'),GPS_all_arr)
start_x=GPS_all[0][0] #GPS起点x,y换算值 1229
start_y=GPS_all[0][1]
end_x=GPS_all[-1][0] #GPS终点x,y换算值
end_y=GPS_all[-1][1]
c_line = np.polyfit([start_x,end_x],[start_y,end_y],1)
x_line = np.linspace(start_x,end_x,len(GPS_all))
y_line = np.polyval(c_line,x_line)
GPS_all_line = np.column_stack((x_line,y_line))
if flagPlot == 1:
# 绘制原始点
# plt.figure(1)
plt.plot(GPS_all_arr[:,0],GPS_all_arr[:,1],'*') #绘图时，将x y互换位置以匹配地图视角
# 绘制原始直线和起止点
plt.plot(GPS_all_line[:,0],GPS_all_line[:,1],'-') #绘图时，将x y互换位置以匹配地图视角
plt.plot(start_x,start_y,'ro')
plt.plot(end_x,end_y,'bo')
#np.save(os.path.join(cur_path,'../map/xueyuanLine.npy'),GPS_all_line)
startPoint = np.array([start_x,start_y])
endPoint = np.array([end_x,end_y])
distOffset = 0
mapEndPoint = []
mapEndPoint.append([startPoint,endPoint])
for i in xrange(numAllLane-1):
distOffset += distOffsetStep
startPointNew,endPointNew = findOffsetLinePoint(startPoint,endPoint,distOffset) # findOffsetLinePoint(startPoint,endPoint,distOffset)
mapEndPoint.append([startPointNew,endPointNew])
if flagPlot == 1:
# 绘制偏移直线和起止点
GPS_all_lineOffset = genMapPoint(startPointNew,endPointNew,numGPSallPoint)
plt.plot(GPS_all_lineOffset[:,0],GPS_all_lineOffset[:,1],'c-') #绘图时，将x y互换位置以匹配地图视角
plt.plot(startPointNew[0],startPointNew[1],'ro')
plt.plot(endPointNew[0],endPointNew[1],'bo')
#print (startPointNew[0]-start_x)*(end_x-start_x)+(startPointNew[1]-start_y)*(end_y-start_y)
#np.save(os.path.join(cur_path,'../map/xueyuanLineOffset.npy'),GPS_all_lineOffset)
# numGPSallPoint = (endPointNew[0]-startPointNew[0])/len(GPS_all)
return mapEndPoint, numGPSallPoint
def Callback(msg1,msg2):
latitude = msg1.latitude
longitude = msg1.longitude
azimuth = msg2.azimuth
desire_route = Route()
# current_x, current_y = gps_2_xy(longitude, latitude, 3380400.6653839252, 245529.56042564771 ) # 按需求更改初始位置
current_x, current_y = gps_2_xy(longitude, latitude, x_ori, y_ori ) # 按需求更改初始位置
desire_route.current_x = current_x
desire_route.current_y = current_y
#
current_x = (current_x+3700)*100
current_y = (current_y+3700)*100
current_x = int(current_x)
current_y = int(current_y)
azimuth *= 100
azimuth = int(azimuth)
data = (c_char*8)()
data[0] = chr((current_x & 0x0F0000) >> 16)
data[1] = chr((current_x & 0x00FF00) >> 8)
data[2] = chr(current_x & 0x0000FF)
data[3] = chr((current_y & 0x0F0000) >> 16)
data[4] = chr((current_y & 0x00FF00) >> 8)
data[5] = chr(current_y & 0x0000FF)
data[6] = chr((azimuth & 0x00FF00) >> 8)
data[7] = chr(azimuth & 0x0000FF)
libtest.trans_can_msg(fd, data, data_type["PC1"])
# print("trans ok")
# route1_x, route1_y = gnss_route_xy()
route1_x,route1_y,route2_x,route2_y = gnss_route_xy()
desire_route.endpoint_x = route2_x
desire_route.endpoint_y = route2_y
desire_route.startpoint_x = route1_x
desire_route.startpoint_y = route1_y
route1_x = (route1_x+3700)*100
route1_y = (route1_y+3700)*100
route1_x = int (route1_x)
route1_y = int (route1_y)
data = (c_char*8)()
data[0] = chr((route1_x & 0x0F0000) >> 16)
data[1] = chr((route1_x & 0x00FF00) >> 8)
data[2] = chr(route1_x & 0x0000FF)
data[3] = chr((route1_y & 0x0F0000) >> 16)
data[4] = chr((route1_y & 0x00FF00) >> 8)
data[5] = chr(route1_y & 0x0000FF)
data[6] = chr((0 & 0x00FF00) >> 8)
data[7] = chr(0 & 0x0000FF)
# for d in data:
# print hex(ord(d))
libtest.trans_can_msg(fd, data, data_type["PC2"])
# print("trans2 ok")
# route2_x, route2_y = gnss_route_xy()
route2_x = (route2_x+3700)*100
route2_y = (route2_y+3700)*100
route2_x = int (route2_x)
route2_y = int (route2_y)
data = (c_char*8)()
data[0] = chr((route2_x & 0x0F0000) >> 16)
data[1] = chr((route2_x & 0x00FF00) >> 8)
data[2] = chr(route2_x & 0x0000FF)
data[3] = chr((route2_y & 0x0F0000) >> 16)
data[4] = chr((route2_y & 0x00FF00) >> 8)
data[5] = chr(route2_y & 0x0000FF)
data[6] = chr((0 & 0x00FF00) >> 8)
data[7] = chr(0 & 0x0000FF)
pub_route.publish(desire_route)
# for d in data:
# print hex(ord(d))
libtest.trans_can_msg(fd, data, data_type["PC3"])