Initial import of ORISYS arm workstation
This commit is contained in:
cheng
@@ -0,0 +1,261 @@
|
||||
#!/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 "（空）")
|
||||
)
|
||||
Reference in New Issue
Block a user