配置：消灭代码里的硬编码臂 SN，并给"换臂漏改"加一把尺子

## 为什么 "设备 ID 全部可配置"这条要求在代码里只做了一半：site.yaml 能配，但**代码和静态配置里到处躺着 Rizon4-063989 当兜底**。这类兜底的危害不是"改起来麻烦"，而是它把"配置没生效"伪装成"配置生效了"： * `robot_sn` 默认 `Rizon4-063989` → 换臂后节点去订**上一台臂**的关节话题，节点起来了、不报错、什么都不动； * launch 里用 `ee_frame == "Rizon4-063989_flange"` 当"用户没指定"的哨兵 → 换个站点这个判断永远不成立，于是 site.yaml 里的 ee_frame 再也不会被用上，静态 TF 挂错坐标系、点还照样查得到； * `if SN == "Rizon4-063989"` 决定要不要拉触觉服务 → 换臂后触觉静默消失。 ## 改了什么 1. **删掉所有"兜底成某台臂"的默认值**（15 处脚本 + 6 个 launch + 6 个节点）。没配就为空 / 直接 `:?` 报错，宁可起不来也不要连错机器： - `scripts/lib/export_site_env.py`：别名解析改为从 `robots[]` 现算（`arm_short_id` / `resolve_robot_sn` / `describe_arms`），选不出臂时抛 `ValueError`；`main()` 捕获后打人话 + 退出码 2（供 shell `eval`）； - 手眼文件/路点/工作目录由当前臂 short id 派生，不再写死 `89_*`； - `ROBOT_HAS_TACTILE` 改为读 site.yaml 的 `robots[].tactile`； - `arm_application/site_io.py` 新增 `joint_prefix_default()` / `base_frame_default()` / `ee_frame_default()`：优先读 `SITE_*`，退而由 `SITE_ROBOT_SN` 现算，**不写死 SN**； - `arm.urdf.xacro` 的 `robot_sn` 默认值改成明显的占位符 `UNSET_ARM_SN` （关节名会带出来，报错信息自解释）； - `ros2_control.launch.py` / `move_group.launch.py` **原先根本不传 robot_sn**，靠 xacro 写死默认值"蒙对"本机，现在都从环境取。 2. **新增 `scripts/check_arm_prefixes.py`**：Flexiv 把 SN 编进关节名，所以有一批一级配置（SRDF/controllers/joint_names/bringup/servo/...）必须跟着当前臂走。漏改的症状是"MoveIt 规划成功但机械臂不动"，很难查。这个脚本逐文件逐行列出对不上的地方，并直接给出修法。 `--list` 还会说明 SN 来自 site.yaml 还是被 `ROBOT_SN` 覆盖。 3. **文档**：`FRAMEWORK_RULES.md` 新增 R1.1.1（两类含臂名文件怎么分、换臂四步）+ 检查清单第 12 条 + 反例 2 条；`HANDBOOK.md` 新增 3.6.1 与症状表 2 行。 4. **顺带修掉的真 bug**（都是这次改动带出来、被 pyflakes + 真跑抓住的）： - `convert_flexiv_traj.py` / `rdk_drag_bridge.py` 用了 `os` 但没 import → `--help` 直接 NameError； - `check_arm_prefixes.py` 第一版正则用 `\b` 收尾，而关节名是 `Rizon4-063989_joint1`（`_` 属于 \w，`\b` 不成立）→ SRDF 里 24 处硬编码**一处都没报**，还打印 OK。已加负向测试钉住； - 删掉若干死代码/死 import（`pos_line`/`flat`/`names`、`IfCondition` 等）。 ## 验收（都真跑过） - `python3 -m pyflakes <全部改过的 py>` → 干净 - 逐包 pytest：233 + 69 + 78 + 24 + 236 = **640 passed** - `env -u ROBOT_SN python3 scripts/check_arm_prefixes.py` → OK（8 文件） - 换成 SN=Rizon4-777001 的临时 site.yaml → 报出**8 个**文件不一致并给修法（退出码 1） - 故意不给 SITE_* → 各 launch 的 `robot_sn` / `ee_frame` / `handeye_file` 默认值为 `''`，不再悄悄指向上一台臂 - `ros2 launch ... --show-args` 对 stack/sim/ros2_control/mock_control/ move_group/display/visual_servo **全部通过** - `convert_flexiv_traj.py --help` 正常；拿 `xunhuan.traj` 真转 5 个路点，输出 `joint_prefix: "Rizon4-063989_"` 来自 site.yaml Co-authored-by: Cursor <cursoragent@cursor.com>
2026-09-24 09:18:44 +08:00
parent fac2d5384d
commit f5ed44a987
40 changed files with 1187 additions and 150 deletions
@@ -62,23 +62,121 @@ def list_robots(data: dict) -> list[dict]:
    return out


