Improve reconnection and error handling in HAL

little_red_rover/little_red_rover/hal.py

@@ -22,12 +22,18 @@ def __init__(self):
 
         self.decode_error_count = 0
 
-        threading.Thread(target=self.run_loop).start()
+        threading.Thread(target=self.run_loop, daemon=True).start()
 
     def run_loop(self):
         while not rospy.is_shutdown():
             try:
-                data = self.connection.recv_packet()
+                data = None
+                while data == None:
+                    try:
+                        data = self.connection.recv_packet()
+                    except Exception as e:
+                        print(e)
+
                 packet = messages.NetworkPacket()
                 packet.ParseFromString(bytes(data))
 
@@ -39,7 +45,7 @@ def run_loop(self):
                 if self.decode_error_count > 20:
                     print(f"Failed to decode {self.decode_error_count} packets.")
             except Exception as e:
-                print(f"Error: {e}")
+                print(f"HAL: Error - {e}")
 
 
 def main(_=None):

little_red_rover/little_red_rover/rover_connection.py

@@ -1,37 +1,50 @@
 import struct
 import socket
+import typing
+import time
 
 
 class RoverConnection:
     def __init__(self, endpoint):
         self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
         self.socket.settimeout(5.0)
         self.endpoint = endpoint
-        self.socket.connect(endpoint)
+        try:
+            self.socket.connect(self.endpoint)
+        except Exception:
+            pass
 
-    def recv_packet(self) -> bytes:
+    def recv_packet(self) -> typing.Union[bytes, None]:
         """
         Packets are prefixed with the byte string LRR, followed by the message length in bytes.
         """
 
         data = None
-        while data == None:
-            try:
-                while self.socket.recv(3, socket.MSG_PEEK) != b"LRR":
-                    self.socket.recv(1)
+        try:
+            while self.socket.recv(3, socket.MSG_PEEK) != b"LRR":
+                self.socket.recv(1)
 
-                assert self.recv_length(3) == b"LRR"
-                length = struct.unpack("H", self.recv_length(2))[0]
-                data = self.recv_length(length)
-            except Exception as e:
-                print(f"Rover connection hit error: {e}. Reconnecting...")
+            assert self.recv_length(3) == b"LRR"
+            length = struct.unpack("H", self.recv_length(2))[0]
+            data = self.recv_length(length)
+        except Exception as e:
+            print(f"Rover connection hit an error: {e}. Reconnecting...")
+            self.reconnect()
+
+        return data
+
+    def reconnect(self):
+        while True:
+            try:
                 self.socket.close()
                 self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                 self.socket.settimeout(5.0)
                 self.socket.connect(self.endpoint)
-                print(f"Error: {e}")
-
-        return data
+                print("Reconnected!")
+                return
+            except Exception as e:
+                time.sleep(1.0)
+                print(f"Error while reconnecting: {e}. Trying again in 1 second...")
 
     def recv_length(self, length) -> bytes:
         data = bytearray()
@@ -42,6 +55,8 @@ def recv_length(self, length) -> bytes:
 
     def send(self, msg: bytes):
         try:
+            self.socket.sendall(b"LRR")
+            self.socket.sendall(len(msg).to_bytes(2, byteorder="little"))
             self.socket.sendall(msg)
         except OSError as e:
             if e.errno == 9: