從多個串行端口實時讀取延遲（GPS，通過 RS232 - USB 適配器）

Question

目前，我正在通過各種接口開發具有多個傳感器的 ROS 系統。 兩個最重要的傳感器是 GPS 和陀螺儀，它們通過 RS232-USB 適配器連接。

我想強調的是，我對串行連接並不熟悉或不熟悉。

我使用 python 讀取串行數據並將它們發布到 ROS-topic 並通過 Matlab Simulink 訂閱它們。

我一直在同時使用一個 GPS 和陀螺儀，完全沒有問題（每個制造商都使用了不同制造商的適配器，比方說，我有 2 個 RS232-USB 適配器制造商；TA 和 TB）。

由於某些目的，我決定安裝一個額外的 GPS（因此同時使用了 2 個 GPS 和一個陀螺儀）。 假設我把那 2 個 GPS 放得很近； 小於1m。

這兩個 GPS 具有相同的串行端口設置（相同的波特率、相同的奇偶校驗、相同的停止位、相同的更新頻率等），並且我為兩個 GPS 使用了相同的 RS232-USB 適配器 TA 類型。

但是，訂閱 Matlab Simulink 時出現了明顯的延遲（我使用了模擬數據檢查/記錄器）。 其中一台 GPS 遲到了！ 我的意思是說一個 GPS 沒有實時更新。

例如，在 t(time)=3 時，兩個 GPS 都應該在 p(position)=10。 但是一個GPS在另一個GPS之后40~60秒后達到p=10； GPS 是正確的（時間與位置是實時的）。 下面我附上一張圖。

那兩條曲線應該不會那么遠

我不知道是什么原因導致了這個問題，陀螺儀根本沒有延遲問題，似乎只有那兩個 GPS 在互相競爭發送數據。

編輯 (12/11/2019 12:34 GMT+9)

下面是我的一個 GPS 代碼，而對於另一個 GPS，我只是創建一個新的 .py，並使用完全相同的代碼，但具有不同的節點名稱和 rostopic。

#!/usr/bin/env python

import serial
import types
import re
import rospy
import time
import threading

from datetime import datetime
from shipcon.msg import gps_position
from shipcon.msg import gps_local_position
from shipcon.msg import gps_navinfo
from shipcon.msg import gps_time


#Constants-----------------------------------------------------------------------------------------
PUBLISH_RATE = 10 #Hz
SERIAL_PORT = "/dev/ttyUSB1"
BAUD_RATE = 115200