-# --arm 89|91|left|right 与 SN 别名。禁止把 89 的手眼/相机套到 91。
-_ARM_ALIASES = {
-    "89": "Rizon4-063989",
-    "left": "Rizon4-063989",
-    "左": "Rizon4-063989",
-    "左臂": "Rizon4-063989",
-    "左臂89": "Rizon4-063989",
-    "91": "Rizon4-063991",
-    "right": "Rizon4-063991",
-    "右": "Rizon4-063991",
-    "右臂": "Rizon4-063991",
-    "右臂91": "Rizon4-063991",
+# 侧别词 —— 这是**语言别名**，不是站点数据：任何站点都希望
+# `--arm left` / `--arm 左` / `--arm 左臂` 能选中 `robots[]` 里 `arm: left` 的那台。
+#
+# 这里**绝不允许**出现具体 SN（`Rizon4-063989` 之类）。历史上这里是一张
+# `89 -> Rizon4-063989` 的写死映射表：换一台机器人后 `--arm 89` 仍然会选中
+# 上一台，而且不报错（R1.1）。现在 `89` 这个写法的来源是
+# **这台机器 SN 的末两位**，由 `_arm_index()` 从 `robots[]` 现算出来。
+_SIDE_WORDS = {
+    "left": "left",
+    "l": "left",
+    "左": "left",
+    "左臂": "left",
+    "right": "right",
+    "r": "right",
+    "右": "right",
+    "右臂": "right",
 }

 _ARM_SECTION_KEYS = ("camera", "gripper", "paths", "servo", "handeye", "go_home", "yolo")


+def arm_short_id(sn: str) -> str:
+    """Flexiv 序列号的"臂号"= 末两位数字（``Rizon4-063989`` → ``89``）。
+
+    手眼文件名、结果文件都用这个编号。它是从 SN 派生出来的，不是常量。
+    """
+    tail = str(sn or "").strip().rsplit("-", 1)[-1]
+    return tail[-2:] if tail.isdigit() and len(tail) >= 2 else ""
+
+
+def _arm_index(data: dict) -> dict[str, str]:
+    """从 ``robots[]`` 派生 ``别名 -> SN`` 索引。
+
+    别名包括：完整 SN、``label``（如 ``左臂89``）、侧别词（``left``/``左``）、
+    SN 末两位（``89``）。全部来自 site.yaml —— 这是"换一台机器人只改配置"的前提。
+    """
+    index: dict[str, str] = {}
+    for item in list_robots(data):
+        sn = str(item.get("sn") or "").strip()
+        if not sn:
+            continue
+        names = {sn.lower()}
+        label = str(item.get("label") or "").strip()
+        if label:
+            names.add(label.lower())
+        arm = str(item.get("arm") or "").strip().lower()
+        if arm:
+            names.add(arm)
+            names.update(word for word, side in _SIDE_WORDS.items() if side == arm)
+        short = arm_short_id(sn)
+        if short:
+            names.add(short)
+            if arm:
+                names.add(f"{arm}{short}")
+        for name in names:
+            if name:
+                index.setdefault(name, sn)
+    return index
+
+
+def _looks_like_full_sn(token: str) -> bool:
+    """区分"完整 SN"与"臂别名"。
+
+    ``Rizon4-063989`` 这种带连字符的写法允许不在 ``robots[]`` 里
+    （单臂站点可以只写 ``robot.sn`` 不登记清单）；而 ``89`` / ``left`` 这类
+    **别名**查不到就必须报错，不能当成 SN 硬用。
+    """
+    return "-" in token
+
+
+def normalize_robot_sn(data: dict, want: str) -> str:
+    """把 89 / left / 左臂 / label / 完整 SN 归一成完整 SN（查不到则原样返回）。"""
+    return resolve_robot_sn(data, want)[0]
+
+
+def resolve_robot_sn(data: dict, want: str) -> tuple[str, bool]:
+    """归一化并报告是否命中。
+
+    返回 ``(sn, matched)``。``matched=False`` 表示这个写法在 ``robots[]`` 里
+    查不到 —— 调用方必须**报错**（见 ``resolve_robot``），不能静默拿它当 SN，
+    否则会去连一台不存在的机器人。
+    """
+    w = (want or "").strip()
+    if not w:
+        return "", False
+    index = _arm_index(data)
+    hit = index.get(w.lower())
+    if hit:
+        return hit, True
+    if _looks_like_full_sn(w):
+        return w, True
+    return w, False
+
+
+def describe_arms(data: dict) -> str:
+    """给报错信息用：列出本配置里可选的全部机械臂。"""
+    lines = []
+    for item in list_robots(data):
+        sn = str(item.get("sn") or "").strip()
+        if not sn:
+            continue
+        bits = [f"  - {sn}"]
+        arm = str(item.get("arm") or "").strip()
+        label = str(item.get("label") or "").strip()
+        if arm or label:
+            bits.append(f"arm={arm or '?'}")
+        if label:
+            bits.append(f"label={label}")
+        short = arm_short_id(sn)
+        if short:
+            bits.append(f"--arm {short}")
+        lines.append("  ".join(bits))
+    return "\n".join(lines) or "  （robot.robots[] 是空的）"
+
+
 def _deep_merge(base: dict, overlay: dict) -> dict:
    out = dict(base)
    for k, v in overlay.items():
@@ -89,33 +187,39 @@ def _deep_merge(base: dict, overlay: dict) -> dict:
    return out


-def normalize_robot_sn(data: dict, want: str) -> str:
-    """把 89/91/left/right/label 映射成完整的 Flexiv SN。"""
-    w = (want or "").strip()
-    if not w:
-        return w
-    mapped = _ARM_ALIASES.get(w) or _ARM_ALIASES.get(w.lower())
-    if mapped:
-        return mapped
-    for item in list_robots(data):
-        sn = str(item.get("sn") or "").strip()
-        label = str(item.get("label") or "").strip()
-        arm = str(item.get("arm") or "").strip().lower()
-        if w == sn or w == label or w.lower() == arm:
-            return sn
-        if sn.endswith(w) and w.isdigit() and len(w) >= 2:
-            return sn
-    return w
-
-
 def resolve_robot(data: dict, sn: str | None = None) -> dict:
-    """当前生效的机械臂：环境变量 ``ROBOT_SN`` 覆盖 robot.sn；清单补齐 ip/iface/frames。"""
+    """当前生效的机械臂：环境变量 ``ROBOT_SN`` 覆盖 robot.sn；清单补齐 ip/iface/frames。
+
+    选不出来时**抛 ``ValueError``**，绝不猜：
+
+    - 给了别名（``89`` / ``left``）但 ``robots[]`` 里没有对应的臂；
+    - ``robot.sn`` 没写、也没给 ``--arm``。
+
+    以前这两种情况会静默落回一个写死的 SN 或一个空的 ``sn``，结果就是
+    "配置漏了一键，程序照样启动，连的是哪台看运气"。
+    """
    robot = dict(data.get("robot") or {})
    catalog = list_robots(data)
-    want = normalize_robot_sn(
-        data,
-        sn or os.environ.get("ROBOT_SN") or os.environ.get("ROBOT_ARM") or robot.get("sn") or "",
+    raw = (
+        sn
+        or os.environ.get("ROBOT_SN")
+        or os.environ.get("ROBOT_ARM")
+        or robot.get("sn")
+        or ""
    )
+    want, matched = resolve_robot_sn(data, raw)
+    if str(raw).strip() and not matched:
+        raise ValueError(
+            f"选不出机械臂：{str(raw).strip()!r} 既不是 robot.robots[] 里的别名，"
+            f"也不像完整 SN（完整 SN 形如 Rizon4-063989）。\n"
+            f"config/site.yaml 里现有：\n{describe_arms(data)}\n"
+            f"用法：所有终端加同一个 --arm，或直接写完整 SN。"
+        )
+    if not want:
+        raise ValueError(
+            "config/site.yaml 没有指定机械臂：robot.sn 为空，也没给 --arm。\n"
+            f"可选：\n{describe_arms(data)}"
+        )
    match: dict = {}
    for item in catalog:
        if str(item.get("sn") or "").strip() == want:
@@ -124,16 +228,13 @@ def resolve_robot(data: dict, sn: str | None = None) -> dict:
    out = {k: v for k, v in robot.items() if k != "robots"}
    out.update({k: v for k, v in match.items() if k != "robots"})
    out["sn"] = want
-    if not str(out.get("arm") or "").strip():
-        if "063989" in want:
-            out["arm"] = "left"
-        elif "063991" in want:
-            out["arm"] = "right"
-    if not str(out.get("joint_prefix") or "").strip() and want:
+    # 侧别只认 robots[].arm。**不要**从 SN 里猜数字（"063989 就是左臂"）——
+    # 那是站点数据写进了代码，换个站点就会把左右搞反。
+    if not str(out.get("joint_prefix") or "").strip():
        out["joint_prefix"] = f"{want}_"
-    if not str(out.get("base_frame") or "").strip() and want:
+    if not str(out.get("base_frame") or "").strip():
        out["base_frame"] = f"{want}_base_link"
-    if not str(out.get("ee_frame") or "").strip() and want:
+    if not str(out.get("ee_frame") or "").strip():
        out["ee_frame"] = f"{want}_flange"
    net = data.get("network") or {}
    if not str(out.get("iface") or "").strip():
@@ -200,10 +301,19 @@ def flatten(data: dict) -> dict:
    except Exception:
        weights_resolved = weights

+    # 派生文件名的口径：臂号 = 当前 SN 末两位（见 arm_short_id）。
+    # 用当前臂现算，**不要**钉死 "89" —— 否则换成 91 或另一台臂时，
+    # 两边会共用同一个手眼文件，表现是"标定做了但不生效"。
+    short = arm_short_id(str(robot.get("sn") or ""))
+    he_mode = str(he.get("mode") or "eye_in_hand")
+    he_stem = f"{short}_{he_mode}" if short else he_mode
+
    out = {
        "SITE_CONFIG": str(site_path()),
        "SITE_ROS2_ROOT": str(_root()),
-        "SITE_ROBOT_SN": str(robot.get("sn") or "Rizon4-063989"),
+        # 必须来自配置。缺了就留空让调用方报错 —— 兜底成某个真实 SN
+        # 等于"配置漏了照样启动，连的是哪台看运气"。
+        "SITE_ROBOT_SN": str(robot.get("sn") or ""),
        "SITE_ROBOT_IP": str(robot.get("ip") or "192.168.100.1"),
        "SITE_PC_IP": str(robot.get("pc_ip") or "192.168.100.10"),
        "SITE_JOINT_PREFIX": str(robot.get("joint_prefix") or ""),
@@ -212,6 +322,10 @@ def flatten(data: dict) -> dict:
        "SITE_ROBOT_TOOL_NAME": str(robot.get("tool_name") or ""),
        "SITE_ROBOT_LABEL": str(robot.get("label") or ""),
        "SITE_ROBOT_ARM": str(robot.get("arm") or ""),
+        "SITE_ROBOT_SHORT_ID": short,
+        # 本臂有没有装 Orisys 触觉。以前脚本靠 `SN == Rizon4-063989` 来判断，
+        # 于是"哪台臂有传感器"变成了代码里的常量。现在由 site.yaml 说了算。
+        "SITE_ROBOT_HAS_TACTILE": "1" if robot.get("tactile") else "0",
        "SITE_NAMED_POSES_FILE": named_poses,
        "SITE_ROBOT_CATALOG": ",".join(
            str(r.get("sn") or "") for r in list_robots(data) if r.get("sn")
@@ -220,7 +334,7 @@ def flatten(data: dict) -> dict:
        "SITE_YOLO_WEIGHTS_RESOLVED": weights_resolved,
        "SITE_HANDEYE_WAYPOINTS": str(
            paths.get("handeye_waypoints")
-            or str(_root() / "handeye" / "poses" / "89_eye_in_hand.yaml")
+            or str(_root() / "handeye" / "poses" / f"{he_stem}.yaml")
        ),
        "SITE_HANDEYE_CALIBRATE": str(
            paths.get("handeye_calibrate")
@@ -231,13 +345,14 @@ def flatten(data: dict) -> dict:
            or str(_root() / "handeye" / "waypoints.yaml")
        ),
        "SITE_HANDEYE_WORKDIR": str(
-            paths.get("handeye_workdir") or str(_root() / "handeye" / "tmp")
+            paths.get("handeye_workdir")
+            or str(_root() / "handeye" / "tmp" / short)
        ),
        "SITE_HANDEYE_FILE": str(
            paths.get("handeye_file")
-            or str(_root() / "handeye" / "results" / "89_eye_in_hand.yaml")
+            or str(_root() / "handeye" / "results" / f"{he_stem}.yaml")
        ),
-        "SITE_HANDEYE_MODE": str(he.get("mode") or "eye_in_hand"),
+        "SITE_HANDEYE_MODE": he_mode,
        # 板尺寸默认值必须与 site.yaml / 结果文件头里的实际板子一致。
        # 这里原先是 11/16/0.011（一张早期用过、现已不存在的板），与 site.yaml 的
        # 8/11/0.015 冲突：一旦 site.yaml 缺这几个键就会静默用错尺寸，
@@ -278,8 +393,10 @@ def flatten(data: dict) -> dict:
        "SITE_D435_WIDTH": str(int(((data.get("cameras") or {}).get("d435") or {}).get("width") or 1280)),
        "SITE_D435_HEIGHT": str(int(((data.get("cameras") or {}).get("d435") or {}).get("height") or 720)),
        "SITE_D435_FPS": str(int(((data.get("cameras") or {}).get("d435") or {}).get("fps") or 30)),
-        "SITE_D405_LEFT_SERIAL": str(((data.get("cameras") or {}).get("d405_left") or {}).get("serial") or "352122274408"),
-        "SITE_D405_RIGHT_SERIAL": str(((data.get("cameras") or {}).get("d405_right") or {}).get("serial") or "260522274442"),
+        # 序列号**不给默认值**：兜底成某个真实序列号 = 换站点后悄悄打开别人的相机。
+        # 缺了就让相机节点以"序列号为空"报错，比连错设备好得多（M1）。
+        "SITE_D405_LEFT_SERIAL": str(((data.get("cameras") or {}).get("d405_left") or {}).get("serial") or ""),
+        "SITE_D405_RIGHT_SERIAL": str(((data.get("cameras") or {}).get("d405_right") or {}).get("serial") or ""),
        "SITE_YOLO_CONF": str(yolo.get("conf") or 0.10),
        "SITE_YOLO_IMGSZ": str(int(yolo.get("imgsz") or 640)),
        "SITE_YOLO_DEVICE": str(yolo.get("device") or "0"),
@@ -369,14 +486,10 @@ def perception_overrides(data: dict) -> dict:
    cam_ns, cam_frame, depth_topic = camera_frames(data)
    obj = str(yolo.get("object_frame") or "").strip()
    if not obj or obj == "tracked_object":
-        arm = str((_robot or {}).get("arm") or "")
-        sn = str((_robot or {}).get("sn") or "")
-        if "063991" in sn or arm == "right":
-            obj = "tracked_object_91"
-        elif "063989" in sn or arm == "left":
-            obj = "tracked_object_89"
-        else:
-            obj = "tracked_object"
+        # 跟踪目标的坐标系按**臂号**派生（SN 末两位），两侧互不干扰。
+        # 以前是从 SN 里找 "063989"/"063991" 子串，换个站点就失去意义了。
+        short = arm_short_id(str((_robot or {}).get("sn") or ""))
+        obj = f"tracked_object_{short}" if short else "tracked_object"
    return {
        "weights": str(paths.get("yolo_weights") or ""),
        "conf": float(yolo.get("conf") or 0.10),
@@ -466,7 +579,14 @@ def main() -> int:
        os.environ["ROBOT_SN"] = str(args.sn)
    path = Path(args.config).expanduser() if args.config else site_path()
    data = load_site(path)
-    flat = flatten(data)
+    try:
+        flat = flatten(data)
+    except ValueError as exc:
+        # 调用方是 `eval "$(python3 export_site_env.py)"`，抛栈对使用者毫无意义，
+        # 而且 eval 拿到空字符串后 SITE_* 全是空的，真正失败的地方会离这里很远。
+        # 所以这里必须打印一句人能看懂的原因并明确非零退出。
+        print(f"错误：{exc}", file=sys.stderr)
+        return 2
    if args.json:
        print(json.dumps({"site": data, "env": flat}, ensure_ascii=False, indent=2))
        return 0
@@ -535,19 +655,27 @@ def main() -> int:
            if isinstance(he_data, dict) and (
                he_data.get("uncalibrated") or "T_4x4" not in he_data
            ):
+                arm_hint = flat.get("SITE_ROBOT_SHORT_ID") or flat.get("SITE_ROBOT_ARM") or "?"
                print(
                    f"  警告：{he_path.name} 没有针对该机械臂标定 "
-                    f"(run_handeye.sh teach/calibrate --arm {flat.get('SITE_ROBOT_ARM') or '91'})",
+                    f"(run_handeye.sh teach/calibrate --arm {arm_hint})",
                    file=sys.stderr,
                )
-        if "063991" in flat["SITE_ROBOT_SN"] and flat.get("SITE_CAM_SERIAL") == flat.get("SITE_D405_LEFT_SERIAL"):
+        # 右臂不该用左腕 D405 的序列号 —— 判据是**侧别**，不是 SN 里的数字。
+        # （两台臂的相机装反了不会报错，只会"跟踪一直差几厘米"。）
+        if (
+            flat.get("SITE_ROBOT_ARM") == "right"
+            and flat.get("SITE_CAM_SERIAL")
+            and flat.get("SITE_CAM_SERIAL") == flat.get("SITE_D405_LEFT_SERIAL")
+        ):
            print(
-                "  警告：91 用的是左腕 D405 的序列号 — 请检查 robots[].camera.serial",
+                "  警告：右臂用的是左腕 D405 的序列号 — 请检查 robots[].camera.serial",
                file=sys.stderr,
            )
        if "RGI" in str(flat.get("SITE_GRIP_MODEL") or ""):
+            arm_hint = flat.get("SITE_ROBOT_SHORT_ID") or flat.get("SITE_ROBOT_ARM") or "?"
            print(
-                "  提示：本臂是 RGI。CLI: bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open|rotate",
+                f"  提示：本臂是 RGI。CLI: bash ~/ros2/scripts/run_gripper.sh --arm {arm_hint} --cli open|rotate",
                file=sys.stderr,
            )
        w = Path(flat["SITE_YOLO_WEIGHTS"]).expanduser()