add debug mode

utils.py

@@ -3,6 +3,9 @@
 import socket
 import json
 
+DEBUG: bool = True
+
+
 class RobotClient:
     def __init__(self, host, port):
         self.host = host
@@ -11,9 +14,14 @@ class RobotClient:
         self.sock.connect((self.host, self.port))
 
     def _send(self, cmd) -> dict:
+        if (DEBUG):
+            print(f"DEBUG: sended: {cmd}")
         self.sock.sendall(json.dumps(cmd).encode("utf-8"))
         data = self.sock.recv(1024).decode("utf-8")
-        return json.loads(data)
+        ret = json.loads(data)
+        if (DEBUG):
+            print(f"DEBUG: received: {ret}")
+        return ret
 
     def rpy_to_quat(self, arr):
         cmd = {"command": "rpy_to_quaternion", "rpy": arr}