#!/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 "（空）") )