From 239d727cf4fe6b3b7a3e83a9a193a0df9f707f1c Mon Sep 17 00:00:00 2001 From: Michal Sojka Date: Sat, 1 Apr 2023 19:52:22 +0200 Subject: [PATCH] Read serial line in chunks instead of character by character MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This significantly reduces CPU load caused by this driver. Without this change, CPU load on a powerful x86 machine with a USB-connected GPS is 36%. The output of the strace command looks like this: ... pselect6(35, [33 34], [], [], {tv_sec=1, tv_nsec=999999000}, NULL) = 1 (in [33], left {tv_sec=1, tv_nsec=999997160}) read(33, "$", 1) = 1 pselect6(35, [33 34], [], [], {tv_sec=1, tv_nsec=999999000}, NULL) = 1 (in [33], left {tv_sec=1, tv_nsec=999997160}) read(33, "G", 1) = 1 pselect6(35, [33 34], [], [], {tv_sec=1, tv_nsec=999999000}, NULL) = 1 (in [33], left {tv_sec=1, tv_nsec=999997160}) read(33, "P", 1) = 1 pselect6(35, [33 34], [], [], {tv_sec=1, tv_nsec=999999000}, NULL) = 1 (in [33], left {tv_sec=1, tv_nsec=999997180}) read(33, "R", 1) = 1 ... It can be seen that characters are read from the serial line one by one. This is due to how IOBase.readline() interacts with pySerial. In this commit, we implement the same functionality as IOBase.readline(), but in a way allowing to read the serial line characters in bigger chunks. With it, CPU load decreases to 7.5% and strace reports this: ... pselect6(35, [33 34], [], [], {tv_sec=1, tv_nsec=926286000}, NULL) = 1 (in [33], left {tv_sec=1, tv_nsec=906510472}) read(33, "$GPRMC,,V,,,,,,,,,,N*53\r\n$GPGGA,"..., 422) = 143 pselect6(35, [33 34], [], [], {tv_sec=1, tv_nsec=906287000}, NULL) = 1 (in [33], left {tv_sec=1, tv_nsec=886945652}) read(33, "$GPRMC,,V,,,,,,,,,,N*53\r\n$GPGGA,"..., 279) = 143 pselect6(35, [33 34], [], [], {tv_sec=1, tv_nsec=886721000}, NULL) = 1 (in [33], left {tv_sec=1, tv_nsec=866882570}) read(33, "$GPRMC,,V,,,,,,,,,,N*53\r\n$GPGGA,"..., 136) = 136 pselect6(35, [33 34], [], [], {tv_sec=1, tv_nsec=999996000}, NULL) = 1 (in [33], left {tv_sec=1, tv_nsec=999991549}) read(33, ",,*57\r\n", 1024) = 7 pselect6(35, [33 34], [], [], {tv_sec=1, tv_nsec=999827000}, NULL) = 1 (in [33], left {tv_sec=1, tv_nsec=985949688}) read(33, "$GPRMC,,V,,,,,,,,,,N*53\r\n$GPGGA,"..., 1017) = 143 pselect6(35, [33 34], [], [], {tv_sec=1, tv_nsec=985746000}, NULL) = 1 (in [33], left {tv_sec=1, tv_nsec=966235438}) read(33, "$GPRMC,,V,,,,,,,,,,N*53\r\n$GPGGA,"..., 874) = 143 ... Reading from the kernel happens most of the time in chunks of 143 bytes (for our GPS) and therefore, the system call overhead is 143× lower. Tested on Septentrio mosaic-H GPS. --- .../nodes/nmea_serial_driver.py | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/src/libnmea_navsat_driver/nodes/nmea_serial_driver.py b/src/libnmea_navsat_driver/nodes/nmea_serial_driver.py index ce60838..294faac 100755 --- a/src/libnmea_navsat_driver/nodes/nmea_serial_driver.py +++ b/src/libnmea_navsat_driver/nodes/nmea_serial_driver.py @@ -31,6 +31,7 @@ # POSSIBILITY OF SUCH DAMAGE. import serial +import io import rclpy @@ -50,17 +51,22 @@ def main(args=None): GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) driver.get_logger().info("Successfully connected to {0} at {1}.".format(serial_port, serial_baud)) try: + data = bytearray() while rclpy.ok(): - data = GPS.readline().strip() - try: - if isinstance(data, bytes): - data = data.decode("utf-8") - driver.add_sentence(data, frame_id) - except ValueError as e: - driver.get_logger().warn( - "Value error, likely due to missing fields in the NMEA message. Error was: %s. " - "Please report this issue at github.com/ros-drivers/nmea_navsat_driver, including a bag file " - "with the NMEA sentences that caused it." % e) + data.extend(GPS.read(1024)) # read at most 1024 bytes + lines = data.splitlines(keepends=True) + # process complete lines + for line in lines[0:-1]: + line = line.decode("utf-8").rstrip() + try: + driver.add_sentence(line, frame_id) + except ValueError as e: + driver.get_logger().warn( + "Value error, likely due to missing fields in the NMEA message. Error was: %s. " + "Please report this issue at github.com/ros-drivers/nmea_navsat_driver, including a bag file " + "with the NMEA sentences that caused it." % e) + # continue with the last, incomplete line + data = lines[-1] except Exception as e: driver.get_logger().error("Ros error: {0}".format(e))