从多个串行端口实时读取延迟（GPS，通过 RS232 - USB 适配器）

Question

At the moment i am working on a ROS system with multiple sensors through various interfaces. Two of the most important sensors are GPS and Gyro that are connected through RS232-USB adapter.

I would like to emphasize that i am not in anyway familiar or expert in serial connections.

I use python to read the serial data and publish them to ROS-topic and subscribe them via Matlab Simulink.

I had been using one GPS and Gyro simultaneously and there were no problem at all (adapters from different manufacturer were used for each, let's say, i have 2 RS232-USB adapter manufacturers; TA and TB).

Due to some purposes, i decided to install an additional GPS (hence 2 GPS(s) and a Gyro simultaneously were used in real time). Let's say i put those 2 GPS quite close; less than 1m.

Those two GPS have identical serial port setting (same baud rate, same parity, same stopbits, same update frequency, etc.) and i used the same RS232-USB adapter TA type for both GPS.

However, there was a significant delay when subscribing in Matlab Simulink (i used simulation data inspection/logger). One of the GPS was late! What i mean by late is that one GPS did not update in real time.

For example, at t(time)=3, both GPS should be at p(position)=10. But one GPS reaches p=10 after 40~60 seconds after the other GPS; GPS which is correct (time vs position is real time). Below i attach a graph.

Those two curves should not be that far

I dont know what might have caused this problem, Gyro has no delay problem at all, it seems only those 2 GPS are racing each other to send the data.

Edit (12/11/2019 12:34 GMT+9)

Below is my code for one GPS, and for other GPS i simply create a new .py, and use exactly the same code but with different node name and 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

I used Matlab to subscribe and log all the data from the sensors. At the moment the only calculation that i am doing in Matlab is just a conversion of GPS long/lat data to XY coordinates to get the real time trajectory of my vessel. This conversion is a very simple calculation which i dont think will cause a serious delay.

If you could give me solution, or even an insight, it would be very appreciated. Thank you very much.

Warmest regards

Answer 1

I do not have enough rep to post a comment, so this is not really an answer. There are some issues with the way you have set up your python node. It might appear to be working, but it is definitely doing it quite inefficiently. There might be more issues, but the following issues should be solved:

1. move the rospy.init_node('NODE_Gps', anonymous = True) to your '__main__'. Currently you are re-initializing the node every time you publish the message. This has considerable overhead and is only meant to be done once when the node is started.
2. Very similar to point 1, move the all of the self.pub_... = rospy.Publisher(...) lines to your '__main__'. Creating the publishers is meant to be done only once, in fact I would get rid of the entire "RosPublish class". If you want some separation of ROS related things and your GPS handling code then make them a function and call it from the get_gps() function. If you want to keep your RosPublish class then move the three first lines of publish_gps(gps) to the RosPublish class init part and create the RosPublish object in your main.

However you decide to refactor your node, keep in mind that rospy.init_node and the rospy.Publisher lines should only be called once.

Some more notes on the code:

1. You are creating a thread to constantly read a line from the serial port. From this issue at pyserial git , it seems that might cause some issues with the CPU usage. Combine that with the unnecessary overhead of initializing the ROS node and publishers it could well be that you are using up a lot of your resources, especially if you run two of these. What is your CPU usage, when you run the node? You could tie the reading of the line to the publishing rate, only ask for the new line when you want to publish a message. And/or implement the serial reading in a way that is not resource heavy.
2. The naming of the variable in the publish_gps(gps) function ros = rospy.Rate(PUBLISH_RATE) , it really bothers me for some reason. The name of the variable has absolutely nothing to do with what it represents and is very misleading.
3. The way you have structured your code seems very arbitrary in general. Why are the "classes" classes and "functions" functions. I think, and it's just my personal opinion, you could do this whole thing without any classes, just functions, that do one specific thing. I think you could make it look a lot better and more understandable.