diff --git a/utils.py b/utils.py index a1ba527..8cfd213 100644 --- a/utils.py +++ b/utils.py @@ -3,7 +3,6 @@ import socket import json import time - import threading import queue @@ -46,8 +45,9 @@ class RobotClient: self._callback_queue = queue.Queue() self._recv_thread.start() - def _recv_loop(self): + decoder = json.JSONDecoder() + buffer = "" while True: try: data = self.sock.recv(4096).decode("utf-8") @@ -55,31 +55,57 @@ class RobotClient: 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) + buffer += data + while buffer: + try: + obj, index = decoder.raw_decode(buffer) + buffer = buffer[index:].lstrip() + if DEBUG: + print(f"{YELLOW}DEBUG: received:{RESET} {obj}") + if obj["ret"] == RobotJasonServerError.RESUTL_CALL_BACK: + self._callback_queue.put(obj) + else: + self._response_queue.put(obj) + except json.JSONDecodeError: + break except Exception as e: if DEBUG: print(f"{RED}DEBUG: Exception in recv loop: {e}{RESET}") break - def _send(self, cmd) -> dict: + def _send_single_json(self, cmd) -> dict: if DEBUG: print(f"{CYAN}DEBUG: sended:{RESET} {cmd}") self.sock.sendall(json.dumps(cmd).encode("utf-8")) + + try: + response = self._response_queue.get(timeout=5) + try: + extra = self._response_queue.get_nowait() + raise RuntimeError(f"Expected single JSON response, but got extra: {extra}") + except queue.Empty: + return response + except queue.Empty: + raise RuntimeError("Timeout waiting for single-json response") + + def _send_multi_json(self, cmd) -> list: + if DEBUG: + print(f"{CYAN}DEBUG: sended:{RESET} {cmd}") + self.sock.sendall(json.dumps(cmd).encode("utf-8")) + + results = [] + start_time = time.time() while True: try: - ret = self._response_queue.get(timeout=5) - if ret: - return ret + result = self._response_queue.get(timeout=5) + results.append(result) + if result.get("msg") == "succ" or result.get("ret") == 0: + break except queue.Empty: - raise RuntimeError("Timeout waiting for response") + if time.time() - start_time > 10: + raise RuntimeError("Timeout waiting for multi-json response") + break + return results def get_callback_event(self, timeout=0.1): try: @@ -89,138 +115,72 @@ class RobotClient: def rpy_to_quat(self, arr): cmd = {"command": "rpy_to_quaternion", "rpy": arr} - ret = self._send(cmd) + ret = self._send_single_json(cmd) return ret["ret"] def get_current_waypoint(self): cmd = {"command": "get_current_waypoint"} - ret = self._send(cmd) + ret = self._send_single_json(cmd) return ret["ret"]["joint"], ret["ret"]["pos"], ret["ret"]["ori"] def inverse_kin(self, joint_radian: list, pos: list, ori: list) -> list: - """ - Выполняет обратную кинематику, возвращает углы суставов (в радианах). - - :param joint_radian: текущая поза (6 углов в рад) - :param pos: целевая позиция [x, y, z] в метрах - :param ori: ориентация в виде кватерниона [w, x, y, z] - :return: список из 6 углов суставов (в радианах) - """ cmd = { "command": "inverse_kin", "joint_radian": joint_radian, "pos": pos, "ori": ori, } - ret = self._send(cmd) + ret = self._send_single_json(cmd) if ret["msg"] != "succ": raise RuntimeError(f"Inverse kinematics failed: {ret}. \n Was sended {cmd}") - return ret["ret"][ - "joint" - ] # возвращается как {"joint": [...], "pos": [...], "ori": [...]} + return ret["ret"]["joint"] def move_to_pose(self, pose: dict): - """ - Перемещает робота в указанную позицию и ориентацию. - - :param command: словарь с ключами "pos" и "ori" (ори — в радианах, RPY) - """ - # quat = self.rpy_to_quat(command["ori"]) - - # 2. Получаем текущее состояние робота joint_start, _, _ = self.get_current_waypoint() - - # 3. Вычисляем инверсную кинематику joint_target = self.inverse_kin(joint_start, pose["pos"], pose["ori"]) - - # 4. Выполняем линейное перемещение self.move_line(joint_target) def add_waypoint(self, joint_radian: list): cmd = {"command": "add_waypoint", "joint_radian": joint_radian} - ret = self._send(cmd) + ret = self._send_single_json(cmd) if (ret.get("msg") != "succ"): raise RuntimeError(f"add_waypoint failed: {ret}") def move_track(self, track = "CARTESIAN_MOVEP"): cmd = {"command": "move_track", "track": track} - ret = self._send(cmd) - if (ret.get("msg") != "succ"): + ret = self._send_multi_json(cmd) + if not any(r.get("msg") == "succ" for r in ret): raise RuntimeError(f"move_track failed: {ret}") def move_linear_cartesian(self, pose: dict): - """ - Выполняет линейное движение в декартовом пространстве между текущей позицией и заданной позой. - - :param pose: словарь с ключами "pos" (XYZ в метрах) и "ori" (кватернион [w, x, y, z]) - """ - # 1. Получаем текущее состояние joint_start, _, _ = self.get_current_waypoint() - - # 2. Добавляем текущую позицию как waypoint self.add_waypoint(joint_start) - - # 3. Вычисляем IK joint_target = self.inverse_kin(joint_start, pose["pos"], pose["ori"]) - - # 4. Добавляем целевую позицию как waypoint self.add_waypoint(joint_target) - - # 5. Выполняем траекторию self.move_track() def get_robot_state(self) -> int: - """ - Получает текущее состояние робота. - - :return: Одно из значений: - 0 — Stopped - 1 — Running - 2 — Paused - 3 — Resumed - :raises RuntimeError: если не удалось получить состояние - """ cmd = {"command": "get_robot_state"} - ret = self._send(cmd) + ret = self._send_single_json(cmd) if ret.get("msg") != "succ": raise RuntimeError(f"get_robot_state failed: {ret}") return ret["ret"] def move_line(self, joint_radian: list) -> None: - """ - Выполняет линейное перемещение в заданную позу, сохраняя текущую ориентацию. - - :param joint_radian: целевая поза (список из 6 значений в радианах) - :raises RuntimeError: если команда не была выполнена успешно - """ cmd = {"command": "move_line", "joint_radian": joint_radian} - ret = self._send(cmd) - if ret.get("ret") == 3004: - return - if ret.get("msg") != "succ": + ret = self._send_multi_json(cmd) + if not any(r.get("msg") == "succ" or r.get("ret") == 0 for r in ret): raise RuntimeError(f"move_line failed: {ret}") def set_joint_maxacc(self, acc: float): - """ - Устанавливает одинаковое максимальное ускорение для всех суставов (в rad/s²). - - :param acc: ускорение для всех 6 суставов - :raises RuntimeError: если команда завершилась с ошибкой - """ cmd = {"command": "set_joint_maxacc", "joint_maxacc": [acc] * 6} - ret = self._send(cmd) + ret = self._send_single_json(cmd) if ret.get("msg") != "succ": raise RuntimeError(f"set_joint_maxacc failed: {ret}") def set_joint_maxvelc(self, vel: float): - """ - Устанавливает одинаковую максимальную скорость для всех суставов (в rad/s). - - :param vel: скорость для всех 6 суставов - :raises RuntimeError: если команда завершилась с ошибкой - """ cmd = {"command": "set_joint_maxvelc", "joint_maxvelc": [vel] * 6} - ret = self._send(cmd) + ret = self._send_single_json(cmd) if ret.get("msg") != "succ": raise RuntimeError(f"set_joint_maxvelc failed: {ret}")