diff --git a/utils.py b/utils.py index 1fe0f46..1029d6e 100644 --- a/utils.py +++ b/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,30 @@ class RobotClient: # 4. Выполняем линейное перемещение self.move_line(joint_target) + self.wait_until_stopped() - self.get_robot_state() + + def wait_until_stopped(self, timeout: float = 10.0, poll_interval: float = 0.3): + """ + Ожидает, пока робот не перейдёт в состояние Stopped (0). + + :param timeout: максимальное время ожидания в секундах + :param poll_interval: интервал опроса в секундах + :raises TimeoutError: если робот не остановился за указанное время + """ + start = time.time() + while time.time() - start < timeout: + try: + status = self.get_robot_state() + if DEBUG: + print(f"{CYAN}DEBUG: robot status:{RESET} {status}") + if status == RobotStatus.Stopped: + return + except Exception as e: + if DEBUG: + print(f"{YELLOW}DEBUG: failed to get robot status:{RESET} {e}") + time.sleep(poll_interval) + raise TimeoutError("Ожидание состояния 'Stopped' превысило таймаут") def add_waypoint(self, joint_radian: list): cmd = {"command": "add_waypoint", "joint_radian": joint_radian}