Arduino 读取GPS 数据发送解析并发布ROS topic(一) https://blog.csdn.net/Fourier_Legend/article/details/84107494
本部分将主要讲将串口接受到的数据,进行处理并发布成 ros topic
从串口读到的数据如下
$GNGA,120025.000,0000.0000,N,00000.0000,E,0,00,0.0,0.0,M,0.0,M,,0000*77
$GNGA,120026.000,0000.0000,N,00000.0000,E,0,00,0.0,0.0,M,0.0,M,,0000*74
其中“ $GNGGA 所在的行就是GPS 的信息,格式如下:
$GNGGA,(1),(2),(3),(4),(5),(6),(7),(8),(9),M,(10),M,(11),(12)*hh(CR)(LF)
(1) UTC 时间,格式为 hhmmss.ss;
(2) 纬度,格式为 ddmm.mmmmm(度分格式);
(3) 纬度半球,N 或 S(北纬或南纬);
(4) 经度,格式为 dddmm.mmmmm(度分格式);
(5) 经度半球,E 或 W(东经或西经);
(6) GPS 状态,0=未定位,1=非差分定位,2=差分定位;
(7) 正在使用的用于定位的卫星数量(00~12)
(8) HDOP 水平精确度因子(0.5~99.9)
(9) 海拔高度(-9999.9 到 9999.9 米)
(10) 大地水准面高度(-9999.9 到 9999.9 米)
(11) 差分时间(从最近一次接收到差分信号开始的秒数,非差分定位,此项为空)
(12) 差分参考基站标号(0000 到 1023,首位 0 也将传送,非差分定位,此项为空)
主要思路是根据 逗号的位置将一个个信息分割开, 得到经度,纬度,GPS状态, 海拔等信息。
ROS 需要 NavSatStatus 消息格式类型,所以接下来就是将得到的信息组装成需要的 msg类型
#-*- coding:utf-8 –*-
import rospy
import serial
import time
from std_msgs.msg import String
from sensor_msgs.msg import NavSatFix
from sensor_msgs.msg import NavSatStatus
from std_msgs.msg import Header
timestamp = ""
latitude = ""
longitude = ""
altitude = ""
status = ""
satellite_num = ""
location_accuracy = ""
gps_pub = rospy.Publisher('/gps', NavSatFix, queue_size=10) # 定义发布的信息
rospy.init_node('gps_publisher', anonymous=True)
def subInfo(gps_info, comma_count, start_ind, end_ind): # 将消息根据逗号位置提取出来
if(comma_count is 2):
global timestamp
timestamp = gps_info[start_ind+1:end_ind]
if(comma_count is 3):
global latitude
latitude = gps_info[start_ind+1:end_ind]
if(comma_count is 5):
global longitude
longitude = gps_info[start_ind+1:end_ind]
if(comma_count is 7):
global status
status = gps_info[start_ind+1:end_ind]
if(comma_count is 8):
global satellite_num
satellite_num = gps_info[start_ind+1:end_ind]
if(comma_count is 9):
global location_accuracy
location_accuracy = gps_info[start_ind+1:end_ind]
if(comma_count is 10):
global altitude
altitude = gps_info[start_ind+1:end_ind]
gpsTopicPublush(timestamp,latitude,longitude,status,
satellite_num,location_accuracy,altitude)
def subGPSstr(gps_info):
print "subGPSstr : ", gps_info
comma_count = 0
start_ind = 0
for i in range(len(gps_info)): # 判断逗号的位置
if(gps_info[i] is ','):
#print comma_count
comma_count= comma_count + 1
subInfo(gps_info, comma_count, start_ind, i)
start_ind = i
def gpsTopicPublush(timestamp,latitude,longitude,status, #将得到的信息组装成 NavSatStatus 消息类型
satellite_num,location_accuracy,altitude):
# Header
header_ins = Header( frame_id = "gps_link", stamp = rospy.Time.now() )
# NavSatStatus
if(status is '0'):
gps_status = -1;
else:
gps_status = 0
navsatstatus_ins = NavSatStatus(status = gps_status)
navstafix_ins = NavSatFix(header = header_ins,
status = navsatstatus_ins,
latitude = float(latitude),
longitude = float(longitude),
altitude = float(altitude),
)
global gps_pub
gps_pub.publish(navstafix_ins)
if __name__ == '__main__':
ser=serial.Serial("/dev/ttyACM0",38400,timeout=0.5) # 打开串口的位置
#ser.open () #打开端口
gps_str = ""
while(1):
s = ser.read(65) #从端口读65个字节
if(len(s) > 50 and "\000" not in s and "\r" not in s): # 排除不好的消息类型
subGPSstr(s)
上图左侧为代码, 右下角为串口读到的数据, 右上角为发不出来的topic信息(rostopic echo /gps)
参考资料:
Arduino 读取GPS 数据发送解析并发布ROS topic(一) https://blog.csdn.net/Fourier_Legend/article/details/84107494