add thread for callback receiving

utils.py

@@ -4,6 +4,9 @@ import socket
 import json
 import time
 
+import threading
+import queue
+
 DEBUG: bool = True
 RESET = "\033[0m"
 CYAN = "\033[36m"
@@ -38,22 +41,51 @@ class RobotClient:
         self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
         self.sock.connect((self.host, self.port))
 
+        self._recv_thread = threading.Thread(target=self._recv_loop, daemon=True)
+        self._response_queue = queue.Queue()
+        self._callback_queue = queue.Queue()
+        self._recv_thread.start()
+
+
+    def _recv_loop(self):
+        while True:
+            try:
+                data = self.sock.recv(4096).decode("utf-8")
+                if not data:
+                    if DEBUG:
+                        print("DEBUG: socket closed by server")
+                    break
+                ret = json.loads(data)
+                if DEBUG:
+                    print(f"{YELLOW}DEBUG: received:{RESET} {ret}")
+                if ret["ret"] == RobotJasonServerError.RESUTL_CALL_BACK:
+                    self._callback_queue.put(ret)
+                    if DEBUG:
+                        print(f"{YELLOW}DEBUG: received callback event: {ret['msg']}")
+                else:
+                    self._response_queue.put(ret)
+            except Exception as e:
+                if DEBUG:
+                    print(f"{RED}DEBUG: Exception in recv loop: {e}{RESET}")
+                break
+
     def _send(self, cmd) -> dict:
         if DEBUG:
             print(f"{CYAN}DEBUG: sended:{RESET} {cmd}")
         self.sock.sendall(json.dumps(cmd).encode("utf-8"))
-        
         while True:
-            data = self.sock.recv(4096).decode("utf-8")
-            ret = json.loads(data)
-            if DEBUG:
-                print(f"{YELLOW}DEBUG: received:{RESET} {ret}")
-            if ret["ret"] == RobotJasonServerError.RESUTL_CALL_BACK:
-                if DEBUG:
-                    print(f"{YELLOW}DEBUG: received callback event (ignored as response): {ret['msg']}")
-                continue
-            else:
-                return ret
+            try:
+                ret = self._response_queue.get(timeout=5)
+                if ret:
+                    return ret
+            except queue.Empty:
+                raise RuntimeError("Timeout waiting for response")
+
+    def get_callback_event(self, timeout=0.1):
+        try:
+            return self._callback_queue.get(timeout=timeout)
+        except queue.Empty:
+            return None
 
     def rpy_to_quat(self, arr):
         cmd = {"command": "rpy_to_quaternion", "rpy": arr}