add wain_until_stopped method

utils.py

@@ -2,6 +2,7 @@
 
 import socket
 import json
+import time
 
 DEBUG: bool = True
 RESET = "\033[0m"
@@ -9,6 +10,14 @@ CYAN = "\033[36m"
 YELLOW = "\033[33m"
 RED = "\033[31m"
 
+
+class RobotStatus:
+    Stopped = 0
+    Running = 1
+    Paused = 2
+    Resumed = 3
+
+
 class RobotClient:
     def __init__(self, host, port):
         self.host = host
@@ -89,8 +98,24 @@ class RobotClient:
 
         # 4. Выполняем линейное перемещение
         self.move_line(joint_target)
+        self.wait_until_stopped()
 
-        self.get_robot_state()
+    def wait_until_stopped(self, timeout=10.0, poll_interval=0.2):
+        start = time.time()
+        last_status = None
+        while time.time() - start < timeout:
+            try:
+                status = self.get_robot_state()
+                if status != last_status:
+                    print(f"DEBUG: Robot status changed to {status}")
+                    last_status = status
+                if status == RobotStatus.Stopped:
+                    print("DEBUG: Robot is stopped.")
+                    return
+            except Exception as e:
+                print(f"DEBUG: Failed to get robot status: {e}")
+            time.sleep(poll_interval)
+        raise TimeoutError("Timeout: Robot did not reach 'Stopped' status within timeout")
 
     def add_waypoint(self, joint_radian: list):
         cmd = {"command": "add_waypoint", "joint_radian": joint_radian}