配置：消灭代码里的硬编码臂 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
@@ -648,10 +648,13 @@ class RotaryGripper(BaseGripper):

@dataclass
 class DualArmGripperConfig:
-    """双臂夹爪配置。"""
+    """双臂夹爪配置。
+
+    这里**没有** ``left_arm_sn`` / ``right_arm_sn``：它们曾经存在但从来没有被读过
+    （只写不读的死字段，R5.5），而且默认值是本机的 SN —— 换台臂就会误导读者。
+    哪个侧别是哪台臂，由 ``load_from_site()`` 从 site.yaml 的 ``robots[].arm`` 决定。
+    """

-    left_arm_sn: str = "Rizon4-063989"
-    right_arm_sn: str = "Rizon4-063991"
    left_gripper: ParallelGripperConfig = field(default_factory=ParallelGripperConfig)
    right_gripper: RotaryGripperConfig = field(default_factory=RotaryGripperConfig)

@@ -742,22 +745,21 @@ def load_dual_arm_gripper_config(path: str | Path) -> DualArmGripperConfig:
        auto_initialize=bool(rotary.get("auto_initialize", True)),
    )
    return DualArmGripperConfig(
-        left_arm_sn=left.get("robot_sn", "Rizon4-063989"),
-        right_arm_sn=right.get("robot_sn", "Rizon4-063991"),
        left_gripper=left_cfg,
        right_gripper=right_cfg,
    )


 def load_from_site(data: dict) -> DualArmGripperConfig:
-    """从 ``~/ros2/config/site.yaml`` 构建 ``DualArmGripperConfig``。"""
+    """从 ``~/ros2/config/site.yaml`` 构建 ``DualArmGripperConfig``。
+
+    左右靠 ``robots[].arm`` 认，**不靠 SN 里的数字**：``"063989 就是左臂"``
+    是站点数据，写进代码后换个站点就会把左右夹爪搞反
+    （症状是"右臂的旋转爪去开左臂的平行爪"，串口对不上才报错）。
+    """
    robots = list((data.get("robot") or {}).get("robots") or [])
    left = next((r for r in robots if str(r.get("arm") or "").lower() == "left"), {})
    right = next((r for r in robots if str(r.get("arm") or "").lower() == "right"), {})
-    if not left:
-        left = next((r for r in robots if "063989" in str(r.get("sn") or "")), {})
-    if not right:
-        right = next((r for r in robots if "063991" in str(r.get("sn") or "")), {})
    g = data.get("gripper") or {}
    parallel = g.get("parallel") or {}
    rotary = g.get("rotary") or {}
@@ -796,8 +798,6 @@ def load_from_site(data: dict) -> DualArmGripperConfig:
        init_timeout=float(rotary.get("init_timeout") or 15.0),
    )
    return DualArmGripperConfig(
-        left_arm_sn=str(left.get("sn") or "Rizon4-063989"),
-        right_arm_sn=str(right.get("sn") or "Rizon4-063991"),
        left_gripper=left_cfg,
        right_gripper=right_cfg,
    )
@@ -31,15 +31,52 @@ def _print_status(label: str, status) -> None:
    )


-def _normalize_arm(raw: str) -> str:
-    t = (raw or "").strip().lower()
-    if t in ("89", "left", "l", "063989"):
-        return "left"
-    if t in ("91", "right", "r", "063991"):
-        return "right"
-    if t in ("both", "dual", "all"):
+def _load_site_exporter():
+    """复用 ``scripts/lib/export_site_env.py`` —— 它才是 --arm 别名的权威定义。
+
+    这里**不自己再抄一份** ``89 -> SN`` 的映射：抄一份就多一处会漂移的地方（M2）。
+    """
+    root = Path(__file__).resolve().parents[3]
+    lib = str(root / "scripts" / "lib")
+    if lib not in sys.path:
+        sys.path.insert(0, lib)
+    import export_site_env  # type: ignore
+
+    return export_site_env
+
+
+def _normalize_arm(raw: str, data: dict) -> str:
+    """把 --arm 的写法归一成 ``left`` / ``right`` / ``both``。
+
+    别名（``89`` 这类）由 ``export_site_env`` 从 site.yaml 的 ``robots[]`` 现算
+    （臂号 = 该臂 SN 末两位）。代码里**不写死** SN，否则换台机器人后会指错臂，
+    而症状是"给右臂发命令，左臂动了"。
+    """
+    t = (raw or "").strip()
+    if t.lower() in ("both", "dual", "all"):
        return "both"
