ros_contorl/scripts/lib/rdk_helpers.py

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