ros_contorl/scripts/check_arm_prefixes.py

#!/usr/bin/env python3
"""检查「静态配置里的关节前缀」是否与 site.yaml 当前选中的臂一致。

为什么需要这个脚本
------------------
本框架里有两类文件：

**一类跟着 site.yaml 走（本脚本负责检查）**
    ``src/arm_control/config/controllers.yaml``、``srdf/arm.srdf``、
    ``moveit_controllers.yaml``、``bringup.yaml``、``joint_names.yaml``、
    ``servo.yaml``、``xunhuan_trajectory.yaml`` ……

    这些文件里写的是**具体臂的关节名**（``Rizon4-063989_joint1``）。Flexiv 官方
    的关节命名把 SN 编在名字里，所以换一台臂，这些文件必须跟着改。

**一类按臂各留一份（本脚本只提示、不报错）**
    ``config/site.yaml``、``named_poses_89.yaml`` / ``named_poses_91.yaml``、
    ``dual_grippers.yaml``（两条臂的夹爪都在里面）、``handeye/waypoints.yaml``。

    这些是**现场资产**，本来就该保存多台臂各自的标定/示教结果。

为什么必须显式检查
------------------
换臂后如果只改了 ``site.yaml``、忘了改上面第一类文件，症状是：

* 控制器起不来，报 ``Joint 'Rizon4-063989_joint1' not found``；
* 或者更糟——URDF 里关节名变了、SRDF 里没变，MoveIt 规划出的轨迹
  全部作用在**不存在的关节**上，表现为"规划成功但机械臂不动"。

这类失败在真机上排查代价很高，所以做成一条命令：**改完 site.yaml 先跑它**。

用法
----
    python3 scripts/check_arm_prefixes.py            # 检查
    python3 scripts/check_arm_prefixes.py --list     # 只列出当前臂的应然前缀

退出码：0 = 一致；1 = 有文件对不上（会给出逐条 file:line）。
"""

from __future__ import annotations

import argparse
import re
import sys
from pathlib import Path

_HERE = Path(__file__).resolve().parent
_REPO = _HERE.parent
sys.path.insert(0, str(_HERE / "lib"))

from export_site_env import flatten, load_site  # noqa: E402  （本仓库自己的加载器）

# 必须跟着当前臂走的文件（相对仓库根）。
REQUIRED_FILES: tuple[str, ...] = (
    "src/arm_control/config/controllers.yaml",
    "src/arm_control/config/named_poses.yaml",
    "src/arm_moveit_config/config/moveit_controllers.yaml",
    "src/arm_moveit_config/srdf/arm.srdf",
    "src/arm_bringup/config/bringup.yaml",
    "src/arm_description/config/joint_names.yaml",
    "src/arm_application/config/servo.yaml",
    "src/arm_application/config/xunhuan_trajectory.yaml",
)

# 按臂各留一份的现场资产：含别的臂的 SN 是对的，不报错。
PER_ARM_FILES: tuple[str, ...] = (
    "config/site.yaml",
    "config/site.example.yaml",
    "src/arm_control/config/named_poses_89.yaml",
    "src/arm_control/config/named_poses_91.yaml",
    "src/arm_application/config/dual_grippers.yaml",
    "handeye/waypoints.yaml",
)

# Flexiv 关节名里的 SN 形态：Rizon4-063989 / Rizon4s-123456 …
#
# 注意别用 ``\b`` 收尾：关节名是 ``Rizon4-063989_joint1``，下划线属于 \w，
# 写成 ``\b`` 会**一个都匹配不到**（本脚本第一版就踩了这个坑，
# 结果 SRDF 里 24 处硬编码全部漏报）。
_SN_TOKEN = re.compile(r"Rizon\d+[A-Za-z]*-\d{6}(?!\d)")


