Rewrite nmea_socket_driver to use serversocket

src/libnmea_navsat_driver/nodes/nmea_socket_driver.py

@@ -33,14 +33,41 @@
 """Defines the main method for the nmea_socket_driver executable."""
 
 
-import socket
+import select
 import sys
+import traceback
+
+try:
+    import socketserver
+except ImportError:
+    import SocketServer as socketserver  # Python 2.7
 
 import rospy
 
 from libnmea_navsat_driver.driver import RosNMEADriver
 
 
+class NMEAMessageHandler(socketserver.DatagramRequestHandler):
+    def handle(self):
+        for line in self.rfile:
+            line = line.strip()
+            if not line:
+                continue
+
+            try:
+                self.server.driver.add_sentence(line, self.server.frame_id)
+            except ValueError:
+                rospy.logwarn(
+                    "ValueError, likely due to missing fields in the NMEA "
+                    "message. Please report this issue at "
+                    "https://github.com/ros-drivers/nmea_navsat_driver"
+                    ", including the following:\n\n"
+                    "```\n" +
+                    repr(line) + "\n\n" +
+                    traceback.format_exc() +
+                    "```")
+
+
 def main():
     """Create and run the nmea_socket_driver ROS node.
 
@@ -49,69 +76,35 @@ def main():
     ROS parameters:
         ~ip (str): IPV4 address of the socket to open.
         ~port (int): Local port of the socket to open.
-        ~buffer_size (int): The size of the buffer for the socket, in bytes.
         ~timeout (float): The time out period for the socket, in seconds.
     """
     rospy.init_node('nmea_socket_driver')
 
     try:
         local_ip = rospy.get_param('~ip', '0.0.0.0')
         local_port = rospy.get_param('~port', 10110)
-        buffer_size = rospy.get_param('~buffer_size', 4096)
         timeout = rospy.get_param('~timeout_sec', 2)
     except KeyError as e:
         rospy.logerr("Parameter %s not found" % e)
         sys.exit(1)
 
-    frame_id = RosNMEADriver.get_frame_id()
+    # Create a socket
+    server = socketserver.UDPServer((local_ip, local_port), NMEAMessageHandler,
+                                    bind_and_activate=False)
+    server.frame_id = RosNMEADriver.get_frame_id()
+    server.driver = RosNMEADriver()
 
-    driver = RosNMEADriver()
+    # Start listening for connections
+    server.server_bind()
+    server.server_activate()
 
-    # Connection-loop: connect and keep receiving. If receiving fails,
-    # reconnect
-    while not rospy.is_shutdown():
-        try:
-            # Create a socket
-            socket_ = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
-
-            # Bind the socket to the port
-            socket_.bind((local_ip, local_port))
-
-            # Set timeout
-            socket_.settimeout(timeout)
-        except socket.error as exc:
-            rospy.logerr(
-                "Caught exception socket.error when setting up socket: %s" %
-                exc)
-            sys.exit(1)
-
-        # recv-loop: When we're connected, keep receiving stuff until that
-        # fails
+    # Handle incoming connections until ROS shuts down
+    try:
         while not rospy.is_shutdown():
-            try:
-                data, remote_address = socket_.recvfrom(buffer_size)
-
-                # strip the data
-                data_list = data.strip().split("\n")
-
-                for data in data_list:
-
-                    try:
-                        driver.add_sentence(data, frame_id)
-                    except ValueError as e:
-                        rospy.logwarn(
-                            "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)
-
-            except socket.error as exc:
-                rospy.logerr(
-                    "Caught exception socket.error during recvfrom: %s" % exc)
-                socket_.close()
-                # This will break out of the recv-loop so we start another
-                # iteration of the connection-loop
-                break
-
-        socket_.close()  # Close socket
+            rlist, _, _ = select.select([server], [], [], timeout)
+            if server in rlist:
+                server.handle_request()
+    except Exception:
+        rospy.logerr(traceback.format_exc())
+    finally:
+        server.server_close()