-    raise SystemExit(f"未知的 --arm {raw!r}（可用 89|91|left|right|both）")
+    site_env = _load_site_exporter()
+    sn, matched = site_env.resolve_robot_sn(data, t)
+    if not matched:
+        raise SystemExit(
+            f"未知的 --arm {raw!r}：不在 site.yaml 的 robot.robots[] 里。\n"
+            f"可用：\n{site_env.describe_arms(data)}\n"
+            f"也可以直接写 left|right|both。"
+        )
+    for row in (data.get("robot") or {}).get("robots") or []:
+        if str(row.get("sn") or "").strip() != sn:
+            continue
+        arm = str(row.get("arm") or "").strip().lower()
+        if arm in ("left", "right"):
+            return arm
+        raise SystemExit(
+            f"--arm {raw!r} 选中了 {sn}，但 site.yaml 里这台臂没写 arm: left|right，"
+            f"无法判断操作哪个夹爪。请补上这个键。"
+        )
+    raise SystemExit(
+        f"--arm {raw!r} 解析出 {sn}，但它不在 site.yaml 的 robot.robots[] 里，"
+        f"无法判断是哪个夹爪。"
+    )


 def test_parallel(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
@@ -93,7 +130,18 @@ def test_rotary(manager: DualArmGripperManager, args: argparse.Namespace) -> Non

 def main() -> int:
    parser = argparse.ArgumentParser(description="DH PGEA / RGI 命令行工具（串口来自 site.yaml）")
-    parser.add_argument("--arm", default=os.environ.get("ROBOT_ARM") or os.environ.get("ROBOT_SN") or "89")
+    parser.add_argument(
+        "--arm",
+        # 默认取当前臂（parse_arm_args.sh / load_site_config.sh 已经把它放进环境）。
+        # **不**兜底成某个具体臂号：那等于"没选臂时也能跑，跑的是哪台看运气"。
+        default=(
+            os.environ.get("SITE_ROBOT_ARM")
+            or os.environ.get("ROBOT_SN")
+            or os.environ.get("SITE_ROBOT_SN")
+            or ""
+        ),
+        help="left|right|both，或该臂的臂号（= SN 末两位，由 site.yaml 决定）",
+    )
    parser.add_argument(
        "--test",
        default="status",
@@ -114,7 +162,7 @@ def main() -> int:
        return 1
    data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
    manager = DualArmGripperManager(load_from_site(data))
-    arm = _normalize_arm(str(args.arm))
+    arm = _normalize_arm(str(args.arm), data)
    print(f"site={site} arm={arm} test={args.test}")

    try:
@@ -33,6 +33,12 @@ from arm_application.geom_ik import (
    geometric_jacobian,
    quat_xyzw_to_R,
 )
+from arm_application.site_io import (
+    base_frame_default,
+    ee_frame_default,
+    joint_prefix_default,
+)
+


 def _load_ee_cfg() -> dict:
@@ -45,9 +51,9 @@ def _load_ee_cfg() -> dict:
    robot = resolve_robot(data)
    ee_z = float(servo.get("ee_z_m") or 0.24)
    return {
-        "prefix": str(robot.get("joint_prefix") or "Rizon4-063989_"),
-        "base": str(robot.get("base_frame") or "Rizon4-063989_base_link"),
-        "ee": str(robot.get("ee_frame") or "Rizon4-063989_flange"),
+        "prefix": str(robot.get("joint_prefix") or joint_prefix_default()),
+        "base": str(robot.get("base_frame") or base_frame_default()),
+        "ee": str(robot.get("ee_frame") or ee_frame_default()),
        "z": ee_z,
        # 回 home 前先竖直升到这个 Z（避免撞到墙面/工装）；
        # 然后再回到 Flexiv 的命名初始位姿。
@@ -45,6 +45,8 @@ from arm_application.handeye_realsense import (
    query_realsense_color_intrinsics,
    resolve_realsense_topics,
 )
+from arm_application.site_io import joint_prefix_default
+
 from arm_msgs.action import FollowJointTrajectory
 from arm_msgs.srv import GetArmState

@@ -82,7 +84,7 @@ class HandeyeCalibrate(Node):
        super().__init__('handeye_calibrate')

        self.declare_parameter('mode', 'eye_in_hand')  # eye_in_hand | eye_to_hand
-        self.declare_parameter('joint_prefix', 'Rizon4-063989_')
+        self.declare_parameter('joint_prefix', joint_prefix_default())
        self.declare_parameter('joint_names', DEFAULT_JOINTS)
        self.declare_parameter('velocity_scale', 0.10)
        self.declare_parameter('action_name', 'follow_joint_trajectory')
@@ -26,7 +26,6 @@ rdk（软件 Free Drive）：
 from __future__ import annotations

 import os
-import sys
 import threading
 import time
 from datetime import datetime, timezone
@@ -249,7 +248,7 @@ class HandeyeTeach(Node):
        self.declare_parameter('height', 720)
        self.declare_parameter('fps', 15)
        self.declare_parameter('backend', 'ros')  # ros | rdk
-        self.declare_parameter('robot_sn', 'Rizon4-063989')
+        self.declare_parameter('robot_sn', os.environ.get('SITE_ROBOT_SN', '').strip())
        self.declare_parameter('drag_primitive', 'FloatingJoint')  # or FloatingCartesian
        self._serial = _param_as_str(self, 'camera_serial', '')
        self.declare_parameter('auto_exposure', True)
@@ -36,6 +36,8 @@ from arm_application.handeye_realsense import (
    is_realsense_source,
    resolve_realsense_topics,
 )
+from arm_application.site_io import joint_prefix_default
+
 from arm_msgs.action import FollowJointTrajectory
 from arm_msgs.srv import GetArmState

@@ -111,7 +113,7 @@ def _image_to_bgr(msg: Image) -> np.ndarray:
 class HandeyeWaypoints(Node):
    def __init__(self) -> None:
        super().__init__('handeye_waypoints')
-        self.declare_parameter('joint_prefix', 'Rizon4-063989_')
+        self.declare_parameter('joint_prefix', joint_prefix_default())
        self.declare_parameter('joint_names', DEFAULT_JOINTS)
        self.declare_parameter('velocity_scale', 0.12)
        self.declare_parameter('action_name', 'follow_joint_trajectory')
@@ -4,12 +4,12 @@
 from __future__ import annotations

 import time
-from typing import List

 import rclpy
 from rclpy.action import ActionClient
 from rclpy.node import Node

+from arm_application.site_io import joint_prefix_default
 from arm_msgs.action import FollowJointTrajectory

 DEFAULT_JOINTS = [
@@ -28,7 +28,7 @@ DEFAULT_POSITIONS = [
 class RunJointTrajectory(Node):
    def __init__(self) -> None:
        super().__init__('run_joint_trajectory')
-        self.declare_parameter('joint_prefix', 'Rizon4-063989_')
+        self.declare_parameter('joint_prefix', joint_prefix_default())
        self.declare_parameter('joint_names', DEFAULT_JOINTS)
        self.declare_parameter('velocity_scale', 0.15)
        self.declare_parameter('action_name', 'follow_joint_trajectory')
@@ -40,6 +40,11 @@ from arm_application.geom_ik import (
 )
 from arm_application.pid_controller import PidController
 from arm_application.kalman_filter import KalmanFilter3D
+from arm_application.site_io import (
+    base_frame_default,
+    ee_frame_default,
+    joint_prefix_default,
+)


 DEFAULT_JOINTS = [
@@ -62,10 +67,10 @@ class ServoNode(Node):
            f"servo_node 已启动 pid={os.getpid()} thread_id={tid}"
        )

-        self.declare_parameter("joint_prefix", "Rizon4-063989_")
+        self.declare_parameter("joint_prefix", joint_prefix_default())
        self.declare_parameter("joint_names", DEFAULT_JOINTS)
-        self.declare_parameter("base_frame", "Rizon4-063989_base_link")
-        self.declare_parameter("ee_frame", "Rizon4-063989_flange")
+        self.declare_parameter("base_frame", base_frame_default())
+        self.declare_parameter("ee_frame", ee_frame_default())
        self.declare_parameter("camera_frame", "d405_color_optical_frame")
        self.declare_parameter("traj_topic", "rizon_arm_controller/joint_trajectory")
        self.declare_parameter("point_topic", "visual_servo/filtered_point")
@@ -40,3 +40,43 @@ def deep_merge(base: dict[str, Any], overlay: dict[str, Any]) -> dict[str, Any]:
        else:
            out[key] = val
    return out
+
+
+def joint_prefix_default() -> str:
+    """当前臂的关节名前缀（``Rizon4-XXXXXX_``），用作 ROS 参数的默认值。
+
+    来源顺序：
+
+    1. ``SITE_JOINT_PREFIX`` —— ``scripts/env.sh`` → ``load_site_config.sh`` 从
+       site.yaml 导出，这就是**权威值**；
+    2. 由 ``SITE_ROBOT_SN`` 现算（``Rizon4-063989`` → ``Rizon4-063989_``）；
+    3. 都没有 → 空字符串。
+
+    **绝不写死某个具体 SN。** 以前这里的默认值是 ``"Rizon4-063989_"``，
+    于是换台机器人后 ``ros2 run`` 直接起的节点会去订**上一台臂**的关节话题，
+    表现为"节点起来了、什么都不动、也不报错"（R1.1）。
+    """
+    prefix = os.environ.get("SITE_JOINT_PREFIX", "").strip()
+    if prefix:
+        return prefix
+    sn = os.environ.get("SITE_ROBOT_SN", "").strip()
+    return f"{sn}_" if sn else ""
+
+
+def base_frame_default() -> str:
+    """当前臂的基座坐标系（``<SN>_base_link``），用作 ROS 参数默认值。"""
+    frame = os.environ.get("SITE_BASE_FRAME", "").strip()
+    if frame:
+        return frame
+    sn = os.environ.get("SITE_ROBOT_SN", "").strip()
+    return f"{sn}_base_link" if sn else ""
+
+
+def ee_frame_default() -> str:
+    """当前臂的法兰坐标系（``<SN>_flange``），用作 ROS 参数默认值。"""
+    frame = os.environ.get("SITE_EE_FRAME", "").strip()
+    if frame:
+        return frame
+    sn = os.environ.get("SITE_ROBOT_SN", "").strip()
+    return f"{sn}_flange" if sn else ""
+
@@ -117,9 +117,14 @@ def _launch_setup(context, *args, **kwargs):
    ee_frame = LaunchConfiguration("ee_frame").perform(context).strip()
    cam_frame = LaunchConfiguration("camera_frame").perform(context).strip()
    yolo = site.get("yolo") or {}
-    if not ee_frame or ee_frame == "Rizon4-063989_flange":
+    # "没指定"用**空串**表示，不再拿一个具体坐标系名当哨兵：
+    # 之前写的是 `ee_frame == "Rizon4-063989_flange"`，换个站点以后
+    # 这个判断永远不成立，于是 site.yaml 里的 ee_frame 再也不会被用上
+    # （症状是静态 TF 挂到了错的坐标系上，而点还是能查到）。
+    if not ee_frame:
        ee_frame = str(robot.get("ee_frame") or ee_frame)
-    if not cam_frame or cam_frame == "d405_color_optical_frame":
+    # 同 ee_frame：用空串表示"没指定"，不拿具体坐标系名当哨兵。
+    if not cam_frame:
        cam_frame = str(
            perc_over.get("camera_frame")
            or yolo.get("camera_frame")
@@ -216,17 +221,20 @@ def _launch_setup(context, *args, **kwargs):
 def generate_launch_description() -> LaunchDescription:
    return LaunchDescription(
        [
+            # 默认值从环境取（scripts/env.sh → load_site_config.sh 导出的
+            # SITE_HANDEYE_FILE / SITE_EE_FRAME 已经是**当前臂**的值）。
+            # 不写死某个臂的文件名/坐标系：写死的话换臂后这里就悄悄指向上一台。
            DeclareLaunchArgument(
                "handeye_file",
-                default_value="/home/cc/ros2/handeye/results/89_eye_in_hand.yaml",
+                default_value=os.environ.get("SITE_HANDEYE_FILE", ""),
            ),
            DeclareLaunchArgument(
                "ee_frame",
-                default_value="Rizon4-063989_flange",
+                default_value=os.environ.get("SITE_EE_FRAME", ""),
            ),
            DeclareLaunchArgument(
                "camera_frame",
-                default_value="d405_color_optical_frame",
+                default_value=os.environ.get("SITE_CAMERA_FRAME", ""),
            ),
            DeclareLaunchArgument(
                "enable_control",