cheng
262 lines
8.3 KiB
Python
262 lines
8.3 KiB
Python
#!/usr/bin/env python3
|
|
"""RDK 操作公共函数（必须在 rdk_py310 / Python 3.10 下运行）。
|
|
|
|
背景与约束
|
|
----------
|
|
非夕的 `flexivrdk` Python 绑定与 ROS 环境的 Python 3.11 ABI 不兼容，所以所有
|
|
直接调用 RDK 的代码必须以子进程方式跑在 `/home/cc/rdk_py310/bin/python` 下。
|
|
因此本文件不能用任何 3.11+ 语法，也不 import ROS。
|
|
|
|
抽取原因
|
|
--------
|
|
`ensure_operational` / `ensure_idle` / tool 相关函数原先在
|
|
`scripts/rdk_switch_tool.py` 与 `scripts/rdk_drag_bridge.py` 里各写了一份，
|
|
行为已经出现细微差异（一个用 ServoOn 一个用 Enable，超时时间不同）。
|
|
统一到本文件后，两处调用方行为一致。
|
|
|
|
重要限制
|
|
--------
|
|
RDK 对同一台机器人是**独占访问**：同时有两个进程连着同一个 SN 会失败或行为异常。
|
|
所以框架里所有 RDK 操作都应当经由 `rdk_drag_bridge.py` 这一个长驻进程串行化，
|
|
不要让业务节点各自起 RDK 连接。
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import os
|
|
import time
|
|
from typing import Any, Dict, List, Optional
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# 使能与故障
|
|
# ---------------------------------------------------------------------------
|
|
|
|
def ensure_operational(robot: Any, timeout_s: float = 10.0) -> Optional[str]:
|
|
"""确保机器人处于 operational（已使能、无故障）。
|
|
|
|
返回 ``None`` 表示成功，否则返回给用户看的错误说明。
|
|
|
|
这是幂等的：已经 operational 时不做任何事。
|
|
"""
|
|
try:
|
|
if robot.fault():
|
|
robot.ClearFault()
|
|
time.sleep(0.5)
|
|
if not robot.operational():
|
|
# 不同 RDK 版本一个叫 ServoOn 一个叫 Enable。
|
|
for fn in ("ServoOn", "Enable"):
|
|
if hasattr(robot, fn):
|
|
try:
|
|
getattr(robot, fn)()
|
|
break
|
|
except Exception:
|
|
pass
|
|
deadline = time.monotonic() + timeout_s
|
|
while not robot.operational() and time.monotonic() < deadline:
|
|
time.sleep(0.2)
|
|
if not robot.operational():
|
|
return (
|
|
"机器人未处于 operational 状态（故障 / 急停未复位？）。"
|
|
"请在示教器上清故障、松开急停，并确认处于 AUTO(Remote)。"
|
|
)
|
|
except Exception as exc: # noqa: BLE001
|
|
return str(exc)
|
|
return None
|
|
|
|
|
|
def clear_fault(robot: Any) -> Optional[str]:
|
|
"""清除故障。返回 ``None`` 表示成功。"""
|
|
try:
|
|
robot.ClearFault()
|
|
time.sleep(0.5)
|
|
except Exception as exc: # noqa: BLE001
|
|
return str(exc)
|
|
return None
|
|
|
|
|
|
def ensure_idle(robot: Any, mode: Any, timeout_s: float = 10.0) -> Optional[str]:
|
|
"""确保机器人切到 IDLE 模式（工具切换等操作的前提）。
|
|
|
|
返回 ``None`` 表示成功，否则返回错误说明。
|
|
"""
|
|
try:
|
|
robot.SwitchMode(mode.IDLE)
|
|
return None
|
|
except Exception:
|
|
pass
|
|
|
|
err = ensure_operational(robot, timeout_s=timeout_s)
|
|
if err:
|
|
return err
|
|
try:
|
|
robot.SwitchMode(mode.IDLE)
|
|
except Exception as exc: # noqa: BLE001
|
|
return f"无法切到 IDLE: {exc}"
|
|
return None
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# 工具坐标系
|
|
# ---------------------------------------------------------------------------
|
|
|
|
def single_arm_groups(robot: Any) -> List[Any]:
|
|
"""返回工具 API 的关节组列表。
|
|
|
|
RDK 1.9 的 Tool API 是单臂的（没有 JointGroup 概念），此时返回 ``[None]``
|
|
作为哨兵，调用方据此走不带 group 参数的分支。老版本返回实际组名列表。
|
|
"""
|
|
try:
|
|
info = robot.info()
|
|
groups = getattr(info, "single_arm_groups", None) or {}
|
|
keys = list(groups.keys()) if isinstance(groups, dict) else list(groups)
|
|
if keys:
|
|
return keys
|
|
except Exception:
|
|
pass
|
|
try:
|
|
import flexivrdk
|
|
|
|
if hasattr(flexivrdk, "JointGroup"):
|
|
return [flexivrdk.JointGroup.ARM_1]
|
|
except Exception:
|
|
pass
|
|
return [None]
|
|
|
|
|
|
def tool_name(tool: Any, group: Any) -> str:
|
|
"""读当前工具名。"""
|
|
try:
|
|
return str(tool.name() if group is None else tool.name(group))
|
|
except Exception:
|
|
return ""
|
|
|
|
|
|
def tool_params(tool: Any, group: Any) -> Dict[str, Any]:
|
|
"""读当前工具参数，归一化成普通 dict。
|
|
|
|
返回 ``{"mass_kg": float, "com_m": [x,y,z], "tcp_m": [x,y,z]}``。
|
|
读不到的字段用 0 / 空列表。
|
|
"""
|
|
try:
|
|
params = tool.params() if group is None else tool.params(group)
|
|
except Exception:
|
|
return {"mass_kg": 0.0, "com_m": [], "tcp_m": []}
|
|
|
|
def _floats(value: Any) -> List[float]:
|
|
try:
|
|
return [float(v) for v in list(value)[:3]]
|
|
except Exception:
|
|
return []
|
|
|
|
return {
|
|
"mass_kg": float(getattr(params, "mass", 0.0) or 0.0),
|
|
"com_m": _floats(getattr(params, "CoM", []) or []),
|
|
"tcp_m": _floats(getattr(params, "tcp_location", []) or []),
|
|
}
|
|
|
|
|
|
def iface_ipv4s(name: str) -> List[str]:
|
|
"""网卡名 → 其上的 IPv4。FastDDS 白名单只认地址，不认 ``eth0`` / ``enx…``。"""
|
|
import fcntl
|
|
import socket
|
|
import struct
|
|
import subprocess
|
|
|
|
name = (name or "").strip()
|
|
if not name:
|
|
return []
|
|
if name.count(".") == 3:
|
|
return [name]
|
|
addrs: List[str] = []
|
|
try:
|
|
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
ifreq = struct.pack("256s", name.encode("utf-8")[:15])
|
|
addrs.append(socket.inet_ntoa(fcntl.ioctl(s.fileno(), 0x8915, ifreq)[20:24]))
|
|
s.close()
|
|
except OSError:
|
|
pass
|
|
try:
|
|
out = subprocess.check_output(
|
|
["ip", "-4", "-o", "addr", "show", "dev", name],
|
|
text=True,
|
|
stderr=subprocess.DEVNULL,
|
|
)
|
|
for line in out.splitlines():
|
|
parts = line.split()
|
|
if "inet" in parts:
|
|
addrs.append(parts[parts.index("inet") + 1].split("/")[0])
|
|
except Exception:
|
|
pass
|
|
seen = set()
|
|
uniq: List[str] = []
|
|
for a in addrs:
|
|
if a and a not in seen:
|
|
seen.add(a)
|
|
uniq.append(a)
|
|
return uniq
|
|
|
|
|
|
def rdk_whitelist_lists(iface: str = "", pc_ip: str = "") -> List[List[str]]:
|
|
"""给 ``flexivrdk.Robot(sn, listen)`` 用的白名单候选。
|
|
|
|
只放 IPv4。把 ``eth0`` 这种网卡名塞进去，FastDDS 会把真地址全部滤掉，
|
|
症状是 ping 通、RDK 报 Failed to connect。
|
|
"""
|
|
lists: List[List[str]] = []
|
|
pc = (pc_ip or os.environ.get("SITE_PC_IP") or "").strip()
|
|
if pc.count(".") == 3:
|
|
lists.append([pc])
|
|
iface = (iface or "").strip()
|
|
if iface.count(".") == 3:
|
|
lists.append([iface])
|
|
elif iface:
|
|
lan = [a for a in iface_ipv4s(iface) if a.startswith("192.168.2.")]
|
|
if lan:
|
|
lists.append(lan)
|
|
all_ips = iface_ipv4s(iface)
|
|
if all_ips and all_ips != lan:
|
|
lists.append(all_ips)
|
|
seen = set()
|
|
out: List[List[str]] = []
|
|
for item in lists:
|
|
key = tuple(item)
|
|
if key not in seen:
|
|
seen.add(key)
|
|
out.append(item)
|
|
return out
|
|
|
|
|
|
def switch_tool(tool: Any, group: Any, target: str) -> None:
|
|
"""切换工具。失败抛异常。"""
|
|
if group is None:
|
|
tool.Switch(target)
|
|
else:
|
|
tool.Switch(group, target)
|
|
|
|
|
|
def resolve_tool_name(tool: Any, want: str) -> str:
|
|
"""把用户给的模糊名字解析成 Studio 工具池里的准确名字。
|
|
|
|
大小写不敏感。找不到时抛 ``ValueError``，消息里列出全部可选项 ——
|
|
这样操作员能立刻看到拼错在哪，而不是面对一个"切换失败"。
|
|
"""
|
|
want = (want or "").strip()
|
|
if not want:
|
|
return ""
|
|
try:
|
|
names = list(tool.list() or [])
|
|
except Exception as exc: # noqa: BLE001
|
|
raise ValueError(f"无法读取 Studio 工具列表: {exc}") from exc
|
|
|
|
if want in names:
|
|
return want
|
|
lower = {str(n).lower(): str(n) for n in names}
|
|
hit = lower.get(want.lower())
|
|
if hit:
|
|
return hit
|
|
raise ValueError(
|
|
f"工具 '{want}' 不在 Studio 工具池里。可选: "
|
|
+ (", ".join(str(n) for n in names) if names else "（空）")
|
|
)
|