def _active_prefix() -> str:
    """当前臂的关节前缀（``Rizon4-063989_``）。取不到就报错退出。

    这里刻意**不兜底**成任何具体前缀：读不出 site.yaml 时，
    "检查通过"是假象，宁可让调用方看到失败（R1.1）。

    三种失败都必须变成"人话 + 退出码 2"，不能让 Python traceback 直接糊到脸上
    —— 这个脚本是换臂流程里的第 3 步，用户往往正处在"配置改到一半"的状态：

    * ``site.yaml`` 不存在（``load_site`` 抛 ``FileNotFoundError``）；
    * ``robot.sn`` 为空、或 ``robots[]`` 里查不到（``flatten`` 抛 ``ValueError``）。
    """
    try:
        data = load_site()
    except FileNotFoundError as exc:
        print(
            f"读不到 site 配置：{exc}\n"
            "没有当前臂就无从检查。先确认 config/site.yaml 存在，"
            "或让 SITE_CONFIG 指向它。",
            file=sys.stderr,
        )
        raise SystemExit(2) from exc

    try:
        flat = flatten(data)
    except ValueError as exc:
        # 典型就是"robot.sn 没填 / 别名对不上"——把原始信息原样带出来，
        # 它已经写明了现有哪几台臂可选。
        print(
            f"解析 site 配置失败，取不到当前臂：\n{exc}",
            file=sys.stderr,
        )
        raise SystemExit(2) from exc

    prefix = str(flat.get("SITE_JOINT_PREFIX", "") or "").strip()
    if not prefix or prefix == "_":
        print(
            "site.yaml 里没解析出关节前缀（SITE_JOINT_PREFIX 为空）。\n"
            "请确认 robot.robots[].sn 已填，且该臂就是 robot.sn 选中的那一台。",
            file=sys.stderr,
        )
        raise SystemExit(2)
    return prefix


def _scan(path: Path, want_sn: str) -> list[str]:
    """返回该文件里所有"不属于 want_sn"的 SN 出现处，格式 ``行号: 内容``。"""
    hits: list[str] = []
    try:
        text = path.read_text(encoding="utf-8")
    except OSError as exc:  # 文件缺失也值得报出来
        return [f"读不到文件：{exc}"]
    for lineno, line in enumerate(text.splitlines(), start=1):
        for token in _SN_TOKEN.findall(line):
            if token != want_sn:
                hits.append(f"{lineno}: {line.strip()}")
                break
    return hits


def main() -> int:
    parser = argparse.ArgumentParser(description=__doc__.splitlines()[0])
    parser.add_argument(
        "--list",
        action="store_true",
        help="只打印当前臂的应然前缀与 SN，不做检查",
    )
    args = parser.parse_args()

    prefix = _active_prefix()
    want_sn = prefix.rstrip("_")

    if args.list:
        print(f"当前臂 SN    : {want_sn}")
        print(f"关节前缀     : {prefix}")
        # 说明这个 SN 是哪来的：环境变量 ROBOT_SN 会**覆盖** site.yaml 的
        # robot.sn（为了让"当前终端选哪台臂"能临时换），排查时容易看错。
        import os

        env_sn = os.environ.get("ROBOT_SN", "").strip()
        if env_sn:
            print(f"来源         : 环境变量 ROBOT_SN={env_sn}（覆盖 site.yaml 的 robot.sn）")
        else:
            print("来源         : config/site.yaml 的 robot.sn")
        return 0

    problems: dict[str, list[str]] = {}
    for rel in REQUIRED_FILES:
        path = _REPO / rel
        hits = _scan(path, want_sn)
        if hits:
            problems[rel] = hits

    if not problems:
        print(f"OK：{len(REQUIRED_FILES)} 个跟随臂走的一级配置文件都指向 {want_sn}。")
        return 0

    print(
        f"发现 {len(problems)} 个配置文件与当前臂 {want_sn} 不一致：\n",
        file=sys.stderr,
    )
    for rel, hits in problems.items():
        print(f"  {rel}", file=sys.stderr)
        for hit in hits[:8]:
            print(f"      {hit}", file=sys.stderr)
        if len(hits) > 8:
            print(f"      …… 另有 {len(hits) - 8} 处", file=sys.stderr)
        print("", file=sys.stderr)

    old_sns = sorted({tok for rel in problems for hit in problems[rel]
                      for tok in _SN_TOKEN.findall(hit)})
    print("修法（把旧 SN 文本替换成当前 SN，再重新 build）：", file=sys.stderr)
    for old in old_sns:
        if old == want_sn:
            continue
        print(f"  # {old} → {want_sn}", file=sys.stderr)
        print(
            "  grep -rl " + repr(old) + " " + " ".join(REQUIRED_FILES)
            + f" | xargs sed -i 's/{old}/{want_sn}/g'",
            file=sys.stderr,
        )
    print(
        "\n注意：SRDF / controllers 里的关节名是**结构**，替换后请跑\n"
        "  python3 scripts/check_arm_prefixes.py   # 应输出 OK\n"
        "  bash scripts/build.sh                   # 重新 build 让 install 同步",
        file=sys.stderr,
    )
    return 1


if __name__ == "__main__":
    raise SystemExit(main())