add multi json parsing
This commit is contained in:
parent
9374384e74
commit
36a9889a1c
1 changed files with 54 additions and 94 deletions
148
utils.py
148
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}")
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue