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