Arduino 读取GPS 数据发送解析并发布ROS topic(二)

Arduino 读取GPS 数据发送解析并发布ROS topic(一) https://blog.csdn.net/Fourier_Legend/article/details/84107494

概述

本部分将主要讲将串口接受到的数据，进行处理并发布成 ros topic

思路

1. 从串口读到的数据如下

   $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 也将传送,非差分定位,此项为空)

2. 主要思路是根据 逗号的位置将一个个信息分割开， 得到经度，纬度，GPS状态， 海拔等信息。

3. 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