diff --git a/utils.py b/utils.py index ff29e67..a1ba527 100644 --- a/utils.py +++ b/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}