#Class-----------------------------------------------------------------------------------------
class GpsSerial:
    def __init__(self, serial_port, baud_rate):
        self.gpsArr = []
        self.gpsStr = ""
        self.pos = gps_position()
        self.localpos = gps_local_position()
        self.navinfo = gps_navinfo()
        self.time = gps_time()
        self.ser = serial.Serial(
            port = serial_port,
            baudrate = baud_rate,
            parity = serial.PARITY_NONE,
            bytesize = serial.EIGHTBITS,
            stopbits = serial.STOPBITS_ONE,
            timeout = None,
            xonxoff = 0, #Enable software flow control
            rtscts = 0, #Enable Hardware(RTS,CTS) flow control
            )

    def get_gps(self):
        self.gpsStr = self.ser.readline()
        self.gpsArr = self.gpsStr.split(',')

        if re.match(r"^\$.*", self.gpsArr[0]):

            if self.gpsArr[0] == '$PTNL':
                if self.gpsArr[1] == 'GGK':
                    self.pos.latitude_deg = int(float(self.gpsArr[4]) // 100)
                    self.pos.latitude_min = float(self.gpsArr[4])  - (self.pos.latitude_deg * 100)
                    self.pos.latitude_dir = self.gpsArr[5]
                    self.pos.longitude_deg = int(float(self.gpsArr[6]) // 100)
                    self.pos.longitude_min = float(self.gpsArr[6])  - (self.pos.longitude_deg * 100)
                    self.pos.longitude_dir = self.gpsArr[7]
                    self.pos.gps_quality = int(self.gpsArr[8])
                    self.pos.satellite_number = int(self.gpsArr[9])
                elif self.gpsArr[1] == 'VGK':
                    if self.gpsArr[4] =='-' or self.gpsArr[4] == '':
                        pass
                    else:
                        self.localpos.localpos_e = float(self.gpsArr[4])#float( re.sub(r'\+', "", self.gpsArr[4]) )
                    if self.gpsArr[5] =='-' or self.gpsArr[5] == '':
                        pass
                    else:
                        self.localpos.localpos_n = float(self.gpsArr[5])#float( re.sub(r'\+', "", self.gpsArr[5]) )
                    if self.gpsArr[6] =='-' or self.gpsArr[6] == '':
                        pass
                    else:
                        self.localpos.localpos_z = float(self.gpsArr[6])#float( re.sub(r'\+', "", self.gpsArr[6]) )
                elif self.gpsArr[1] == 'VHD':
                    self.localpos.local_azimath = float(self.gpsArr[4])
                    self.localpos.local_vertical = float(self.gpsArr[6])
                    self.localpos.local_range = float(self.gpsArr[8])

            elif self.gpsArr[0] == '$GPGST':
                self.pos.latitude_err = float(self.gpsArr[6])
                self.pos.longitude_err = float(self.gpsArr[7])
                self.pos.ellipse_err_maj = float(self.gpsArr[3])
                self.pos.ellipse_err_min = float(self.gpsArr[4])
                self.navinfo.truenorth_heading_err = float(self.gpsArr[5])

            elif self.gpsArr[0] == '$GPROT':
                if self.gpsArr[1] =='-' or self.gpsArr[1] == '':
                    pass
                else:
                    self.navinfo.rotation_rate = float(self.gpsArr[1])

            elif self.gpsArr[0] == '$GPVTG':
                if self.gpsArr[1] =='' or self.gpsArr[5]=='' or self.gpsArr[7]=='':
                    pass
                else:
                    self.navinfo.truenorth_heading = float(self.gpsArr[1])
                    self.navinfo.speed_knot = float(self.gpsArr[5])
                    self.navinfo.speed_km = float(self.gpsArr[7])

            elif self.gpsArr[0] == '$GPZDA':
                tmptime = datetime.strptime(self.gpsArr[1]+','+self.gpsArr[4]+self.gpsArr[3]+self.gpsArr[2], '%H%M%S.%f,%Y%m%d')
                self.time.day = tmptime.day
                self.time.month = tmptime.month
                self.time.year = tmptime.year
                self.time.hour = tmptime.hour
                self.time.min = tmptime.minute
                self.time.sec = tmptime.second

        elif self.gpsArr[0] == '$GPGGA':
                self.pos.latitude_deg = int(float(self.gpsArr[2]) // 100)
                self.pos.latitude_min = float(self.gpsArr[2])  - (self.pos.latitude_deg * 100)
                self.pos.latitude_dir = self.gpsArr[3]
                self.pos.longitude_deg = int(float(self.gpsArr[4]) // 100)
                self.pos.longitude_min = float(self.gpsArr[4])  - (self.pos.longitude_deg * 100)
                self.pos.longitude_dir = self.gpsArr[5]
                #self.pos.gps_quality = int(self.gpsArr[8])
                #self.pos.satellite_number = int(self.gpsArr[7]) 

class RosPublish:
    def __init__(self):
        self.pub_pos = rospy.Publisher('gps_position', gps_position, queue_size = 1)
        self.pub_localpos = rospy.Publisher('gps_localposition', gps_local_position, queue_size = 1)
        self.pub_navinfo = rospy.Publisher('gps_navinfo', gps_navinfo, queue_size = 1)
        self.pub_time = rospy.Publisher('gps_time', gps_time, queue_size = 1)

    def ros_log(self, pos, localpos, navinfo, time):
        rospy.loginfo(pos)
        rospy.loginfo(localpos)
        rospy.loginfo(navinfo)
        rospy.loginfo(time)

    def ros_pub(self, pos, localpos, navinfo, time):
        self.pub_pos.publish(pos)
        self.pub_localpos.publish(localpos)
        self.pub_navinfo.publish(navinfo)
        self.pub_time.publish(time)        



#Function-----------------------------------------------------------------------------------------
def publish_gps(gps):
    rospy.init_node('NODE_Gps', anonymous = True)
    ros = rospy.Rate(PUBLISH_RATE)
    pub = RosPublish()

    pub.ros_log(gps.pos, gps.localpos, gps.navinfo, gps.time)
    pub.ros_pub(gps.pos, gps.localpos, gps.navinfo, gps.time)
    ros.sleep()


def refresh_gps(gps):
    while not rospy.is_shutdown():
        gps.get_gps()



#Main-----------------------------------------------------------------------------------------
if __name__ == '__main__':
    try:
        gps = GpsSerial(SERIAL_PORT, BAUD_RATE)
        thread_1 = threading.Thread(target = refresh_gps, args = (gps,))
        thread_1.start()

        while not rospy.is_shutdown():
            publish_gps(gps)

    except rospy.ROSInterruptException:
        pass

我使用 Matlab 訂閱和記錄來自傳感器的所有數據。 目前，我在 Matlab 中所做的唯一計算只是將 GPS 長/緯度數據轉換為 XY 坐標，以獲得我船只的實時軌跡。 這種轉換是一個非常簡單的計算，我認為不會造成嚴重的延遲。

如果您能給我解決方案，甚至是見解，將不勝感激。 非常感謝。

溫馨問候

Answer 1

我沒有足夠的代表發表評論，所以這不是一個真正的答案。 您設置 Python 節點的方式存在一些問題。 它可能看起來有效，但它確實效率很低。 可能還有更多問題，但應解決以下問題：

1. 將rospy.init_node('NODE_Gps', anonymous = True)到你的 '__main__'。 目前，您每次發布消息時都會重新初始化節點。 這具有相當大的開銷，並且僅在節點啟動時執行一次。
2. 與第 1 點非常相似，將所有self.pub_... = rospy.Publisher(...)行移動到您的“__main__”。 創建發布者只需要完成一次，實際上我會擺脫整個“RosPublish 類”。 如果你想將 ROS 相關的東西和你的 GPS 處理代碼分開，那么把它們變成一個函數並從 get_gps() 函數中調用它。 如果你想保留你的 RosPublish 類，那么將 publish_gps(gps) 的前三行移動到 RosPublish 類的初始化部分，並在你的主目錄中創建 RosPublish 對象。

不管你決定重構你的節點，記住 rospy.init_node 和 rospy.Publisher 行應該只被調用一次。

關於代碼的更多說明：

1. 您正在創建一個線程以不斷從串行端口讀取一行。 從pyserial git 的這個問題來看，似乎可能會導致 CPU 使用率出現一些問題。 再加上初始化 ROS 節點和發布者的不必要開銷，很可能會消耗大量資源，尤其是當您運行其中兩個時。 當您運行節點時，您的 CPU 使用率是多少？ 您可以將行的閱讀與發布率聯系起來，僅在您要發布消息時才要求新行。 和/或以不占用大量資源的方式實現串行讀取。
2. publish_gps(gps) 函數中變量的命名ros = rospy.Rate(PUBLISH_RATE) ，出於某種原因確實讓我感到困擾。 變量的名稱與其所代表的內容完全沒有關系，而且非常具有誤導性。
3. 您構建代碼的方式通常看起來非常隨意。 為什么是“類”類和“函數”函數。 我認為，這只是我個人的意見，你可以在沒有任何類的情況下完成整個事情，而只是做一件特定事情的函數。 我認為你可以讓它看起來更好，更容易理解。