colorized debug

utils.py

@@ -4,6 +4,9 @@ import socket
 import json
 
 DEBUG: bool = True
+RESET = "\033[0m"
+CYAN = "\033[36m"
+YELLOW = "\033[33m"
 
 
 class RobotClient:
@@ -14,13 +17,13 @@ class RobotClient:
         self.sock.connect((self.host, self.port))
 
     def _send(self, cmd) -> dict:
-        if (DEBUG):
-            print(f"DEBUG: sended: {cmd}")
+        if DEBUG:
+            print(f"{CYAN}DEBUG: sended:{RESET} {cmd}")
         self.sock.sendall(json.dumps(cmd).encode("utf-8"))
         data = self.sock.recv(1024).decode("utf-8")
         ret = json.loads(data)
-        if (DEBUG):
-            print(f"DEBUG: received: {ret}")
+        if DEBUG:
+            print(f"{YELLOW}DEBUG: received:{RESET} {ret}")
         return ret
 
     def rpy_to_quat(self, arr):