克隆后还缺仿真安装树和两边的编译目录。把 build、build_sim、install_sim 一并放进仓库；仿真入口不再写死本机 conda，build 里指向源码的链接改成相对路径。

Co-authored-by: Cursor <cursoragent@cursor.com>

install_sim/arm_sim_gazebo/lib/arm_sim_gazebo/gazebo_sim_launcher

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm_sim_gazebo==0.1.0','console_scripts','gazebo_sim_launcher'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm_sim_gazebo==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm_sim_gazebo==0.1.0', 'console_scripts', 'gazebo_sim_launcher')())

install_sim/arm_sim_gazebo/lib/arm_sim_gazebo/sim_camera_adapter

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm_sim_gazebo==0.1.0','console_scripts','sim_camera_adapter'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm_sim_gazebo==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm_sim_gazebo==0.1.0', 'console_scripts', 'sim_camera_adapter')())

install_sim/arm_sim_gazebo/lib/arm_sim_gazebo/sim_gripper_adapter

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm_sim_gazebo==0.1.0','console_scripts','sim_gripper_adapter'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm_sim_gazebo==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm_sim_gazebo==0.1.0', 'console_scripts', 'sim_gripper_adapter')())

install_sim/arm_sim_gazebo/lib/arm_sim_gazebo/sim_home_service

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm_sim_gazebo==0.1.0','console_scripts','sim_home_service'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm_sim_gazebo==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm_sim_gazebo==0.1.0', 'console_scripts', 'sim_home_service')())

install_sim/arm_sim_gazebo/lib/arm_sim_gazebo/sim_tactile_adapter

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm_sim_gazebo==0.1.0','console_scripts','sim_tactile_adapter'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm_sim_gazebo==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm_sim_gazebo==0.1.0', 'console_scripts', 'sim_tactile_adapter')())

install_sim/arm_sim_gazebo/lib/arm_sim_gazebo/sim_truth_perception

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm_sim_gazebo==0.1.0','console_scripts','sim_truth_perception'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm_sim_gazebo==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm_sim_gazebo==0.1.0', 'console_scripts', 'sim_truth_perception')())

install_sim/arm_sim_gazebo/lib/arm_sim_gazebo/urdf_normalize

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm_sim_gazebo==0.1.0','console_scripts','urdf_normalize'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm_sim_gazebo==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm_sim_gazebo==0.1.0', 'console_scripts', 'urdf_normalize')())

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo-0.1.0-py3.11.egg-info/PKG-INFO

@@ -0,0 +1,14 @@
+Metadata-Version: 2.4
+Name: arm_sim_gazebo
+Version: 0.1.0
+Summary: Gazebo 仿真后端：描述归一化 + 启动 Gazebo 与 ros2_control 桥接
+Maintainer: arm_dev
+Maintainer-email: dev@example.com
+License: Apache-2.0
+Requires-Dist: setuptools
+Requires-Dist: PyYAML
+Dynamic: license
+Dynamic: maintainer
+Dynamic: maintainer-email
+Dynamic: requires-dist
+Dynamic: summary

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo-0.1.0-py3.11.egg-info/SOURCES.txt

@@ -0,0 +1,46 @@
+package.xml
+setup.cfg
+setup.py
+../../build_sim/arm_sim_gazebo/arm_sim_gazebo.egg-info/PKG-INFO
+../../build_sim/arm_sim_gazebo/arm_sim_gazebo.egg-info/SOURCES.txt
+../../build_sim/arm_sim_gazebo/arm_sim_gazebo.egg-info/dependency_links.txt
+../../build_sim/arm_sim_gazebo/arm_sim_gazebo.egg-info/entry_points.txt
+../../build_sim/arm_sim_gazebo/arm_sim_gazebo.egg-info/requires.txt
+../../build_sim/arm_sim_gazebo/arm_sim_gazebo.egg-info/top_level.txt
+../../build_sim/arm_sim_gazebo/arm_sim_gazebo.egg-info/zip-safe
+arm_sim_gazebo/__init__.py
+arm_sim_gazebo/gazebo_sim_launcher.py
+arm_sim_gazebo/robot_description.py
+arm_sim_gazebo/sim_camera_adapter.py
+arm_sim_gazebo/sim_cameras.py
+arm_sim_gazebo/sim_grasp_lock.py
+arm_sim_gazebo/sim_gripper.py
+arm_sim_gazebo/sim_gripper_adapter.py
+arm_sim_gazebo/sim_gripper_kin.py
+arm_sim_gazebo/sim_home_service.py
+arm_sim_gazebo/sim_live_pose.py
+arm_sim_gazebo/sim_pose_service.py
+arm_sim_gazebo/sim_scene.py
+arm_sim_gazebo/sim_site.py
+arm_sim_gazebo/sim_tactile.py
+arm_sim_gazebo/sim_tactile_adapter.py
+arm_sim_gazebo/sim_truth_perception.py
+arm_sim_gazebo/sim_wrist_ft.py
+arm_sim_gazebo/truth_holes.py
+arm_sim_gazebo/urdf_normalize.py
+launch/gazebo_sim.launch.py
+resource/arm_sim_gazebo
+test/test_check_sim_scene.py
+test/test_repo_root.py
+test/test_robot_description.py
+test/test_sim_cameras.py
+test/test_sim_grasp_lock.py
+test/test_sim_gripper.py
+test/test_sim_gripper_kin.py
+test/test_sim_pose_service.py
+test/test_sim_scene.py
+test/test_sim_site.py
+test/test_sim_tactile.py
+test/test_sim_truth_perception.py
+test/test_truth_holes.py
+test/test_urdf_normalize.py

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo-0.1.0-py3.11.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo-0.1.0-py3.11.egg-info/entry_points.txt

@@ -0,0 +1,8 @@
+[console_scripts]
+gazebo_sim_launcher = arm_sim_gazebo.gazebo_sim_launcher:main
+sim_camera_adapter = arm_sim_gazebo.sim_camera_adapter:main
+sim_gripper_adapter = arm_sim_gazebo.sim_gripper_adapter:main
+sim_home_service = arm_sim_gazebo.sim_home_service:main
+sim_tactile_adapter = arm_sim_gazebo.sim_tactile_adapter:main
+sim_truth_perception = arm_sim_gazebo.sim_truth_perception:main
+urdf_normalize = arm_sim_gazebo.urdf_normalize:main

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo-0.1.0-py3.11.egg-info/requires.txt

@@ -0,0 +1,2 @@
+setuptools
+PyYAML

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo-0.1.0-py3.11.egg-info/top_level.txt

@@ -0,0 +1 @@
+arm_sim_gazebo

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo-0.1.0-py3.11.egg-info/zip-safe

@@ -0,0 +1 @@
+

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/__init__.py

@@ -0,0 +1,12 @@
+"""Gazebo 仿真后端。
+
+只暴露纯函数级别的能力，不在这里 import rclpy —— 这样
+urdf_normalize 可以被单测直接导入，不需要 ROS 运行时。
+"""
+from .urdf_normalize import (  # noqa: F401
+    NormalizeReport,
+    normalize_file,
+    normalize_urdf,
+)
+
+__all__ = ["NormalizeReport", "normalize_file", "normalize_urdf"]

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/gazebo_sim_launcher.py

@@ -0,0 +1,221 @@
+"""gazebo_sim_launcher —— devices/sim/gazebo/manifest.yaml 声明的那个可执行。
+
+职责刻意做得很薄：它只负责"准备描述"这件事，然后交给常规的 launch 文件去跑。
+不把整套启动逻辑塞进可执行文件里，是因为 `ros2 launch` 的日志/退出码/中断处理
+都更成熟，自己重造一份只会更难调试。
+
+用法：
+    gazebo_sim_launcher                      # 准备描述并启动仿真
+    gazebo_sim_launcher --print-urdf         # 只看展开+归一化后的 URDF（排查用）
+    gazebo_sim_launcher --check              # 只做自检，不启动任何东西
+    gazebo_sim_launcher --headless false     # 带 GUI
+    gazebo_sim_launcher --dry-run            # 打印将要执行的命令，不执行
+
+为什么 --print-urdf / --check 值得单独做：仿真跑不起来时，绝大多数问题都在
+"描述对不对"这一层（link 被丢、关节名对不上）。能把描述单独打出来看，
+比对着整套仿真的日志找要快一个量级。这两个选项都不需要 Gazebo 在跑。
+"""
+from __future__ import annotations
+
+import argparse
+import os
+import sys
+from pathlib import Path
+from typing import List, Optional
+
+from .robot_description import (
+    SimDescriptionError,
+    check_base_plugins,
+    default_controllers,
+    default_world,
+    default_xacro,
+    prepare_urdf,
+    repo_root,
+    world_name,
+)
+from .sim_site import (
+    SimSiteError,
+    load_sim_site,
+    physics_enabled_in_world,
+    resolve_world,
+    unwired_warnings,
+)
+
+LAUNCH_PKG = "arm_sim_gazebo"
+LAUNCH_FILE = "gazebo_sim.launch.py"
+
+# 仿真实际使用的关节前缀，用来跟 site.yaml 的 sim.joint_prefix 比对。
+# 它由 arm.sim.urdf.xacro 的 arm_prefix 默认值决定，并且 controllers.yaml
+# 里硬编码了 sim_joint1..7，所以暂时**不可配置** —— 改了这里不改那两处会散架。
+# site.yaml 若写了一个不一样的值，unwired_warnings() 会告警，而不是静默忽略。
+ACTUAL_JOINT_PREFIX = "sim_"
+
+
+def _build_launch_cmd(extra: List[str]) -> List[str]:
+    # 用 python -m 调 ros2 而不是 exec "ros2"：这样在 ros2 CLI 不在 PATH 的
+    # 环境里也能给出清晰报错，而不是一句 "No such file or directory"。
+    return ["ros2", "launch", LAUNCH_PKG, LAUNCH_FILE, *extra]
+
+
+def main(argv: Optional[List[str]] = None) -> int:
+    parser = argparse.ArgumentParser(
+        prog="gazebo_sim_launcher",
+        description="准备仿真描述并启动 Gazebo（arm_sim_gazebo）。",
+    )
+    parser.add_argument(
+        "--print-urdf",
+        action="store_true",
+        help="把展开并归一化后的 URDF 打到 stdout，不启动仿真",
+    )
+    parser.add_argument(
+        "--check",
+        action="store_true",
+        help="只做自检（能否找到仓库根、描述能否展开、归一化了什么），不启动",
+    )
+    parser.add_argument(
+        "--dry-run",
+        action="store_true",
+        help="打印将要执行的 ros2 launch 命令，不执行",
+    )
+    parser.add_argument(
+        "--no-gripper",
+        action="store_true",
+        help="不挂夹爪（纯运动学验证时更快）",
+    )
+    parser.add_argument(
+        "--no-sensors",
+        action="store_true",
+        help="不挂相机与触觉",
+    )
+    parser.add_argument(
+        "--world", default="", help="世界文件路径，覆盖默认的 assembly.world"
+    )
+    parser.add_argument(
+        "--headless",
+        default="true",
+        choices=["true", "false"],
+        help="true=只开 Gazebo 服务端（无显示器环境用）；false=带 GUI",
+    )
+    parser.add_argument(
+        "--verbose",
+        action="store_true",
+        help="打开 Gazebo 详细日志（排查插件加载失败时开）",
+    )
+    args = parser.parse_args(argv)
+
+    # ---- 1) 准备描述（这一步是真正有内容的，其余都是转发）------------------
+    try:
+        root = repo_root()
+        xacro_path = default_xacro(root)
+        controllers = default_controllers(root)
+        # world 的优先级：--world > site.yaml 的 sim.world > 内置默认。
+        # 以前这里只认 --world 和硬编码默认值，而 site.yaml / manifest.yaml
+        # 都宣称改 sim.world 就生效 —— 结果改了毫无反应且不报错。
+        world, world_origin = resolve_world(args.world, root, fallback=default_world(root))
+        world = str(world)
+        site = load_sim_site(root)
+        urdf, report = prepare_urdf(
+            xacro_path=xacro_path,
+            controllers_file=controllers,
+            load_gripper=not args.no_gripper,
+            with_sensors=not args.no_sensors,
+        )
+        wname = world_name(world)
+        missing = check_base_plugins(Path(world))
+    except (SimDescriptionError, SimSiteError) as exc:
+        print(f"[gazebo_sim_launcher] 失败：\n{exc}", file=sys.stderr)
+        return 2
+
+    # site.yaml 里"声明了但没接线"的键：与其静默无效，不如明说。
+    # 见 sim_site.unwired_warnings() —— 那里逐条说明了为什么还没接。
+    for _w in unwired_warnings(
+        site,
+        actual_joint_prefix=ACTUAL_JOINT_PREFIX,
+        physics_in_world=physics_enabled_in_world(Path(world)),
+    ):
+        print(f"[gazebo_sim_launcher] ⚠ {_w}", file=sys.stderr)
+
+    if missing:
+        # 这一条能省掉一整个下午：world 漏声明基础系统时，仿真看起来完全正常
+        # （/clock 在走、create 打印成功），但机械臂从未进入 Gazebo。
+        print(
+            "[gazebo_sim_launcher] ⚠ world 缺少基础系统: "
+            + ", ".join(missing) + "\n"
+            "    world 里只要声明了任何一个 <plugin>，Gazebo 就不再加载默认系统列表。\n"
+            "    缺 UserCommands → /world/<名的>/create 服务不存在 → 模型根本不会\n"
+            "    被创建，而 create 仍会打印 'OK creation of entity'。最终表现为\n"
+            "    控制器永远拉不起来。修法：在这个 world 里显式声明\n"
+            "    Physics / UserCommands / SceneBroadcaster（见文件里的注释）。",
+            file=sys.stderr,
+        )
+
+    if args.print_urdf:
+        # URDF 走 stdout（可重定向），摘要走 stderr（不污染管道）
+        sys.stdout.write(urdf)
+        print(f"\n[gazebo_sim_launcher] {report.summary()}", file=sys.stderr)
+        return 0
+
+    if args.check:
+        print("[gazebo_sim_launcher] 自检通过")
+        print(f"  仓库根      : {root}")
+        print(f"  描述        : {xacro_path}")
+        print(f"  控制器配置  : {controllers}")
+        print(f"  世界        : {world}  (world 名: {wname})")
+        print(f"  世界来源    : {world_origin}")
+        if site.source is not None:
+            print(f"  site.yaml   : {site.source}")
+        else:
+            print("  site.yaml   : 不存在（用默认值；这不会影响仿真）")
+        print(f"  URDF 归一化 : {report.summary()}")
+        if missing:
+            print(
+                f"  ⚠ world 缺少基础系统: {', '.join(missing)}\n"
+                "    world 里只要声明了任何一个 <plugin>，Gazebo 就不再加载默认系统。\n"
+                "    缺 UserCommands → /world/<名的>/create 服务不存在 → 模型不会被创建，\n"
+                "    而 create 仍打印 'OK creation of entity'，控制器永远拉不起来。"
+            )
+        else:
+            print("  world 基础系统 : Physics / UserCommands / SceneBroadcaster 齐备")
+        if not report.changed:
+            print(
+                "  ⚠ 归一化没有改动任何内容。若后续 Gazebo 报 link 不存在，\n"
+                "    先确认 flexiv_description 的版本是否已自行修好那两个上游缺陷。"
+            )
+        print("\n  下一步: gazebo_sim_launcher --dry-run   # 看会执行什么")
+        return 0
+
+    # ---- 2) 转发给 launch 文件 ---------------------------------------------
+    # 参数名在这里显式映射到 launch 参数，不做通用透传 —— 通用透传会把拼错
+    # 的参数名静默吞掉（ros2 launch 对未知的 key:=value 不报错），
+    # 而"我明明设了 --headless false 却没生效"这种问题极难查。
+    extra = [
+        "headless:=" + args.headless,
+        "verbose:=" + ("true" if args.verbose else "false"),
+    ]
+    if args.world:
+        extra.append("world:=" + args.world)
+    if args.no_gripper:
+        extra.append("load_gripper:=false")
+    if args.no_sensors:
+        extra.append("with_sensors:=false")
+
+    cmd = _build_launch_cmd(extra)
+    print(f"[gazebo_sim_launcher] {report.summary()}")
+    print(f"[gazebo_sim_launcher] exec: {' '.join(cmd)}")
+
+    if args.dry_run:
+        return 0
+
+    try:
+        os.execvp(cmd[0], cmd)
+    except FileNotFoundError:
+        print(
+            "[gazebo_sim_launcher] 找不到 ros2 命令。先激活环境：\n"
+            "    source scripts/sim_env.sh",
+            file=sys.stderr,
+        )
+        return 2
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/robot_description.py

@@ -0,0 +1,612 @@
+"""组装仿真用的机器人描述：xacro 展开 → 归一化 → 交给 ROS / Gazebo。
+
+=============================================================================
+为什么单独一个模块
+=============================================================================
+"把 devices/sim/gazebo/urdf 下的描述变成一份可用的 URDF"这件事有两处需要：
+    1. launch 文件（真正启动仿真时）
+    2. gazebo_sim_launcher 的 --print-urdf（只想看看展开+归一化后长什么样）
+两边各写一份必然漂移，所以实现只放这里，两处都 import 它。
+
+=============================================================================
+仓库根怎么找
+=============================================================================
+devices/ **不是 ROS 包**，所以 $(find ...) 找不到它，必须用文件系统路径。
+按可靠性从高到低试三种：
+    1. $ROS2_WS（scripts/sim_env.sh 会导出）—— 最可靠，显式且便宜
+    2. 从当前工作目录向上找标志文件 —— 手工跑脚本时的常见情形
+    3. 从本模块所在位置向上找 —— 兜底（安装到 install_sim/ 后路径较深）
+三者都失败时给出明确的报错和修复提示，而不是抛一个看不懂的 FileNotFoundError。
+"""
+from __future__ import annotations
+
+import math
+import os
+import re
+import subprocess
+from pathlib import Path
+from typing import Dict, List, Optional, Sequence, Tuple
+
+from .urdf_normalize import NormalizeReport, normalize_urdf
+
+# 用它判断"这个目录是不是仓库根"。选它是因为这个文件是仿真描述的唯一入口，
+# 存在即说明目录结构正确；不像 .git 那样在导出/打包后可能不存在。
+_MARKER = "devices/sim/gazebo/urdf/arm.sim.urdf.xacro"
+
+_XACRO_REL = "devices/sim/gazebo/urdf/arm.sim.urdf.xacro"
+_CONTROLLERS_REL = "devices/sim/gazebo/config/controllers.yaml"
+_WORLD_REL = "devices/sim/gazebo/worlds/assembly.world"
+
+
+class SimDescriptionError(RuntimeError):
+    """描述准备过程中的可预期失败，消息面向使用者而非开发者。"""
+
+
+def repo_root(start: Optional[Path] = None) -> Path:
+    """定位仓库根。找不到时抛 SimDescriptionError 并说明怎么修。"""
+    env = os.environ.get("ROS2_WS", "").strip()
+    if env:
+        p = Path(env).expanduser()
+        if (p / _MARKER).is_file():
+            return p
+
+    seeds = []
+    if start is not None:
+        seeds.append(Path(start).expanduser())
+    seeds.append(Path.cwd())
+    seeds.append(Path(__file__).resolve().parent)
+
+    for seed in seeds:
+        cur = seed.resolve()
+        for _ in range(12):  # 足够覆盖仓库内的任意深度
+            if (cur / _MARKER).is_file():
+                return cur
+            if cur.parent == cur:
+                break
+            cur = cur.parent
+
+    raise SimDescriptionError(
+        "找不到仓库根（即包含 %s 的目录）。\n"
+        "  修法（任选其一）：\n"
+        "    1) source scripts/sim_env.sh   # 它会导出 ROS2_WS\n"
+        "    2) 在仓库根目录下执行\n"
+        "  已试过的起点：%s" % (_MARKER, ", ".join(str(s) for s in seeds))
+    )
+
+
+def default_xacro(root: Optional[Path] = None) -> Path:
+    return (root or repo_root()) / _XACRO_REL
+
+
+def default_controllers(root: Optional[Path] = None) -> Path:
+    return (root or repo_root()) / _CONTROLLERS_REL
+
+
+def default_world(root: Optional[Path] = None) -> Path:
+    return (root or repo_root()) / _WORLD_REL
+
+
+def world_name(world_file: Path) -> str:
+    """从 world 文件里读出 <world name="...">。
+
+    为什么解析而不是让调用方传：world 名必须和文件内容一致，让它们分别配置
+    就等于制造一个"改了文件名忘了改参数"的机会。create 参数写错 world 名时
+    Gazebo 不会报错，只是实体静默地没被创建。
+    """
+    try:
+        text = Path(world_file).read_text(encoding="utf-8", errors="replace")
+    except OSError as exc:
+        raise SimDescriptionError(f"读不到 world 文件 {world_file}: {exc}") from exc
+
+    # 同时接受单引号与双引号：手写 world 时两种都会出现，
+    # 早期只认双引号，于是 `<world name='x'>` 会被判成"找不到 world 名"。
+    m = re.search(r"""<world\b[^>]*\bname\s*=\s*["']([^"']+)["']""", text)
+    if not m:
+        raise SimDescriptionError(
+            f"{world_file} 里找不到 <world name=\"...\">，无法确定 world 名。"
+        )
+    return m.group(1)
+
+
+# =============================================================================
+# 从 world 文件里读模型位姿
+# =============================================================================
+# 为什么需要：Gazebo 里的 <model>（尤其是 static 的工装/相机支架）**不会**产生
+# ROS TF —— TF 只来自 robot_state_publisher。但应用要做 tf2 查询，比如
+# "把 D435 看到的点从相机系转到 world"。少了这条 TF，查询直接报
+# "frame does not exist"，而图像本身看起来完全正常。
+#
+# 为什么是"解析"而不是"再配置一遍"：位姿的唯一真相就在 world 文件里。
+# 另配一份等于制造"改了 world 忘了改配置"的机会，而这种不一致不会报错，
+# 只会让相机看到的东西和它自称的位置对不上 —— 极难查。
+def _find_block(text: str, tag: str, name: str) -> Optional[str]:
+    """取出 <tag name="name">...</tag> 的内容（考虑同名标签嵌套）。
+
+    SDF 里 <model> 可以嵌套 <model>（组合体），<link> 里也可能有 <link>（少见）。
+    只找第一个 </tag> 在嵌套时会截断到错误的位置，所以这里数嵌套深度。
+    """
+    pat = re.compile(
+        r"<%(t)s\b[^>]*\bname\s*=\s*[\"']%(n)s[\"'][^>]*>" % {"t": tag, "n": re.escape(name)}
+    )
+    m = pat.search(text)
+    if not m:
+        return None
+
+    # 自闭合：<link name="l"/> 是完全合法的 SDF（没有子元素）。
+    # 不特判的话会走到下面的"找 </link>"分支、找不到 → 返回 None，
+    # 于是被误报成"这个 link 不存在"，而它明明在。
+    if m.group(0).rstrip().endswith("/>"):
+        return ""
+
+    inner_start = m.end()
+    depth = 1
+    pos = inner_start
+    open_re = re.compile(r"<%(t)s\b" % {"t": tag})
+    close_re = re.compile(r"</%(t)s\s*>" % {"t": tag})
+    while depth > 0:
+        o = open_re.search(text, pos)
+        c = close_re.search(text, pos)
+        if c is None:
+            return None  # 标签没闭合 → 文件本身有问题，交给别的检查去报
+        if o is not None and o.start() < c.start():
+            depth += 1
+            pos = o.end()
+        else:
+            depth -= 1
+            if depth == 0:
+                return text[inner_start : c.start()]
+            pos = c.end()
+    return None
+
+
+def _own_pose(block: str, stop_before: Tuple[str, ...]) -> Tuple[float, float, float, float, float, float]:
+    """取一段块自己的 <pose>（默认全 0）。
+
+    只认**第一个** <pose>，且必须出现在 stop_before 里任一标签之前 ——
+    这样才不会把子元素的 pose 误当成父元素的。例如 <model> 的 pose 必须在
+    第一个 <link>/<model>/<joint> 之前；<link> 的 pose 必须在第一个
+    <visual>/<collision>/<sensor> 之前。
+    """
+    cut = len(block)
+    for tag in stop_before:
+        i = block.find("<" + tag)
+        if 0 <= i < cut:
+            cut = i
+    m = re.search(r"<pose(?:\s[^>]*)?>([^<]*)</pose>", block[:cut])
+    if not m:
+        return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+    vals = m.group(1).split()
+    if len(vals) < 6:
+        return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+    try:
+        return tuple(float(v) for v in vals[:6])  # type: ignore[return-value]
+    except ValueError:
+        return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+
+
+def _rpy_to_quat(r: float, p: float, y: float) -> Tuple[float, float, float, float]:
+    cr, sr = math.cos(r / 2), math.sin(r / 2)
+    cp, sp = math.cos(p / 2), math.sin(p / 2)
+    cy, sy = math.cos(y / 2), math.sin(y / 2)
+    return (
+        sr * cp * cy - cr * sp * sy,
+        cr * sp * cy + sr * cp * sy,
+        cr * cp * sy - sr * sp * cy,
+        cr * cp * cy + sr * sp * sy,
+    )
+
+
+def _quat_to_rpy(q: Tuple[float, float, float, float]) -> Tuple[float, float, float]:
+    x, y, z, w = q
+    # roll (x 轴)
+    sinr = 2 * (w * x + y * z)
+    cosr = 1 - 2 * (x * x + y * y)
+    roll = math.atan2(sinr, cosr)
+    # pitch (y 轴)：用 asin，并夹到 [-1,1] 免得浮点误差导致 domain error
+    sinp = 2 * (w * y - z * x)
+    sinp = max(-1.0, min(1.0, sinp))
+    pitch = math.asin(sinp)
+    # yaw (z 轴)
+    siny = 2 * (w * z + x * y)
+    cosy = 1 - 2 * (y * y + z * z)
+    yaw = math.atan2(siny, cosy)
+    return (roll, pitch, yaw)
+
+
+def _quat_mul(a, b):
+    ax, ay, az, aw = a
+    bx, by, bz, bw = b
+    return (
+        aw * bx + ax * bw + ay * bz - az * by,
+        aw * by - ax * bz + ay * bw + az * bx,
+        aw * bz + ax * by - ay * bx + az * bw,
+        aw * bw - ax * bx - ay * by - az * bz,
+    )
+
+
+def _rotate(q, v):
+    """用四元数旋转向量 v（v 在子系里，返回在父系里的表达）。"""
+    x, y, z, w = q
+    vx, vy, vz = v
+    # t = 2 * (q_vec × v)
+    tx = 2 * (y * vz - z * vy)
+    ty = 2 * (z * vx - x * vz)
+    tz = 2 * (x * vy - y * vx)
+    # v' = v + w*t + q_vec × t
+    return (
+        vx + w * tx + (y * tz - z * ty),
+        vy + w * ty + (z * tx - x * tz),
+        vz + w * tz + (x * ty - y * tx),
+    )
+
+
+def model_link_pose(
+    world_file: Path, model: str, link: str = ""
+) -> Tuple[Tuple[float, float, float], Tuple[float, float, float]]:
+    """返回 world 文件里 <model name=model> 的 <link name=link> 在**世界系**的位姿。
+
+    返回值是 ((x, y, z), (roll, pitch, yaw))。link 留空表示取模型本身的位姿。
+    两者都会做复合（模型位姿 ∘ link 位姿），因为 SDF 的 <pose> 是相对父元素的。
+
+    找不到模型/link 时抛 SimDescriptionError，并提示这里和 cameras.yaml 的
+    world_model 字段必须对应 —— 配错的话相机不会有 TF，而症状是
+    "tf 查询报 frame does not exist"，与相机本身无关，容易查错方向。
+    """
+    text = Path(world_file).read_text(encoding="utf-8", errors="replace")
+
+    mblock = _find_block(text, "model", model)
+    if mblock is None:
+        raise SimDescriptionError(
+            f"{world_file} 里找不到 <model name=\"{model}\">。\n"
+            f"  devices/sim/gazebo/config/cameras.yaml 里若有 world_model: {model}，"
+            f"就必须在 world 里真的有这个模型 —— 否则该相机没有 TF，"
+            f"应用做 tf2 查询会报 frame does not exist。"
+        )
+
+    mp = _own_pose(mblock, ("link", "model", "joint", "plugin"))
+    mq = _rpy_to_quat(mp[3], mp[4], mp[5])
+
+    if not link:
+        return ((mp[0], mp[1], mp[2]), (mp[3], mp[4], mp[5]))
+
+    lblock = _find_block(mblock, "link", link)
+    if lblock is None:
+        raise SimDescriptionError(
+            f"{world_file} 的 <model name=\"{model}\"> 里找不到 <link name=\"{link}\">。"
+        )
+
+    lp = _own_pose(lblock, ("visual", "collision", "sensor", "light"))
+    lq = _rpy_to_quat(lp[3], lp[4], lp[5])
+
+    # 复合：世界系下的位置 = t_model + R_model * t_link
+    t = _rotate(mq, (lp[0], lp[1], lp[2]))
+    pos = (mp[0] + t[0], mp[1] + t[1], mp[2] + t[2])
+    rpy = _quat_to_rpy(_quat_mul(mq, lq))
+    return (pos, rpy)
+
+
+# 只要 world 里声明了任何一个 <plugin>，Gazebo 就不再加载 server.config 的默认
+# 系统列表，这三个基础系统必须由 world 自己声明，否则整个仿真静默失效。
+# 详细后果见 assembly.world 里那段注释，以及这里的 check_base_plugins()。
+BASE_SYSTEMS = {
+    "Physics": "ignition::gazebo::systems::Physics",
+    "UserCommands": "ignition::gazebo::systems::UserCommands",
+    "SceneBroadcaster": "ignition::gazebo::systems::SceneBroadcaster",
+}
+
+
+def check_base_plugins(world_file: Path) -> List[str]:
+    """检查 world 是否漏声明基础系统。返回缺失的短名列表（空 = 没问题）。
+
+    背景（实测，本机 RoboStack/conda 的 Fortress 6.16）：
+        world 里**只要声明了任何一个 <plugin>**，Gazebo 就跳过 server.config 里
+        的默认系统。于是少了 UserCommands → /world/<w>/create 服务不存在 →
+        ros_gz_sim create 超时后回退发 topic、没人收 → **模型从未被创建**，
+        但 create 节点照样打印 "OK creation of entity"。最终表现是控制器永远
+        拉不起来，而每一步日志看起来都是成功的。
+
+    这个检查是纯文本的、不需要 Gazebo 在跑，所以放进 --check 里，
+    让"改了 world 忘了加基础系统"在启动前就被指出来。
+    """
+    text = Path(world_file).read_text(encoding="utf-8", errors="replace")
+    if "<plugin" not in text:
+        return []  # 没声明任何 plugin → Gazebo 用默认配置，反而正常
+
+    missing = []
+    for short, fq in BASE_SYSTEMS.items():
+        # 名字里有点号，用 re.escape 免得被当通配
+        if not re.search(re.escape(fq), text):
+            missing.append(short)
+    return missing
+
+
+def check_tactile_collisions(
+    urdf: str, expected_topics: Optional[Sequence[str]] = None
+) -> List[str]:
+    """把展开后的 URDF 转成 SDF，核对触觉传感器的引用是否真的存在。
+
+    返回问题列表（空 = 没问题）。``expected_topics`` 若给出，还会核对这些话题
+    在 SDF 里是否真的被声明了 —— 这是**最有价值的一项**：传感器标签一旦没进
+    SDF（写错位置、被 sdformat 静默丢掉），Gazebo 里就只会表现为"话题不出现、
+    日志空白"，而本检查能在启动前直接指出是哪个话题缺了。
+
+    已实测确认能抓到的情形：
+      * <contact><collision> 引用的名字在 SDF 里不存在（sdformat 命名规则变了）
+      * <contact> 里没有 <topic>（sdformat 会补一个占位值 __default_topic__，
+        所以这一项不能只判"空"，要连占位值一起判 —— 这是单测逼出来的）
+      * 传感器标签没有产生出任何 force_torque/contact 传感器
+      * 配置里声明的话题在 SDF 里找不到
+
+    =========================================================================
+    为什么必须有这个检查
+    =========================================================================
+    接触传感器的 <collision> 里写的**不是我们起的名字**，而是 sdformat 从
+    URDF 转 SDF 时按规则自动生成的：
+
+        <fixed 关节链的父 link> + "_fixed_joint_lump__" + <本 link> + "_" + collision 名
+
+    本机实测（Fortress 6.16）得到
+        sim_left_finger_mount_fixed_joint_lump__sim_left_finger_tip_collision
+    而 URDF 的 <collision> 不能命名，所以没法给它一个稳定名字。代价是：
+    **换一个 sdformat 版本、命名规则一变，传感器就永远收不到数据 —— 而
+    Gazebo 不会为此报任何错**（话题在、没输出，看起来像"还没接触"）。
+
+    同样容易静默失效的两处，这里一并核对：
+      * force_torque 传感器挂的关节是否存在于 SDF（写错关节名 = 永不发布）
+      * 接触传感器的话题是否在 <contact> 里（写在 <sensor> 层会被忽略，
+        gz 转而使用默认的 scoped 话题名，于是"按 /tactile/left/raw 订阅"
+        永远收不到东西）
+
+    纯文本 + 一次 `ign sdf -p`，不需要 Gazebo 在跑，所以放进启动前的检查里。
+    """
+    try:
+        sdf = _urdf_to_sdf(urdf)
+    except SimDescriptionError as exc:
+        return [f"无法把 URDF 转成 SDF 做触觉检查：{exc}"]
+
+    problems: List[str] = []
+    try:
+        import xml.etree.ElementTree as ET
+
+        root = ET.fromstring(sdf)
+    except ET.ParseError as exc:
+        return [f"SDF 解析失败，无法核对触觉传感器: {exc}"]
+
+    # SDF 里接触传感器的 <collision> 引用的是 collision 的**限定名**。
+    # ⚠️ 不要再拼一次 link 名前缀：sdformat 为 fixed 关节融合出来的 collision
+    #    名字本身就已经含 link 前缀
+    #    （sim_left_finger_mount_fixed_joint_lump__sim_left_finger_tip_collision），
+    #    拼了反而变成一个不存在的名字 —— 于是这个检查会**误报**，
+    #    而误报比不检查更糟：真出问题时没人信它。两种写法都收。
+    collision_names = set()
+    for link in root.iter("link"):
+        lname = link.get("name", "")
+        for col in link.findall("collision"):
+            cname = col.get("name", "")
+            if cname:
+                collision_names.add(cname)
+                collision_names.add(f"{lname}::{cname}")
+            elif lname:
+                collision_names.add(lname)
+
+    joint_names = {j.get("name", "") for j in root.iter("joint")}
+    sdf_topics = {(t.text or "").strip() for t in root.iter("topic")}
+    n_contact = 0
+    n_ft = 0
+
+    for sensor in root.iter("sensor"):
+        stype = sensor.get("type", "")
+        sname = sensor.get("name", "?")
+        if stype == "contact":
+            n_contact += 1
+            contact = sensor.find("contact")
+            if contact is None:
+                problems.append(f"接触传感器 {sname} 没有 <contact> 块")
+                continue
+            ref = (contact.findtext("collision") or "").strip()
+            if not ref or ref == "__default__":
+                problems.append(
+                    f"接触传感器 {sname} 的 <contact> 里没有可用的 <collision>"
+                    f"（实测值: {ref or '空'}）。\n"
+                    f"    留空不会「监测全部」：sdformat 会补成 __default__"
+                    f"（一个不存在的 collision），症状是永远没有输出。"
+                )
+            elif ref not in collision_names:
+                problems.append(
+                    f"接触传感器 {sname} 引用的 collision 在 SDF 里不存在: {ref}\n"
+                    f"    这几乎总是 sdformat 的自动命名规则变了。"
+                    f"现有 collision: {sorted(collision_names)[:4]} ...\n"
+                    f"    用 `ign sdf -p <展开后的 urdf>` 看实际名字，"
+                    f"改 devices/sim/gazebo/urdf/arm.sim.sensors.xacro 里的 <collision>。"
+                )
+            # ⚠️ 同样要防 sdformat 的占位值：缺 <topic> 时它会补一个
+            #    __default_topic__（实测），于是"有没有 <topic>"这种检查会被骗过去。
+            #    __default_topic__ 不是可用话题名 —— gz 实际发的是
+            #    /world/<w>/model/<m>/link/<l>/sensor/<s>/contact。
+            ctopic = (contact.findtext("topic") or "").strip()
+            if ctopic in ("", "__default_topic__"):
+                problems.append(
+                    f"接触传感器 {sname} 的 <contact> 里没有可用 <topic>"
+                    f"（实测值: {ctopic or '空'}）。\n"
+                    f"    话题必须写在 contact/topic（写在 <sensor> 层会被静默忽略，"
+                    f"gz 改用默认 scoped 话题名）。"
+                )
+        elif stype == "force_torque":
+            n_ft += 1
+            parent = _sdf_parent_joint(root, sensor)
+            if parent is None:
+                problems.append(
+                    f"力传感器 {sname} 不在任何 <joint> 里。\n"
+                    f"    force_torque 在 SDF 里只能属于关节；用 "
+                    f"<gazebo reference=「关节名」><sensor .../></gazebo> 表达，"
+                    f"sdformat 会把它移进对应的 <joint>。"
+                )
+            elif parent not in joint_names:
+                problems.append(f"力传感器 {sname} 挂在未知关节 {parent} 上")
+            if (sensor.findtext("topic") or "").strip() == "":
+                problems.append(
+                    f"力传感器 {sname} 没有 <topic>（它不会用默认话题名，"
+                    f"而是完全不发布）。"
+                )
+
+    # 传感器是否被整个丢掉 —— 这一项最容易被忽略，也最难现场排查。
+    if n_contact == 0 and n_ft == 0:
+        problems.append(
+            "SDF 里**一个触觉传感器都没有**。\n"
+            "    说明 arm.sim.sensors.xacro 里的 <gazebo reference=...> 名字写错、"
+            "或传感器标签写到了错误的位置 —— sdformat 会把整块**静默丢掉**：\n"
+            "    转换成功、无警告、Gazebo 里话题不出现、日志空白。"
+        )
+    if expected_topics:
+        missing = [t for t in expected_topics if t not in sdf_topics]
+        if missing:
+            problems.append(
+                f"以下触觉话题在 SDF 里不存在: {', '.join(missing)}\n"
+                f"    配置（tactile.yaml）里声明了这些话题，但描述里没有产生它们。\n"
+                f"    常见原因：<gazebo reference=「关节名」> 的关节名不存在"
+                f"（整个传感器被丢掉），或 contact 传感器少写了 <topic>。\n"
+                f"    SDF 里实际有的话题: {sorted(t for t in sdf_topics if t)}"
+            )
+    return problems
+
+
+def _sdf_parent_joint(root, sensor) -> Optional[str]:
+    """在 SDF 树里找 sensor 所属的 <joint> 名。"""
+    for joint in root.iter("joint"):
+        for s in joint.findall("sensor"):
+            if s is sensor:
+                return joint.get("name", "")
+    return None
+
+
+def _urdf_to_sdf(urdf: str) -> str:
+    """调 `ign sdf -p` 把 URDF 文本转成 SDF 文本。
+
+    走命令行而不是 Python 绑定：sdformat 的 Python 绑定在这个环境里没有装，
+    而 ign CLI 是仿真环境的必备件（sim_env.sh 里就有）。
+    """
+    import tempfile
+
+    with tempfile.NamedTemporaryFile("w", suffix=".urdf", delete=False,
+                                     encoding="utf-8") as fh:
+        fh.write(urdf)
+        tmp = fh.name
+    try:
+        r = subprocess.run(
+            ["ign", "sdf", "-p", tmp], capture_output=True, text=True, timeout=120
+        )
+    except FileNotFoundError as exc:
+        raise SimDescriptionError(
+            "找不到 ign 命令，无法做 SDF 转换检查。修法: source scripts/sim_env.sh"
+        ) from exc
+    finally:
+        try:
+            os.unlink(tmp)
+        except OSError:
+            pass
+    if r.returncode != 0:
+        raise SimDescriptionError(
+            f"ign sdf 转换失败（退出码 {r.returncode}）: {(r.stderr or r.stdout).strip()[:600]}"
+        )
+    return r.stdout
+
+
+def expand_xacro(
+    xacro_path: Path,
+    mappings: Optional[Dict[str, object]] = None,
+) -> str:
+    """跑 xacro 展开。
+
+    优先用 Python 模块（避免子进程与 PATH 依赖），失败再退回命令行。
+    两者行为一致，但模块方式能拿到更干净的报错。
+    """
+    xacro_path = Path(xacro_path)
+    if not xacro_path.is_file():
+        raise SimDescriptionError(f"xacro 文件不存在: {xacro_path}")
+
+    try:
+        import xacro  # type: ignore
+    except ImportError:
+        return _expand_via_cli(xacro_path, mappings)
+
+    try:
+        doc = xacro.process_file(str(xacro_path), mappings=mappings or {})
+    except Exception as exc:  # xacro 的异常类型很杂，统一转成可读报错
+        raise SimDescriptionError(
+            f"xacro 展开失败: {xacro_path}\n  原因: {exc}\n"
+            f"  单独复现: xacro {xacro_path}"
+        ) from exc
+    return doc.toxml()
+
+
+def _expand_via_cli(xacro_path: Path, mappings: Optional[Dict[str, object]]) -> str:
+    cmd = ["xacro", str(xacro_path)]
+    for k, v in (mappings or {}).items():
+        cmd.append(f"{k}:={v}")
+    try:
+        r = subprocess.run(cmd, capture_output=True, text=True, timeout=120)
+    except FileNotFoundError as exc:
+        raise SimDescriptionError(
+            "既 import 不到 xacro 模块，也找不到 xacro 命令。\n"
+            "  修法: source scripts/sim_env.sh"
+        ) from exc
+    if r.returncode != 0:
+        raise SimDescriptionError(
+            f"xacro 展开失败（退出码 {r.returncode}）: {xacro_path}\n"
+            f"  {(r.stderr or r.stdout).strip()[:800]}"
+        )
+    return r.stdout
+
+
+def prepare_urdf(
+    xacro_path: Optional[Path] = None,
+    controllers_file: Optional[Path] = None,
+    load_gripper: bool = True,
+    with_sensors: bool = True,
+    rizon_type: str = "Rizon4",
+    arm_prefix: str = "sim",
+    robot_sn: str = "",
+    wrist_ft_topic: str = "",
+) -> Tuple[str, NormalizeReport]:
+    """展开 + 归一化，返回 (URDF 文本, 归一化报告)。
+
+    调用方**必须**把这份 URDF 同时用于两处，否则会出问题：
+        - robot_state_publisher 发布到 /robot_description
+        - ros_gz_sim create 用它 spawn 进 Gazebo
+    gz_ros2_control 的插件是从 /robot_description 读 <ros2_control> 的
+    （SDF 里只存了插件名和参数文件路径，接口声明不在 SDF 里，实测确认）。
+    所以若 RSP 发的是**未归一化**的那份，控制器注册的关节名就会和 Gazebo 里的
+    对不上，症状是 "Joint 'sim_joint1' not found in the robot model"。
+
+    :param wrist_ft_topic: 腕部 FT 的 gz 话题名。**从 wrist_ft.yaml 传进来**，
+        而不是让 xacro 里再写一份 —— 话题名有三处要对上（配置、URDF、
+        应用层按 sn 推出的名字），两处写死迟早漂移。空 = 用 xacro 的默认值。
+    """
+    root = repo_root()
+    xacro_path = Path(xacro_path) if xacro_path else default_xacro(root)
+    controllers_file = (
+        Path(controllers_file) if controllers_file else default_controllers(root)
+    )
+
+    if not Path(controllers_file).is_file():
+        raise SimDescriptionError(
+            f"控制器配置不存在: {controllers_file}\n"
+            f"  它是 arm.sim.gazebo.xacro 里系统插件的 <parameters> 指向的文件，"
+            f"缺了会导致 Gazebo 报 'Failed to load system plugin'。"
+        )
+
+    xacro_args = {
+        "controllers_file": str(Path(controllers_file).resolve()),
+        "load_gripper": "true" if load_gripper else "false",
+        "with_sensors": "true" if with_sensors else "false",
+        "rizon_type": rizon_type,
+        "arm_prefix": arm_prefix,
+        "robot_sn": robot_sn,
+    }
+    # 只在给了值时才覆盖，否则保持 xacro 里那个有注释、能单独跑 xacro 的默认值。
+    if wrist_ft_topic:
+        xacro_args["wrist_ft_topic"] = wrist_ft_topic
+
+    urdf = expand_xacro(xacro_path, xacro_args)
+    normalized, report = normalize_urdf(urdf)
+    return normalized, report

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/sim_camera_adapter.py

@@ -0,0 +1,309 @@
+"""sim_camera_adapter —— 把桥接过来的 Gazebo 相机，变成应用能直接吃的话题。
+
+=============================================================================
+它到底解决什么问题
+=============================================================================
+ros_gz_bridge 只做"传输"：把 gz 的消息类型翻成 ROS 的消息类型，话题名照抄。
+它**改不了**两样东西，而这两样恰恰是仿真里最容易错、也最不报错的：
+
+    1. frame_id
+       gz 盖的是内部路径（实测是 rizon_sim/sim_link7/d405_rgbd），
+       而 TF 里真实存在的坐标系叫 sim_d405_link —— 两者对不上。
+       更麻烦的是：即使改成 sim_d405_link 也还是错的，因为那是"相机机身"系
+       （X 前 / Y 左 / Z 上），而 ROS 的 image/camera_info 必须是光学系
+       （Z 前 / X 右 / Y 下）。tf2 查询两种都能查到、都不报错，
+       但用错的那个把深度反投影成三维点会整体转错。
+       所以这里盖的是 <相机>_color_optical_frame，并同时发布
+       link → optical 的静态 TF。
+
+    2. 深度单位
+       gz 是 32FC1 / 米，真机 realsense 的 aligned_depth 是 16UC1 / 毫米。
+       不转换的话应用按毫米解释会整体差 1000 倍，而且 dtype 不符时
+       numpy 读出来直接是垃圾。
+
+=============================================================================
+为什么是"桥接 + 适配器"两层，而不是合成一层
+=============================================================================
+桥接器改不了 frame_id，所以无论如何都要有第二层做语义转换。
+既然必须有，就让第二层把"改名 + 坐标系 + 单位"全都集中处理，
+而不是让桥接器改一半、适配器补一半 —— 那样出问题时不知道去哪找。
+副产品：gz 原名的话题（/d405/image 等）在 ROS 图里仍然可见，
+排查"是桥接断了还是适配器断了"时能一刀切开。
+
+=============================================================================
+发布的话题（与真机完全一致，应用无需为仿真改任何代码）
+=============================================================================
+    /<ros_ns>/color/image_raw                    rgb8
+    /<ros_ns>/color/camera_info                  plumb_bob（畸变全 0，gz 不模拟畸变）
+    /<ros_ns>/aligned_depth_to_color/image_raw   16UC1 毫米，0 = 无读数
+
+QoS 用 sensor data（BEST_EFFORT, keep_last 1），与真机 realsense 发布端、
+以及应用的订阅端（qos_profile_sensor_data）都对齐。这一点不能想当然：
+发布端 BEST_EFFORT、订阅端 RELIABLE 是不兼容的，表现是"话题在、echo 不到"。
+"""
+from __future__ import annotations
+
+import math
+from typing import Dict, List, Optional
+
+import numpy as np
+import rclpy
+from geometry_msgs.msg import TransformStamped
+from rclpy.node import Node
+from rclpy.qos import (
+    DurabilityPolicy,
+    HistoryPolicy,
+    QoSProfile,
+    ReliabilityPolicy,
+    qos_profile_sensor_data,
+)
+from sensor_msgs.msg import CameraInfo, Image
+from tf2_ros import StaticTransformBroadcaster
+
+from .sim_cameras import (
+    DEFAULT_CAMERAS_REL,
+    CameraConfigError,
+    CameraSpec,
+    depth_m_to_mm16,
+    intrinsics,
+    load_cameras,
+    optical_rpy,
+    rpy_to_quat,
+)
+
+# 图像上盖的坐标系（光学系）相对相机本体系的固定旋转。
+# 见 sim_cameras.OPTICAL_RPY 的说明 —— 这是"看起来正常但三维全错"的那一项。
+
+
+class SimCameraAdapter(Node):
+    def __init__(self) -> None:
+        super().__init__("sim_camera_adapter")
+
+        self.declare_parameter("cameras_file", "")
+        path = str(self.get_parameter("cameras_file").value or "").strip()
+
+        try:
+            specs = load_cameras(path or None, root=self._repo_root())
+        except Exception as exc:  # 配置错误应当直接说清怎么修，而不是抛栈
+            self.get_logger().error(f"\n仿真相机清单读取失败：\n{exc}")
+            raise
+
+        self._specs: List[CameraSpec] = specs
+        self._subs: List[object] = []
+        self._pubs: Dict[str, object] = {}
+        self._warned: set = set()
+
+        # 相机本体 → 光学系的静态 TF。必须在图像到达**之前**就发出去，
+        # 因为 TF 是 transient_local（迟到的订阅者也能拿到），但若从没发过，
+        # 应用查 tf 会直接报 "frame does not exist"。
+        self._tf = StaticTransformBroadcaster(self)
+        self._publish_optical_tfs()
+
+        for s in specs:
+            self._wire(s)
+
+        names = ", ".join(s.ros_ns for s in specs)
+        self.get_logger().info(
+            f"仿真相机适配器就绪: {names}"
+            + f"（深度 {sum(1 for s in specs if s.depth)}/台 已做 米→毫米 转换）"
+        )
+
+    # ------------------------------------------------------------------ 内部
+    def _repo_root(self):
+        """复用 robot_description 的仓库根定位（它已经处理了三种情形）。"""
+        from .robot_description import repo_root
+
+        try:
+            return repo_root()
+        except Exception:
+            return None
+
+    def _qos(self) -> QoSProfile:
+        # 深拷贝一份 sensor data QoS：共享的那个对象若被改动会影响其它节点。
+        return QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=qos_profile_sensor_data.depth,
+        )
+
+    def _publish_optical_tfs(self) -> None:
+        """发布 link → optical 的静态 TF（每台相机一条，平移为零）。"""
+        msgs = []
+        for s in self._specs:
+            t = TransformStamped()
+            t.header.stamp = self.get_clock().now().to_msg()
+            t.header.frame_id = s.link_frame
+            t.child_frame_id = s.optical_frame
+            t.transform.translation.x = 0.0
+            t.transform.translation.y = 0.0
+            t.transform.translation.z = 0.0
+            qx, qy, qz, qw = rpy_to_quat(optical_rpy())
+            t.transform.rotation.x = qx
+            t.transform.rotation.y = qy
+            t.transform.rotation.z = qz
+            t.transform.rotation.w = qw
+            msgs.append(t)
+        self._tf.sendTransform(msgs)
+
+    def _wire(self, s: CameraSpec) -> None:
+        qos = self._qos()
+
+        self._pubs[s.ros_color] = self.create_publisher(Image, s.ros_color, qos)
+        self._pubs[s.ros_info] = self.create_publisher(CameraInfo, s.ros_info, qos)
+        if s.depth:
+            self._pubs[s.ros_depth] = self.create_publisher(Image, s.ros_depth, qos)
+
+        self._subs.append(
+            self.create_subscription(
+                Image, s.gz_image, lambda m, sp=s: self._on_color(sp, m), qos
+            )
+        )
+        self._subs.append(
+            self.create_subscription(
+                CameraInfo, s.gz_info, lambda m, sp=s: self._on_info(sp, m), qos
+            )
+        )
+        if s.depth:
+            self._subs.append(
+                self.create_subscription(
+                    Image, s.gz_depth, lambda m, sp=s: self._on_depth(sp, m), qos
+                )
+            )
+
+    def _restamp(self, msg, frame_id: str) -> None:
+        """把坐标系换掉。就地改，避免每帧多一次消息构造与一次大数组拷贝。"""
+        if msg.header.frame_id != frame_id:
+            msg.header.frame_id = frame_id
+
+    def _on_color(self, s: CameraSpec, msg: Image) -> None:
+        self._note_encoding(s, msg)
+        self._restamp(msg, s.optical_frame)
+        self._pubs[s.ros_color].publish(msg)
+
+    def _on_info(self, s: CameraSpec, msg: CameraInfo) -> None:
+        """重算并覆盖内参 —— gz 发的那份是错的，不能透传。
+
+        本机实测：gz 恒发 fx=fy=277, cx=160, cy=120（等于它的默认 60° 视场 +
+        半分辨率主点），而渲染用的是配置的 87° 视场与 640x480。
+        照抄它做三维反投影会错半米量级，且单目预览里完全看不出来。
+        详见 sim_cameras.intrinsics() 的说明。
+        """
+        w = int(msg.width) or int(s.width)
+        h = int(msg.height) or int(s.height)
+        try:
+            k, p = intrinsics(s, w, h)
+        except CameraConfigError as exc:
+            key = ("info", s.ros_ns)
+            if key not in self._warned:
+                self._warned.add(key)
+                self.get_logger().error(f"{s.ros_ns}: 内参算不出来，{s.ros_info} 无输出。{exc}")
+            return
+
+        key = ("info", s.ros_ns)
+        if key not in self._warned:
+            self._warned.add(key)
+            gz_k = list(msg.k) if len(msg.k) == 9 else []
+            self.get_logger().info(
+                f"{s.ros_ns}: 内参已按视场 {s.horizontal_fov:.4f} rad "
+                f"({math.degrees(s.horizontal_fov):.1f}°) 与 {w}x{h} 重算 —— "
+                f"fx={k[0]:.1f} cx={k[2]:.1f} cy={k[5]:.1f}"
+                + (
+                    f"（覆盖 gz 的 fx={gz_k[0]:.1f} cx={gz_k[2]:.1f} cy={gz_k[5]:.1f}）"
+                    if gz_k
+                    else ""
+                )
+            )
+
+        out = CameraInfo()
+        out.header = msg.header
+        out.header.frame_id = s.optical_frame
+        out.height = h
+        out.width = w
+        out.distortion_model = "plumb_bob"
+        # gz 不模拟镜头畸变，所以畸变系数全 0、R 为单位阵。
+        # 这意味着"仿真里用畸变矫正调出来的参数"在真机上仍需重调 —— 已在
+        # devices/sim/gazebo/README.md 里说明。
+        out.d = [0.0] * 5
+        out.k = list(k)
+        out.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+        out.p = list(p)
+        self._pubs[s.ros_info].publish(out)
+
+    def _on_depth(self, s: CameraSpec, msg: Image) -> None:
+        """32FC1 米 → 16UC1 毫米。
+
+        用 frombuffer 而不是 array(...)：后者会把整个 buffer 再拷一遍。
+        这一步每帧都在跑（30Hz × 640×480×4B），能省则省。
+        """
+        if msg.encoding != "32FC1":
+            # 只提醒一次，避免刷屏
+            key = (s.ros_ns, msg.encoding)
+            if key not in self._warned:
+                self._warned.add(key)
+                self.get_logger().warn(
+                    f"{s.gz_depth} 的编码是 {msg.encoding}，不是预期的 32FC1。"
+                    f"已跳过本次转换 —— 深度话题会没有输出。"
+                    f"若 Gazebo 版本改动了深度编码，需要同步改 sim_cameras."
+                    f"depth_m_to_mm16 的调用处。"
+                )
+            return
+
+        arr = np.frombuffer(msg.data, dtype=np.float32).reshape(msg.height, msg.width)
+        mm = depth_m_to_mm16(arr)
+
+        out = Image()
+        out.header = msg.header
+        out.header.frame_id = s.optical_frame
+        out.height = msg.height
+        out.width = msg.width
+        out.encoding = "16UC1"
+        # gz 给的是连续 buffer；转成 uint16 后每像素 2 字节，step 必须同步改，
+        # 否则下游按 RGB 的 stride 解读会错位。
+        out.is_bigendian = 0
+        out.step = msg.width * 2
+        out.data = mm.tobytes()
+        self._pubs[s.ros_depth].publish(out)
+
+    def _note_encoding(self, s: CameraSpec, msg: Image) -> None:
+        """第一次收到彩色帧时把编码打出来。
+
+        为什么值得单独打印：gz 给的是 rgb8（实测），而真机 realsense 的
+        color/image_raw 也是 rgb8 —— 一致，所以不需要转换。但这一点是实测
+        结论，不是保证；换个 Gazebo 版本或传感器配置可能变成 bgr8。
+        打一行日志，使"颜色不对"时能立刻定位到是不是编码变了。
+        """
+        key = ("enc", s.ros_ns, msg.encoding)
+        if key not in self._warned:
+            self._warned.add(key)
+            self.get_logger().info(
+                f"{s.ros_ns}: 彩色 {msg.width}x{msg.height} encoding={msg.encoding} "
+                f"→ {s.ros_color} (frame={s.optical_frame})"
+            )
+            if msg.encoding not in ("rgb8", "bgr8"):
+                self.get_logger().warn(
+                    f"{s.ros_ns} 的彩色编码是 {msg.encoding}，既不是 rgb8 也不是 "
+                    f"bgr8。应用按 rgb8 解释会得到错误的颜色。"
+                )
+
+
+def main(argv: Optional[List[str]] = None) -> int:
+    rclpy.init(args=argv)
+    try:
+        node = SimCameraAdapter()
+    except Exception:
+        # 参数/配置错误时 rclpy 已经打印了原因；这里只需保证退出码非零，
+        # 让 launch 能把它当成失败（否则会留下一个没有相机的"成功"启动）。
+        rclpy.try_shutdown()
+        return 2
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/sim_cameras.py

@@ -0,0 +1,532 @@
+"""仿真相机的配置解析与格式换算。
+
+=============================================================================
+为什么单独一个模块
+=============================================================================
+"把 Gazebo 的相机输出变成应用能直接吃的话题"这件事里，真正有内容的是三件事：
+
+    1. 深度单位换算  32FC1 米  →  16UC1 毫米（且 0 表示无读数）
+    2. 坐标系盖章    图像要盖**光学系**，不能盖相机本体系
+    3. 话题改名      /d405/image → /d405/color/image_raw 等
+
+这三件事全是纯数据变换，不需要 ROS 跑起来就能验。所以实现放在这里、
+用普通函数表达，节点（sim_camera_adapter.py）只做订阅/发布这层壳。
+好处是换算逻辑可以被单测钉死 —— 深度单位差 1000 倍这种错误，
+在单目预览里肉眼看不出任何异常，只能靠测试兜住。
+
+另外，启动文件里生成 ros_gz_bridge 参数也要用同一份相机清单，
+所以清单的解析也放在这里，避免"桥接用的清单"和"适配器用的清单"各写一份。
+"""
+from __future__ import annotations
+
+import math
+import re
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Dict, List, Optional, Sequence, Tuple
+
+import numpy as np
+import yaml
+
+# 默认配置位置（相对仓库根）。与其它 devices/ 下的配置一样，是**物料**而非代码。
+DEFAULT_CAMERAS_REL = "devices/sim/gazebo/config/cameras.yaml"
+
+# 相机本体系（X 前 / Y 左 / Z 上）→ 光学系（Z 前 / X 右 / Y 下）的固定旋转。
+# 这就是真机 realsense 的 <相机>_color_optical_frame 相对其 link 用的一组角，
+# 且是 ROS 的惯例（REP-103 / REP-105）。
+#
+# ⚠️ 这一步很容易被省掉，而且省掉之后**什么都不会报错**：
+#     应用拿 image 的 frame_id 去做 tf2 查询，本体系是存在的，查询成功；
+#     但那是"相机机身"的朝向，用它把深度反投影成三维点会整体转错。
+#     单目预览看起来完全正常，只有把点投回世界坐标时才露馅 —— 所以
+#     专门有一个端到端测试把"深度反投影后应落在已知平面"作为断言。
+OPTICAL_RPY: Tuple[float, float, float] = (-1.5708, 0.0, -1.5708)
+
+# uint16 能表示的最大毫米数（65535 mm = 65.535 m）。
+# 更远的读数按"无读数"处理（0），而不是饱和到 65535 ——
+# 饱和会让远处凭空出现一个 65.5m 的平面，比"无效"更有害。
+MAX_MM = 65535
+
+
+class CameraConfigError(RuntimeError):
+    """配置有问题时抛出，消息面向使用者。"""
+
+
+@dataclass(frozen=True)
+class CameraSpec:
+    """一台仿真相机的声明。"""
+
+    gz_ns: str
+    ros_ns: str
+    link_frame: str
+    optical_frame: str
+    depth: bool = True
+    horizontal_fov: float = 0.0
+    world_model: str = ""
+    world_link: str = "link"
+    width: int = 0
+    height: int = 0
+
+    # --- Gazebo 侧（ros_gz_bridge 的输入）-----------------------------------
+    @property
+    def gz_image(self) -> str:
+        return f"{self.gz_ns}/image"
+
+    @property
+    def gz_depth(self) -> str:
+        return f"{self.gz_ns}/depth_image"
+
+    @property
+    def gz_info(self) -> str:
+        return f"{self.gz_ns}/camera_info"
+
+    # --- ROS 侧（应用按这些名字订阅，与真机 realsense 一致）------------------
+    @property
+    def ros_color(self) -> str:
+        return f"/{self.ros_ns}/color/image_raw"
+
+    @property
+    def ros_info(self) -> str:
+        return f"/{self.ros_ns}/color/camera_info"
+
+    @property
+    def ros_depth(self) -> str:
+        return f"/{self.ros_ns}/aligned_depth_to_color/image_raw"
+
+    @property
+    def is_world_mounted(self) -> bool:
+        """挂在世界模型上（而非机器人本体）→ 需要额外补一条静态 TF。"""
+        return bool(self.world_model)
+
+
+def load_cameras(path: Optional[Path] = None, root: Optional[Path] = None) -> List[CameraSpec]:
+    """读相机清单。找不到或写错时抛 CameraConfigError，消息里给出修法。"""
+    if path is None:
+        base = root if root is not None else Path.cwd()
+        path = Path(base) / DEFAULT_CAMERAS_REL
+    path = Path(path)
+
+    if not path.is_file():
+        raise CameraConfigError(
+            f"相机清单不存在: {path}\n"
+            f"  它是仿真相机的话题/坐标系唯一来源（桥接与适配器都从它派生）。\n"
+            f"  缺了它仿真里的相机就完全没输出。确认仓库根下存在 "
+            f"{DEFAULT_CAMERAS_REL}。"
+        )
+
+    try:
+        doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
+    except yaml.YAMLError as exc:
+        raise CameraConfigError(f"相机清单 YAML 解析失败: {path}\n  原因: {exc}") from exc
+
+    raw = doc.get("cameras")
+    if not isinstance(raw, list) or not raw:
+        raise CameraConfigError(
+            f"{path} 里没有 cameras 列表（或为空）。至少写一台相机，否则仿真里没有图像。"
+        )
+
+    specs: List[CameraSpec] = []
+    seen: Dict[str, str] = {}
+    for i, item in enumerate(raw):
+        if not isinstance(item, dict):
+            raise CameraConfigError(f"{path} 的 cameras[{i}] 不是映射（键值对）。")
+
+        def need(key: str) -> str:
+            v = str(item.get(key, "") or "").strip()
+            if not v:
+                raise CameraConfigError(
+                    f"{path} 的 cameras[{i}] 缺少 {key}。"
+                    f"（gz_ns / ros_ns / link_frame / optical_frame 都必填）"
+                )
+            return v
+
+        # 必填键先查完，再查取值是否合理 —— 否则一份"缺键且键值也不对"的配置
+        # 会先报一个后者的错，让人以为只是值填错，实际键都还没写全。
+        gz_ns = "/" + need("gz_ns").strip("/")
+        ros_ns = need("ros_ns").strip("/")
+        link_frame = need("link_frame")
+        optical_frame = need("optical_frame")
+
+        # 同一个 ROS 命名空间出现两次 → 两台相机会互相覆盖话题，
+        # 表现为"只有一台出图"或图像在两个相机之间跳，很难查。直接拒绝。
+        if ros_ns in seen:
+            raise CameraConfigError(
+                f"{path}: ros_ns '{ros_ns}' 重复（cameras[{i}] 与 {seen[ros_ns]}）。\n"
+                f"  两台相机用同一个 ros_ns 会互相覆盖话题，必须区分开。"
+            )
+        seen[ros_ns] = f"cameras[{i}]"
+
+        try:
+            fov = float(item.get("horizontal_fov", 0.0) or 0.0)
+        except (TypeError, ValueError):
+            raise CameraConfigError(
+                f"{path} 的 cameras[{i}].horizontal_fov 不是数字。"
+            ) from None
+        # 视场是内参的唯一来源（gz 自己发的 camera_info 是错的，见文件头），
+        # 所以它必须存在且合理。填错的话三维点位会整体错，而且看不出来。
+        if not (0.0 < fov < math.pi):
+            raise CameraConfigError(
+                f"{path} 的 cameras[{i}].horizontal_fov = {fov} 不合理。\n"
+                f"  必须是 (0, π) 弧度之间的值，例如真机 D405 约 87° = 1.5184。\n"
+                f"  它决定内参 fx = fy = W/(2*tan(hfov/2))，填错会让反投影出的"
+                f"三维点整体偏移，而单目预览里完全看不出来。"
+            )
+
+        specs.append(
+            CameraSpec(
+                gz_ns=gz_ns,
+                ros_ns=ros_ns,
+                link_frame=link_frame,
+                optical_frame=optical_frame,
+                depth=bool(item.get("depth", True)),
+                horizontal_fov=fov,
+                world_model=str(item.get("world_model", "") or "").strip(),
+                world_link=str(item.get("world_link", "") or "link").strip() or "link",
+                width=int(item.get("width", 0) or 0),
+                height=int(item.get("height", 0) or 0),
+            )
+        )
+    return specs
+
+
+def intrinsics(spec: CameraSpec, width: int, height: int) -> Tuple[List[float], List[float]]:
+    """由视场与图像尺寸算出 (K, P)。返回扁平列表，可直接塞进 CameraInfo。
+
+    =========================================================================
+    为什么必须自己算，而不是用 gz 发的那份 camera_info
+    =========================================================================
+    本机实测（Fortress 6.16 / RoboStack）：gz 给 rgbd_camera 发的 camera_info
+    **恒为** fx=fy=277, cx=160, cy=120 —— 无论 SDF 里怎么配。这组值对应 gz 的
+    默认参数：60° 视场 + 320x240 图幅。
+
+    而它**渲染**用的却是 SDF 里配的 87° 视场与 640x480。证据是：用已知地面平面
+    对深度反投影做最小二乘，解出 fy=337.2、cy=239.5，与
+        640 / (2*tan(1.5184/2)) = 337.2      480/2 = 240
+    完全吻合（残差中位 0.2mm）。也就是说渲染是对的，只有 camera_info 是默认值。
+
+    后果有多严重：照抄那份 camera_info 反投影，D435 看到的地面会被算到
+    z ≈ -0.49m（真实是 0），偏差半米量级。而单目预览一切正常 ——
+    这类错误只有在用三维点位时才暴露，恰好是本项目装配任务的核心输入。
+
+    因此这里按"渲染实际用的视场"重算，并覆盖 gz 的 camera_info。
+    像素是方的（gz 由同一视场与宽高比推出两个方向），所以 fx = fy。
+
+    ⚠️ 相应地，horizontal_fov 必须与 SDF 里真正渲染用的值一致。
+       它有两个来源，启动时会自动比对并告警（check_fov_consistency）。
+    """
+    if not (0.0 < spec.horizontal_fov < math.pi):
+        raise CameraConfigError(
+            f"相机 {spec.ros_ns} 的 horizontal_fov={spec.horizontal_fov} 不合理，"
+            f"无法算内参。"
+        )
+    if width <= 0 or height <= 0:
+        raise CameraConfigError(
+            f"相机 {spec.ros_ns} 的图像尺寸非法（{width}x{height}），无法算内参。"
+        )
+
+    fx = width / (2.0 * math.tan(spec.horizontal_fov / 2.0))
+    fy = fx  # 方形像素
+    cx = width / 2.0
+    cy = height / 2.0
+
+    k = [fx, 0.0, cx,
+         0.0, fy, cy,
+         0.0, 0.0, 1.0]
+    # P 是 3x4 的投影矩阵。gz 不模拟畸变，所以 R 是单位阵、最后一列为 0。
+    p = [fx, 0.0, cx, 0.0,
+         0.0, fy, cy, 0.0,
+         0.0, 0.0, 1.0, 0.0]
+    return k, p
+
+
+def check_camera_consistency(
+    specs: Sequence[CameraSpec],
+    world_file: Optional[Path] = None,
+    sensors_xacro: Optional[Path] = None,
+) -> List[str]:
+    """比对 cameras.yaml 的声明与 SDF 里真正渲染用的值（视场 + 图幅）。
+
+    为什么值得做这个检查：内参由 cameras.yaml 的 horizontal_fov 与 width/height
+    算出，而**渲染**用的是 SDF 里的值。两者一旦不一致，图像与内参就对不上了 ——
+    症状仍然是"三维点整体错"，而单目预览看不出来。这正是这个模块开头
+    那一长串说明所警告的那类错误，所以要主动比对。
+
+    ⚠️ 图幅（width/height）为什么必须一起比：它决定 fx = W/(2·tan(hfov/2))，
+    也就是"每毫米多少像素"。阶段二的孔检测在原始分辨率图上跑 HoughCircles，
+    hole_detect.yaml 的像素门槛（hole_distance_min 等）只在图幅与真机相同时
+    才有意义。图幅写错不会报任何错，只会让孔检测"莫名其妙不认孔" ——
+    比视场写错更难查（视场错还会让点位整体偏，图幅错只是尺度不对）。
+
+    返回人类可读的告警列表（空 = 没问题）。
+
+    精度说明（有意为之，不做过度解析）：
+      - 世界相机：按 <sensor> 里的 <topic> 精确匹配到对应相机，读到的是精确值。
+      - 腕上相机：它在 xacro 宏里、值由参数传入，展开前无法可靠地归属到具体
+        相机。所以视场退化成"文件里出现过这个值就算过"，图幅则读宏的
+        **参数默认值**（image_width:=/image_height:=，launch 不会覆盖它）——
+        偏宽松，但足以抓住"改了 SDF 忘了改 cameras.yaml"这种最常见的漂移。
+    """
+    warnings: List[str] = []
+
+    if world_file is not None and Path(world_file).is_file():
+        text = Path(world_file).read_text(encoding="utf-8", errors="replace")
+        for s in specs:
+            if not s.is_world_mounted:
+                continue
+            # 找到 <topic> 等于本相机 gz_ns 的那个 <sensor> 块
+            fov = None
+            size = (None, None)
+            for m in re.finditer(r"<sensor\b[^>]*>(.*?)</sensor>", text, re.S):
+                body = m.group(1)
+                tm = re.search(r"<topic>\s*([^<\s]+)\s*</topic>", body)
+                if not tm or "/" + tm.group(1).strip().lstrip("/") != s.gz_ns:
+                    continue
+                fm = re.search(r"<horizontal_fov>\s*([0-9.eE+-]+)\s*</horizontal_fov>", body)
+                if fm:
+                    fov = float(fm.group(1))
+                wm = re.search(r"<width>\s*(\d+)\s*</width>", body)
+                hm = re.search(r"<height>\s*(\d+)\s*</height>", body)
+                size = (
+                    int(wm.group(1)) if wm else None,
+                    int(hm.group(1)) if hm else None,
+                )
+                break
+            if fov is None:
+                warnings.append(
+                    f"相机 {s.ros_ns}: 在 {Path(world_file).name} 里找不到 topic 为 "
+                    f"{s.gz_ns} 的 <sensor> 的 <horizontal_fov>，无法比对。"
+                    f"内参将按 cameras.yaml 的 {s.horizontal_fov} 计算 —— "
+                    f"若与渲染实际用的视场不符，三维点位会整体错。"
+                )
+            elif abs(fov - s.horizontal_fov) > 1e-4:
+                warnings.append(
+                    f"相机 {s.ros_ns}: 视场不一致！cameras.yaml 写 {s.horizontal_fov}，"
+                    f"而 {Path(world_file).name} 里渲染用的是 {fov}。\n"
+                    f"    内参按前者算、图像按后者渲染 → 三维点位会整体错，"
+                    f"而单目预览看不出来。请把两处改成一致。"
+                )
+            _check_size(s, size[0], size[1], Path(world_file).name, warnings)
+
+    if sensors_xacro is not None and Path(sensors_xacro).is_file():
+        text = Path(sensors_xacro).read_text(encoding="utf-8", errors="replace")
+        found = [float(v) for v in re.findall(r"<horizontal_fov>\s*([0-9.eE+-]+)", text)]
+        for s in specs:
+            if s.is_world_mounted:
+                continue
+            if not found:
+                warnings.append(
+                    f"相机 {s.ros_ns}: 在 {Path(sensors_xacro).name} 里没找到 "
+                    f"<horizontal_fov>，无法比对（内参按 cameras.yaml 的 "
+                    f"{s.horizontal_fov} 计算）。"
+                )
+            elif not any(abs(v - s.horizontal_fov) <= 1e-4 for v in found):
+                warnings.append(
+                    f"相机 {s.ros_ns}: 视场可能不一致。cameras.yaml 写 "
+                    f"{s.horizontal_fov}，而 {Path(sensors_xacro).name} 里出现的值是 "
+                    f"{found}。若渲染用的不是 {s.horizontal_fov}，三维点位会整体错。"
+                )
+        # 腕上相机的图幅：读宏的参数默认值（launch 不覆盖它们）
+        mw = re.search(r"image_width:=(\d+)", text)
+        mh = re.search(r"image_height:=(\d+)", text)
+        for s in specs:
+            if s.is_world_mounted:
+                continue
+            _check_size(
+                s,
+                int(mw.group(1)) if mw else None,
+                int(mh.group(1)) if mh else None,
+                Path(sensors_xacro).name,
+                warnings,
+            )
+
+    return warnings
+
+
+def _check_size(
+    spec: "CameraSpec",
+    width: Optional[int],
+    height: Optional[int],
+    where: str,
+    warnings: List[str],
+) -> None:
+    """比对声明图幅与实际渲染图幅，不一致就追加一条告警。
+
+    单独抽出来是因为世界相机与腕上相机的"值从哪读"完全不同（前者读 <sensor>，
+    后者读 xacro 宏默认值），但**比对逻辑与文案必须一致** —— 两处各写一遍
+    迟早会漂移成两套说法。
+
+    ``spec`` 未声明图幅（width/height <= 0）时直接跳过：那表示"这份清单不管尺寸"，
+    此时 intrinsics() 会退回 640x480。没有声明就没有"不一致"可言，
+    报出来只会变成噪声，反而让人忽略真正的告警。
+    """
+    if spec.width <= 0 or spec.height <= 0:
+        return
+    if width is None or height is None:
+        warnings.append(
+            f"相机 {spec.ros_ns}: 在 {where} 里读不到图像尺寸（<width>/<height> 或 "
+            f"image_width:=），无法比对。cameras.yaml 声明 "
+            f"{spec.width}x{spec.height} —— 若渲染的实际尺寸与它不同，"
+            f"内参算出的每毫米像素数就是错的，孔检测的像素门槛会静默失效。"
+        )
+        return
+    if (width, height) != (spec.width, spec.height):
+        warnings.append(
+            f"相机 {spec.ros_ns}: 图像尺寸不一致！cameras.yaml 写 "
+            f"{spec.width}x{spec.height}，而 {where} 里渲染用的是 {width}x{height}。\n"
+            f"    内参按前者算、图像按后者渲染 → fx 的尺度不对（差 "
+            f"{width / max(spec.width, 1):.3f} 倍），"
+            f"所有以像素为单位的门槛（如 hole_distance_min）都会静默失效。\n"
+            f"    注意：真机 D405 是 848x480、D435 是 1280x720；仿真侧要对齐 D405。"
+        )
+
+
+def bridge_args(specs: Sequence[CameraSpec]) -> List[str]:
+    """生成 ros_gz_bridge parameter_bridge 的话题规则。
+
+    只做**传输**：gz 消息类型 → ROS 消息类型，话题名保持 gz 侧的原样。
+    改名与格式换算交给适配器 —— 分开的原因是桥接器改不了 frame_id，
+    而 frame_id 恰恰是最容易错又最不报错的那一项。让适配器统一处理
+    "名字 + 坐标系 + 单位"，逻辑集中在一处。
+
+    注：/d405/image 这类"gz 原名"话题在 ROS 图里也会可见。
+    这是有意保留的：排查"到底是桥接断了还是适配器断了"时，
+    直接 echo 原名话题能一刀切开。
+    """
+    args: List[str] = []
+    for s in specs:
+        args.append(f"{s.gz_image}@sensor_msgs/msg/Image[gz.msgs.Image")
+        args.append(f"{s.gz_info}@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo")
+        if s.depth:
+            # gz 的深度是 float32（本机实测 4 字节/像素），桥接器支持为 32FC1。
+            # 若哪天桥接器不再支持，这里会静默少一个话题 —— 验收时用
+            # `ros2 topic list | grep aligned_depth` 确认它在。
+            args.append(f"{s.gz_depth}@sensor_msgs/msg/Image[gz.msgs.Image")
+    return args
+
+
+def depth_m_to_mm16(depth_m: np.ndarray, max_mm: int = MAX_MM) -> np.ndarray:
+    """深度：32FC1 米 → 16UC1 毫米。0 表示"无读数"（与真机 realsense 一致）。
+
+    规则（每一条都对应一种真机会出现的情况）：
+        非有限值（nan / inf）      → 0。gz 在没命中时可能给 inf，
+                                      真机 realsense 给 0；统一成 0。
+        <= 0                      → 0。真机对无效像素就是给 0，
+                                      负深度没有物理意义。
+        0 < d <= max_mm/1000      → round(d*1000)
+        > max_mm/1000（65.535m）  → 0（见 MAX_MM 的说明：不饱和）
+        舍入后为 0 的极小正数      → 0（例如 0.0001m，落不进 uint16）
+
+    返回与输入同形状的 uint16 数组。
+    """
+    a = np.asarray(depth_m)
+    if a.dtype != np.float32:
+        a = a.astype(np.float32, copy=False)
+
+    out = np.zeros(a.shape, dtype=np.uint16)
+
+    finite = np.isfinite(a)
+    if not finite.any():
+        return out
+
+    # 只对有限且为正的像素做换算；先把它们挑出来，避免对 nan 做比较
+    # （nan 参与的比较全是 False，但显式挑出来意图更清楚，也少一遍遍历）
+    cand = finite & (a > 0.0)
+    if not cand.any():
+        return out
+
+    mm = np.rint(a[cand].astype(np.float64) * 1000.0)
+    ok = (mm > 0.0) & (mm <= float(max_mm))
+    if ok.any():
+        out[cand] = np.where(ok, mm, 0.0).astype(np.uint16)
+    return out
+
+
+#: 腕部相机的视轴与法兰轴线的允许夹角 [度]。
+#: 真机装支架时也不会完全正装，但"看得到工件"要求它基本沿法兰轴；
+#: 20° 是给手工装配留的余量，同时足以把"少转 90°"这类错误拦下来。
+MOUNT_TOL_DEG = 20.0
+
+#: 手外相机的视轴在世界系里的 z 分量上限（-1 = 正朝下）。松于 -0.5 就算没在朝下看。
+WORLD_LOOK_DOWN_COS = -0.5
+
+
+def flange_frame_for(spec: CameraSpec) -> Optional[str]:
+    """从相机的 link_frame 反推它所在臂的法兰 frame：sim_d405_link → sim_flange。
+
+    只对**挂在臂上**的相机有意义（``world_model`` 非空的是世界相机，返回 None）。
+    为什么要反推而不是读配置：``site.yaml`` 的 ``sim.joint_prefix`` 自己都写着
+    "✘ 未接线"，拿一个没接线的值当路径判据会静默失效。
+    link_frame 里的前缀才是真正生效的那个（由 xacro 的 arm_prefix 生成）。
+    """
+    if spec.world_model:
+        return None
+    model = spec.ros_ns.rsplit("_", 1)[-1]          # sim_d405 → d405
+    suffix = f"{model}_link"
+    if not spec.link_frame.endswith(suffix):
+        return None
+    return spec.link_frame[: -len(suffix)] + "flange"
+
+
+def mount_axis_deg(R_flange_optical: np.ndarray) -> float:
+    """光学系视轴（+Z）与**法兰 +Z**的夹角 [度]。0 = 正装在法兰轴线上。
+
+    这是个可以**脱离姿态**核对的量，所以特别有用：不管机械臂当时摆成什么样，
+    "相机有没有沿法兰轴线装"都只在安装处决定。
+
+    为什么它必须接近 0（而不是"调多少都行"）：Gz 的相机视轴是模型/连杆的 +X，
+    而 OPTICAL_RPY(-90°,0,-90°) 把"机身系（X 前/Y 左/Z 上）"转成光学系。
+    两者串起来 ⇒ 相机沿法兰轴线装 ⇔ 光学 +Z ∥ 法兰 +Z。
+    少转那 90° 时：话题有数据、预览有图像、内参也对，只是整个视野搬到了侧面，
+    应用侧表现成"目标永远不在相机前方"（曾真实踩过，见 sensors.xacro 的说明）。
+    """
+    v = np.asarray(R_flange_optical, dtype=float) @ np.array([0.0, 0.0, 1.0])
+    return float(np.degrees(np.arccos(float(np.clip(v[2], -1.0, 1.0)))))
+
+
+def world_look_down_cos(R_world_optical: np.ndarray) -> float:
+    """光学系视轴在世界系里的 z 分量（-1 = 正朝下看，0 = 平视）。"""
+    return float((np.asarray(R_world_optical, dtype=float) @ np.array([0.0, 0.0, 1.0]))[2])
+
+
+def rpy_to_R(rpy: Sequence[float]) -> np.ndarray:
+    """URDF/SDF 的 (roll, pitch, yaw) → 3x3 旋转矩阵。
+
+    复合顺序是 ``R = Rz(yaw)·Ry(pitch)·Rx(roll)``（URDF 规范）—— 顺序不能想当然，
+    反了在"只有一个角非零"时看不出来，两个角同时非零就会错得莫名其妙。
+    """
+    r, p, y = (float(v) for v in rpy)
+    cr, sr = math.cos(r), math.sin(r)
+    cp, sp = math.cos(p), math.sin(p)
+    cy, sy = math.cos(y), math.sin(y)
+    Rx = np.array([[1.0, 0.0, 0.0], [0.0, cr, -sr], [0.0, sr, cr]])
+    Ry = np.array([[cp, 0.0, sp], [0.0, 1.0, 0.0], [-sp, 0.0, cp]])
+    Rz = np.array([[cy, -sy, 0.0], [sy, cy, 0.0], [0.0, 0.0, 1.0]])
+    return Rz @ Ry @ Rx
+
+
+def mount_rpy_axis_deg(rpy: Sequence[float]) -> float:
+    """腕相机的**安装 rpy**（URDF 里那个 fixed joint 的 rpy）→ 视轴与法兰 +Z 的夹角。
+
+    与运行期查 TF 的 :func:`mount_axis_deg` 是同一件事，只是输入不同：
+    没接 TF 时（比如核对 xacro 里的默认值）用这个算。
+    """
+    return mount_axis_deg(rpy_to_R(rpy) @ rpy_to_R(OPTICAL_RPY))
+
+
+def optical_rpy() -> Tuple[float, float, float]:
+    """本体 → 光学系的固定旋转，供静态 TF 使用。"""
+    return OPTICAL_RPY
+
+
+def rpy_to_quat(rpy: Sequence[float]) -> Tuple[float, float, float, float]:
+    """(roll, pitch, yaw) → (x, y, z, w)。只用于发布静态 TF。"""
+    r, p, y = (float(v) for v in rpy)
+    cr, sr = math.cos(r / 2.0), math.sin(r / 2.0)
+    cp, sp = math.cos(p / 2.0), math.sin(p / 2.0)
+    cy, sy = math.cos(y / 2.0), math.sin(y / 2.0)
+    return (
+        sr * cp * cy - cr * sp * sy,
+        cr * sp * cy + sr * cp * sy,
+        cr * cp * sy - sr * sp * cy,
+        cr * cp * cy + sr * sp * sy,
+    )

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/sim_grasp_lock.py

@@ -0,0 +1,329 @@
+"""仿真"抓取锁"（grasp lock）：让夹爪夹住的物体在仿真里真的不会滑掉。
+
+=============================================================================
+它在链路上的位置
+=============================================================================
+    真机：夹爪 46N 握力 + 指垫摩擦 → 工件被"抱住"，机械臂怎么动都不掉。
+    仿真：夹持力 = 位置误差 × 接触刚度（见 gauge/model.sdf 的 kp 说明）。
+         规站着不动时够用；一旦 S01 绕工具轴快转 150°、或 pre_lift 走 60~100mm
+         的连续轨迹，规就在指间打滑（实测工具长度 0.243m → 0.285m、歪 62°），
+         S07 随后拿到的几何全错 —— 现象是"判不到接触"或"规掉了(E1)"。
+
+    本模块做的唯一一件事：**在夹爪确认夹住时，用 DetachableJoint 插件把物体
+    与手指用一个固定关节锁起来；夹爪张开时解锁。**
+
+    这不是"作弊绕过物理"：刚性接触求解器给不出静摩擦迟滞，也不模拟指垫橡胶
+    的贴合，而真实夹持恰恰靠这两件事。锁补的就是这一块。物体与指垫之间的
+    **接触对仍然存在**，所以触觉、腕部 FT 的读数照旧是真实的接触力。
+
+=============================================================================
+为什么用 `ign topic` 命令行而不是 ros_gz_bridge
+=============================================================================
+    触发消息是 `ignition.msgs.Empty`。bridging 一个"空消息"要走
+    ros_gz_bridge 的 Empty 映射，而这条链路上**没有任何东西需要它出现在 ROS
+    侧** —— 唯一的目的就是把 gz 的那个插件叫一下。多接一个 bridge 就多一个
+    "配错了不报错"的点（bridge 类型名对不上时它只是安静地什么也不桥）。
+
+    代价是每次触发起一个短命进程（几十毫秒）。触发频率是"每次夹/放一次"，
+    不是每个控制周期 —— 这个代价可以忽略。
+
+=============================================================================
+静默失效的四个点（改配置时全都要对上）
+=============================================================================
+    ① <child_model>  = 机器人 spawn 的 -name（launch 里 rizon_sim）
+    ② <child_link>   = Gazebo 里**真实存在**的连杆名（URDF 里与固定关节相连的
+                       连杆会被 sdformat 合并，手指只剩 sim_left_finger_mount）
+    ③ attach/detach 话题名与 gauge/model.sdf 里的插件一致
+    ④ 插件确实写进了 gauge 的 SDF
+
+    ①②③④ 全都不会自己报错：锁不触发时，现象只是"规莫名掉件"。
+    所以 :func:`check_grasp_lock_sdf` 把它变成启动时的一条告警/报错。
+"""
+
+from __future__ import annotations
+
+import subprocess
+from dataclasses import dataclass
+from pathlib import Path
+from typing import List, Optional
+
+import yaml
+
+#: 相对仓库根的默认配置路径。
+DEFAULT_GRASP_LOCK_REL = "devices/sim/gazebo/config/grasp_lock.yaml"
+
+#: 被 lock 的物体模型 SDF 的默认位置（用来做静态核对）。
+DEFAULT_OBJECT_SDF_TMPL = "devices/sim/gazebo/models/{object}/model.sdf"
+
+
+class GraspLockError(RuntimeError):
+    """抓取锁配置缺失或写错。消息里必须给出修法（现场就靠这句话）。"""
+
+
+@dataclass(frozen=True)
+class GraspLockSpec:
+    """抓取锁的一份完整描述。"""
+
+    enabled: bool
+    object: str
+    object_link: str
+    robot_model: str
+    robot_link: str
+    attach_topic: str
+    detach_topic: str
+    state_topic: str
+    publish_timeout_ms: int
+
+    def describe(self) -> str:
+        """给启动日志用的一句话。"""
+        if not self.enabled:
+            return "抓取锁：关闭（仿真靠摩擦夹持，机械臂快速运动时可能滑件）"
+        return (
+            f"抓取锁：开 → {self.object}.{self.object_link} 锁到 "
+            f"{self.robot_model}.{self.robot_link}"
+            f"（attach={self.attach_topic} detach={self.detach_topic}）"
+        )
+
+
+def load_grasp_lock(
+    path: Optional[Path] = None, root: Optional[Path] = None
+) -> GraspLockSpec:
+    """读抓取锁配置。找不到就返回一份"关闭"的默认值（仿真仍能跑）。
+
+    **为什么文件缺失不报错**：抓取锁是仿真里的**补救措施**，不是必需品 ——
+    关掉它只是回到"靠摩擦夹持"，多数轻动作仍然能跑。让仿真因为一个可选补救
+    措施起不来，是拿主流程给可选项陪葬。
+    但**文件存在而字段写错**时必须报错：那说明有人动过它、而且写错了。
+    """
+    if path is None:
+        base = root if root is not None else Path.cwd()
+        path = Path(base) / DEFAULT_GRASP_LOCK_REL
+    path = Path(path)
+
+    if not path.is_file():
+        return GraspLockSpec(
+            enabled=False,
+            object="gauge",
+            object_link="body",
+            robot_model="",
+            robot_link="",
+            attach_topic="/sim/grasp_lock/attach",
+            detach_topic="/sim/grasp_lock/detach",
+            state_topic="/sim/grasp_lock/state",
+            publish_timeout_ms=3000,
+        )
+
+    try:
+        doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
+    except yaml.YAMLError as exc:
+        raise GraspLockError(
+            f"抓取锁配置 YAML 解析失败: {path}\n  原因: {exc}"
+        ) from exc
+
+    if not isinstance(doc, dict):
+        raise GraspLockError(f"{path} 的顶层应该是映射（key: value 形式）。")
+
+    enabled = bool(doc.get("enabled", True))
+
+    def _text(key: str, default: str = "") -> str:
+        return str(doc.get(key, default) or "").strip()
+
+    spec = GraspLockSpec(
+        enabled=enabled,
+        object=_text("object", "gauge") or "gauge",
+        object_link=_text("object_link", "body") or "body",
+        robot_model=_text("robot_model"),
+        robot_link=_text("robot_link"),
+        attach_topic=_text("attach_topic", "/sim/grasp_lock/attach"),
+        detach_topic=_text("detach_topic", "/sim/grasp_lock/detach"),
+        state_topic=_text("state_topic", "/sim/grasp_lock/state"),
+        publish_timeout_ms=int(doc.get("publish_timeout_ms", 3000)),
+    )
+
+    if not enabled:
+        return spec
+
+    missing = [
+        k
+        for k in ("robot_model", "robot_link", "attach_topic", "detach_topic")
+        if not getattr(spec, k)
+    ]
+    if missing:
+        raise GraspLockError(
+            f"{path} 开着（enabled: true）但缺字段: {', '.join(missing)}。\n"
+            f"  robot_model 必须等于 launch 里 spawn 机器人的 -name（当前 rizon_sim）；\n"
+            f"  robot_link 必须是 Gazebo 里的**真名**（与固定关节相连的连杆会被\n"
+            f"  sdformat 合并，手指只剩 sim_left_finger_mount）。真名查法：\n"
+            f"      ros2 topic echo /sim/world_pose_info   ← 看全部 child_frame_id\n"
+            f"  这两个名字写错的后果是**静默失效**：锁不触发、规照样滑掉，\n"
+            f"  而日志里只看得到「规掉了」。"
+        )
+    if spec.attach_topic == spec.detach_topic:
+        raise GraspLockError(
+            f"{path} 的 attach_topic 与 detach_topic 相同（{spec.attach_topic}）——\n"
+            f"  那样夹住和放开会走同一条消息，锁的状态完全随机。两个必须不同。"
+        )
+    # 锚点不能是"会跟着张爪动的"那根手指 —— 这是 2026-09-24 用一条假 E1 换来的。
+    if "finger" in spec.robot_link.lower():
+        raise GraspLockError(
+            f"{path} 的 robot_link 是 {spec.robot_link} —— **手指**。\n"
+            f"  被锁的工件会被刚性固定在手指上，而手指会随指令张/合：\n"
+            f"    · S08 棘轮每轮都要\"松爪 → 回转 → 夹紧\"，张爪时手指外移几十毫米，\n"
+            f"      而工件（规）还插在孔里 —— 两处约束打架，物理解算会把工件\n"
+            f"      **从孔里拽出来**（实测：规尖跳 +17.5mm、偏摆 64°），下一步掉地。\n"
+            f"      日志里只看得到「通止规掉了(E1)」，看不出真因。\n"
+            f"  修法：填**不跟着张爪动**的连杆。仿真是法兰 `sim_link7`\n"
+            f"  （夹爪本体 sim_grav_base_link 在 sdformat 里被合并进它了）。\n"
+            f"  真名查法：ros2 topic echo /sim/world_pose_info  ← 列全部 child_frame_id"
+        )
+    if spec.publish_timeout_ms <= 0:
+        raise GraspLockError(
+            f"{path} 的 publish_timeout_ms 必须为正，收到 {spec.publish_timeout_ms}"
+        )
+    return spec
+
+
+def bridge_target_links(spec: GraspLockSpec) -> List[str]:
+    """把"锁的两端"整理成一条便于核对/记录的清单（给日志与自检用）。"""
+    return [f"{spec.object}.{spec.object_link}", f"{spec.robot_model}.{spec.robot_link}"]
+
+
+def check_grasp_lock_sdf(
+    sdf_text: str, spec: GraspLockSpec, sdf_label: str = ""
+) -> List[str]:
+    """静态核对：物体 SDF 里的 DetachableJoint 插件是否与本配置一致。
+
+    为什么值得一查（与腕部 FT 的 check 同一个理由）：
+        插件参数写错时 Gazebo **什么都不说** —— 插件照样加载（只要 child_model
+        找得到），assess 话题照样发，只是关节连不上。现象只有一个：
+        "规莫名其妙掉了"，而那要等到 S07/S08 才显形、且看不到真因。
+        这里在**启动时**把它变成一句话。
+
+    返回告警列表（空 = 没问题）；enabled=false 时永远返回空。
+    """
+    if not spec.enabled:
+        return []
+    where = f"（{sdf_label}）" if sdf_label else ""
+    warnings: List[str] = []
+    if not sdf_text:
+        return [f"读不到物体 SDF{where}，无法核对抓取锁插件"]
+
+    if "DetachableJoint" not in sdf_text:
+        warnings.append(
+            f"物体 SDF{where} 里没有 DetachableJoint 插件 —— 抓取锁不会生效。\n"
+            f"  配置说 enabled: true，但模型里没有插件时**不会报错**，\n"
+            f"  只是夹住的物体依旧会滑（现象＝「规莫名掉了」，看不到真因）。\n"
+            f"  修法：把插件块补进 {spec.object}/model.sdf，参数照 grasp_lock.yaml。"
+        )
+        return warnings
+
+    if spec.robot_model and f"<child_model>{spec.robot_model}</child_model>" not in sdf_text:
+        warnings.append(
+            f"插件里的 <child_model> 不是 {spec.robot_model} —— "
+            f"它必须等于 launch 里 spawn 机器人的 -name。\n"
+            f"  对不上时 DetachableJoint 在 Configure 阶段直接失败，"
+            f"插件被卸载、{spec.state_topic} 也不会出现（这是唯一的线索）。"
+        )
+
+    if spec.robot_link and f"<child_link>{spec.robot_link}</child_link>" not in sdf_text:
+        warnings.append(
+            f"插件里的 <child_link> 不是 {spec.robot_link}。\n"
+            f"  注意这里要填 Gazebo 的**真名**：URDF 里与固定关节相连的连杆会被\n"
+            f"  sdformat 合并（lump），例如手指只剩 sim_left_finger_mount。\n"
+            f"  名字写错时插件照样加载、attach 照样「成功」，但关节连到不存在的\n"
+            f"  连杆上 → 锁不生效。真名查法：ros2 topic echo /sim/world_pose_info"
+        )
+
+    for topic, key in ((spec.attach_topic, "attach_topic"), (spec.detach_topic, "detach_topic")):
+        if topic and f"<{key}>{topic}</{key}>" not in sdf_text:
+            warnings.append(
+                f"插件里的 <{key}> 不是 {topic}。\n"
+                f"  两侧话题名不一致时，锁的触发消息发到一个没人听的地址上 ——\n"
+                f"  不报错，只是永远不锁。"
+            )
+
+    return warnings
+
+
+def _ign_topic_cmd(exe: str, topic: str, timeout_ms: int) -> List[str]:
+    """拼一条 `ign topic` 发布命令（空消息）。
+
+    ⚠️ ``-d``（duration 秒）不是可选的装饰 —— 少了它**大概率丢消息**：
+        CLI 建立 publisher 与网络里发现订阅者需要一点时间，一条 fire-and-forget
+        的消息在发现完成前就发完了。曾经写成 ``-w <秒>``（以为它等订阅者），
+        而这个版本的 ``ign topic`` **没有 -w** —— 参数被当成多余的位置参数：
+            "The following arguments were not expected: 3"
+        于是"上锁/解锁"永远失败，而现象和"锁没生效"完全一样（规照样掉）。
+    """
+    return [
+        exe,
+        "topic",
+        "-t", topic,
+        "-m", "ignition.msgs.Empty",
+        "-p", "",
+        "-d", f"{max(0.5, timeout_ms / 1000.0):.2f}",
+    ]
+
+
+def _find_ign_exe(explicit: Optional[str] = None) -> Optional[str]:
+    """找到 ign / gz 命令行。找不到返回 None（调用方决定是告警还是忽略）。"""
+    import shutil
+
+    if explicit:
+        return explicit
+    for name in ("ign", "gz"):
+        found = shutil.which(name)
+        if found:
+            return found
+    return None
+
+
+def publish_lock_event(
+    spec: GraspLockSpec,
+    attach: bool,
+    exe: Optional[str] = None,
+    logger=None,
+) -> bool:
+    """发一条"上锁/解锁"消息。返回是否成功（失败只记日志，不抛）。
+
+    **为什么不抛异常**：这不是主流程的一步 —— 它是仿真里的补救措施。
+    发不出去时正确行为是"继续跑，但明确告警"，而不是把整条装配流程打断：
+    现场宁可看到"锁没生效、规可能滑"的提示，也不要看到"因为锁发不出去，
+    装配根本没做"。
+    """
+    if not spec.enabled:
+        return False
+    topic = spec.attach_topic if attach else spec.detach_topic
+    what = "上锁" if attach else "解锁"
+    exe_path = _find_ign_exe(exe)
+    if exe_path is None:
+        if logger is not None:
+            logger.warn(
+                f"抓取锁{what}失败：找不到 ign/gz 命令行（PATH 里没有）。\n"
+                f"  后果：夹住的物体在机械臂连续运动时可能滑掉（现象＝「规掉了」）。"
+            )
+        return False
+
+    cmd = _ign_topic_cmd(exe_path, topic, spec.publish_timeout_ms)
+    try:
+        proc = subprocess.run(
+            cmd,
+            stdout=subprocess.PIPE,
+            stderr=subprocess.STDOUT,
+            timeout=max(2.0, spec.publish_timeout_ms / 1000.0 + 2.0),
+        )
+    except (OSError, subprocess.SubprocessError) as exc:
+        if logger is not None:
+            logger.warn(f"抓取锁{what}失败（{topic}）：{exc}")
+        return False
+
+    if proc.returncode != 0:
+        out = (proc.stdout or b"").decode("utf-8", "replace").strip()
+        if logger is not None:
+            logger.warn(
+                f"抓取锁{what}失败（{topic}，退出码 {proc.returncode}）：{out[:200]}"
+            )
+        return False
+    if logger is not None:
+        logger.info(f"抓取锁{what}：{topic}")
+    return True

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/sim_gripper.py

@@ -0,0 +1,618 @@
+"""仿真夹爪的耦合关系、行程换算与命令向量拼装（纯逻辑，不起 ROS）。
+
+=============================================================================
+为什么单独一个模块
+=============================================================================
+把"夹爪服务（mm）"变成"7 个关节的目标角（弧度/米）"，真正有内容的是四件事：
+
+    1. 耦合关系   从动关节 = multiplier × 主关节 + offset
+                   （上游 GN01 描述里主关节是 ``finger_width_joint``，
+                    6 个从动关节按各自的 multiplier 跟随）
+    2. 行程换算   应用层说的是**开口毫米**，Gazebo 要的是**关节值**（米）
+    3. 命令顺序   JointGroupPositionController 收到的数组顺序必须与
+                   controllers.yaml 里 ``joints:`` 完全一致
+    4. 夹住判据   真机有状态字，仿真只能靠"指令到了下限、实际值没跟上"推断
+
+四件都是纯数据变换，不需要 ROS 跑起来就能验。所以实现放这里、用普通函数表达，
+节点（sim_gripper_adapter.py）只做订阅/发布这层壳。这样"multiplier 用错"
+这种错误才能被单测钉住，而不是靠"看起来夹住了"。
+
+=============================================================================
+为什么 Gazebo 里必须由适配器驱动 7 个关节
+=============================================================================
+sdformat 把 URDF 的 ``<mimic>`` **整个丢掉**，gz-sim 也不实现 mimic。
+所以仿真里 6 个从动关节不会跟随主关节，必须由适配器算出角度后**一起下发**。
+``urdf_normalize`` 在归一化时把 ``<mimic>`` 删掉（避免 robot_state_publisher
+和 Gazebo 各说各话），删掉的信息记在 ``NormalizeReport.stripped_mimics`` 里，
+launch 再把那张表作为参数交给本适配器 —— 这样"耦合关系"只有一个来源。
+
+=============================================================================
+毫米 ↔ 关节值的口径（最容易搞错的一处）
+=============================================================================
+``finger_width_joint`` 是上游描述里的**虚拟宽度关节**（注释说它"本应是
+prismatic，但为了配合 mimic 只能是 revolute"），单位是**米**，限位 **0~0.100**，
+语义就是开口宽度：0 = 两指合拢，0.100 = 开口 100 mm。
+
+本适配器采用**绝对（物理）映射**：命令"开口 8 mm"，仿真里两指就真的相距 8 mm。
+（而不是归一化映射 —— 那种把"100% 行程"当成全开，会让中间开口全部失真，
+仿真就失去"拿真几何试算法"的意义。）
+
+⚠️ **但这里有个坑：不能简单地写成 `关节值 = 开口_mm / 1000`。**
+上游的耦合是**线性近似**，而机构本身是非线性的（手指是"转 + 再转"，指垫
+既平移又旋转）。按线性口径命令 20 mm，Gazebo 实测两指相距 **21.75 mm** ——
+抓 20 mm 的工件会空出 1.75 mm，没有接触力，触觉那一步在仿真里直接废掉。
+
+所以开口↔关节值走**URDF 正运动学反解**（见 sim_gripper_kin.py）：
+
+    aperture_to_source(20.0) → 令两指间距 − 合拢间距 == 20.0 的关节值
+
+这个正运动学与 Gazebo 的实测吻合到 0.01 mm（0~100 mm 全程），两边完全独立
+（一个是矩阵乘法，一个是物理引擎），所以"开口 20 mm"在仿真里是物理上真的 20 mm。
+顺带一个推论：GN01 的**实测**行程是 99.93 mm，不是名义的 100.0 ——
+启动日志里报的是实测值。
+
+万一某个夹爪的指尖 link 命名不符合约定，正运动学建不出来，会**退回线性近似**
+并在启动日志里明确写出口径变了（中段偏差可达 4 mm/100mm 行程）。
+别在那种情况下拿仿真去标触觉阈值。
+
+⚠️ 还要注意**仿真夹爪和真机夹爪不是同一个夹爪**：仿真是官方 GN01（行程
+    100 mm），现场是 DH PGEA-100-20-W（``site.yaml`` 的 ``gripper.stroke_mm``
+    = 20 mm）。同一个"8 mm 开口"在两者上都物理正确，但"全开"对应的开口
+    完全不同。适配器启动时会把两者的行程打出来，避免有人默认它们一样。
+"""
+from __future__ import annotations
+
+import re
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Dict, List, Mapping, Optional, Sequence, Tuple
+
+from .sim_gripper_kin import ApertureCurve, KinematicsError, UrdfTree, find_tip_pair
+
+import yaml
+
+# 默认控制器配置位置（相对仓库根）。与真机那份同名不同目录，别搞混。
+DEFAULT_CONTROLLERS_REL = "devices/sim/gazebo/config/controllers.yaml"
+
+#: 适配器负责的控制器名。改这里要同步改 launch 与 controllers.yaml。
+DEFAULT_CONTROLLER = "gripper_controller"
+
+#: 关节值（米）与开口（毫米）的换算系数。
+MM_PER_M = 1000.0
+
+#: 参数里 mimic 表的序列化分隔符。关节名里不会有 ':'，所以安全。
+_MIMIC_SEP = ":"
+
+
+class GripperConfigError(RuntimeError):
+    """夹爪配置/描述有问题时抛出，消息面向使用者。"""
+
+
+@dataclass(frozen=True)
+class MimicRelation:
+    """一条 ``<mimic>`` 耦合关系：从动关节 = multiplier × 主关节 + offset。"""
+
+    joint: str
+    source: str
+    multiplier: float
+    offset: float
+
+    def apply(self, source_value: float) -> float:
+        """按这条关系算出从动关节的值。"""
+        return self.multiplier * source_value + self.offset
+
+    def to_param(self) -> str:
+        """序列化成 ROS 参数用的字符串（launch 与本模块必须用同一格式）。"""
+        return format_mimic_param(self.joint, self.source, self.multiplier, self.offset)
+
+
+def format_mimic_param(joint: str, source: str, multiplier: object, offset: object) -> str:
+    """把一条耦合关系序列化成 ROS 参数用的字符串。
+
+    launch 从 ``NormalizeReport.stripped_mimics`` 拿到的是**四个字符串**，
+    本模块解析的也是这个格式，所以格式化只留这一个实现 ——
+    两边各写一份的话，改了格式就会一边发一边收不到（而且不报错）。
+    """
+    return _MIMIC_SEP.join(
+        (str(joint).strip(), str(source).strip(), repr(float(multiplier)), repr(float(offset)))
+    )
+
+
+class GripperLayout:
+    """夹爪在仿真里的完整描述：关节顺序 + 耦合关系 + 主关节限位。
+
+    由 :func:`layout_from_parts` 构造（它做全部校验）；不要直接 new。
+    """
+
+    def __init__(
+        self,
+        order: Sequence[str],
+        relations: Mapping[str, MimicRelation],
+        source: str,
+        lower: float,
+        upper: float,
+        curve: Optional[ApertureCurve] = None,
+        kin_note: str = "",
+    ) -> None:
+        self.order: Tuple[str, ...] = tuple(order)
+        self.relations: Dict[str, MimicRelation] = dict(relations)
+        self.source = source
+        self.lower = float(lower)
+        self.upper = float(upper)
+        self.feedback = self._pick_feedback()
+        # 由 URDF 正运动学给出的"真实开口曲线"；None = 退回线性近似。
+        self.curve: Optional[ApertureCurve] = curve
+        #: 口径说明（给日志用）：是 FK 精确口径还是线性近似。
+        self.kin_note: str = kin_note
+
+    # --- 反馈关节 ---------------------------------------------------------
+    def _pick_feedback(self) -> str:
+        """挑一个**真的会动**的关节来当开口反馈源。
+
+        ⚠️ 主关节 ``finger_width_joint`` 修不出来：上游描述里它挂在一个叫
+        ``unused_finger_width_link`` 的**孤立假 link** 上（注释原话：
+        "The unused link cannot have 0 mass"），和手指没有任何机械连接。
+        实测（2026-09-24，Gazebo Fortress + gz_ros2_control）：命令它到 0.100，
+        它的状态**始终停在 0**，而 6 个从动关节准确到位。
+        所以拿主关节当反馈源的话，``status`` 会永远报"开口 0 mm"，
+        而夹爪明明开着 —— 这正是"看起来在工作"的静默失效。
+
+        于是改从**与主关节直接相连的那个从动关节**反推开口：
+            state = m × source + o   →   source = (state - o) / m
+        选 |m| 最大的那个（分辨率最好；m 为 0 的不能用）。
+
+        单关节夹爪（没有耦合关系）时退回到主关节自己。
+        """
+        candidates = [
+            (abs(rel.multiplier), joint)
+            for joint, rel in self.relations.items()
+            if rel.source == self.source and abs(rel.multiplier) > 1e-9
+        ]
+        if not candidates:
+            return self.source
+        candidates.sort(reverse=True)
+        return candidates[0][1]
+
+    def feedback_value(self, source_value: float) -> float:
+        """反馈关节在"主关节 = source_value"时应有的值（用于判"没跟上"）。"""
+        if self.feedback == self.source:
+            return float(source_value)
+        return self.value_for(self.feedback, source_value)
+
+    def source_from_feedback(self, feedback_state: float) -> float:
+        """反馈关节实测值 → 等价的主关节值（米）。"""
+        if self.feedback == self.source:
+            return float(feedback_state)
+        rel = self.relations[self.feedback]
+        if abs(rel.multiplier) < 1e-9:
+            raise GripperConfigError(
+                f"反馈关节 {self.feedback!r} 的 multiplier 为 0，无法反推主关节值。"
+            )
+        return (float(feedback_state) - rel.offset) / rel.multiplier
+
+    def aperture_from_feedback(self, feedback_state: float) -> float:
+        """反馈关节的实测值 → 开口（毫米）。"""
+        return self.source_to_aperture(self.source_from_feedback(feedback_state))
+
+    # --- 行程口径 ---------------------------------------------------------
+    @property
+    def stroke_mm(self) -> float:
+        """这个夹爪在仿真里**实际**能开的宽度（毫米）。
+
+        有 FK 曲线时用实测行程（GN01 ≈ 99.93 mm，不是名义的 100.0 ——
+        上游把宽度关节限位定在 0~0.100 m，而线性耦合让指尖只走了 99.93，
+        报名义值会让"全开时开口读数与几何对不上"变成一个查不出的矛盾）。
+        没有 FK 曲线时只能按限位换算，并在 :attr:`kin_note` 里说明是近似。
+
+        真机那份行程在 ``site.yaml`` 的 ``gripper.stroke_mm``；两者不同是正常的
+        （不同夹爪），但要显式打出来，见 :func:`stroke_note`。
+        """
+        if self.curve is not None:
+            return self.curve.stroke_mm
+        return (self.upper - self.lower) * MM_PER_M
+
+    @property
+    def closed_separation_mm(self) -> Optional[float]:
+        """合拢时两侧 tip frame 的固有间距（GN01 ≈ 27 mm）；无 FK 时为 None。
+
+        验收脚本用它把"指尖间距"折成"开口"（指垫合拢时间距不为 0，
+        因为 tip frame 不在指垫表面）。见 scripts/check_sim_gripper.py。
+        """
+        return None if self.curve is None else self.curve.closed_separation_mm
+
+    def aperture_to_source(self, aperture_mm: float) -> float:
+        """开口（毫米）→ 主关节值（米），并按限位截断。
+
+        **绝对物理映射**：夹爪真的开这么宽。越界不报错而是截断（撞到限位是
+        正常操作），但调用方可以从 :meth:`is_clamped` 知道发生了截断。
+
+        ⚠️ 有 FK 曲线时走**反解**，不是线性换算。因为上游的耦合是线性近似、
+        而机构是非线性的：线性口径下"命令 20 mm"实际是 21.75 mm 的指垫间距，
+        抓 20 mm 的工件会空出 1.75 mm（没有接触力），
+        触觉那一步在仿真里就废了。细节见 sim_gripper_kin.py 文件头。
+        """
+        if self.curve is not None:
+            return self.curve.joint_value(aperture_mm)
+        raw = self.lower + float(aperture_mm) / MM_PER_M
+        return min(max(raw, self.lower), self.upper)
+
+    def source_to_aperture(self, source_value: float) -> float:
+        """主关节值（米）→ 开口（毫米）。与 :meth:`aperture_to_source` 互逆。"""
+        if self.curve is not None:
+            return self.curve.aperture_mm(source_value)
+        return (float(source_value) - self.lower) * MM_PER_M
+
+    def is_clamped(self, aperture_mm: float) -> bool:
+        """这个开口请求是否会被限位截断（用来给调用方一句实话）。"""
+        if self.curve is not None:
+            return self.curve.is_clamped(aperture_mm)
+        raw = self.lower + float(aperture_mm) / MM_PER_M
+        return not (self.lower <= raw <= self.upper)
+
+    # --- 命令向量 ---------------------------------------------------------
+    def command_vector(self, aperture_mm: float) -> List[float]:
+        """按 ``order`` 拼出一整组关节目标（主关节 + 从动关节）。
+
+        顺序必须与 controllers.yaml 的 ``joints:`` 一致 —— 顺序错了，
+        JointGroupPositionController 不会报错，只会把角度发给**别的关节**，
+        表现为"夹爪动得很奇怪"。
+        """
+        return self.command_vector_from_source(self.aperture_to_source(aperture_mm))
+
+    def command_vector_from_source(self, source_value: float) -> List[float]:
+        """同上，但直接给主关节值（米），不经过"毫米"这一层来回换算。
+
+        ``stop``（保持当前位置）这类命令手里只有关节状态值，走毫米会在
+        两次换算里引入舍入误差，所以单独留这个入口。
+        """
+        return [self.value_for(name, source_value) for name in self.order]
+
+    def value_for(self, joint: str, source_value: float) -> float:
+        """某个关节在"主关节 = source_value"时应有的值。
+
+        主关节自己就是 ``source_value``；从动关节沿**耦合链**逐级算出来。
+
+        ⚠️ GN01 的耦合不是"6 个手指关节全部直挂主关节"，而是一条链：
+
+            finger_width_joint ─9.404,-0.155→ left_outer_knuckle_joint
+                                                 ├─ 1, 0 → left_inner_knuckle
+                                                 ├─ -1,0 → left_inner_finger
+                                                 ├─ 1, 0 → right_outer_knuckle
+                                                 ├─ 1, 0 → right_inner_knuckle
+                                                 └─ -1,0 → right_inner_finger
+
+        所以必须**逐级**套用 multiplier/offset（先套靠近根的、再套靠近叶的），
+        不能只套一层。只套一层的话，指尖角度会错得不多但一直不对 ——
+        表现为"夹爪动作看着像那么回事，就是夹不准"。
+        """
+        if joint == self.source:
+            return float(source_value)
+
+        # 从 joint 往上爬到根，把沿途的关系按"从根到叶"的顺序套用。
+        chain: List[MimicRelation] = []
+        seen = set()
+        cur = joint
+        while True:
+            rel = self.relations.get(cur)
+            if rel is None:
+                break
+            if cur in seen:
+                raise GripperConfigError(
+                    f"耦合链成环：{cur!r} 又回到了自己（{' → '.join(sorted(seen))}）"
+                )
+            seen.add(cur)
+            chain.append(rel)
+            cur = rel.source
+        if cur != self.source:
+            raise GripperConfigError(
+                f"关节 {joint!r} 的耦合链终止在 {cur!r}，而不是主关节 "
+                f"{self.source!r} —— 描述里的耦合表不完整。"
+            )
+        value = float(source_value)
+        for rel in reversed(chain):
+            value = rel.apply(value)
+        return value
+
+    def source_index(self) -> int:
+        """主关节在命令数组里的下标。"""
+        return self.order.index(self.source)
+
+    def _source_limits_note(self) -> str:
+        return f"限位 [{self.lower:.4f}, {self.upper:.4f}] m"
+
+    def describe(self) -> str:
+        """启动时打给操作者看的一行摘要（含必须知道的口径）。"""
+        note = f"；{self.kin_note}" if self.kin_note else ""
+        return (
+            f"主关节={self.source} {self._source_limits_note()} "
+            f"→ 仿真行程 {self.stroke_mm:.2f} mm；"
+            f"共 {len(self.order)} 个关节，{len(self.relations)} 条耦合{note}"
+        )
+
+    def stroke_note(self, real_stroke_mm: Optional[float]) -> str:
+        """仿真行程与真机行程的对比说明（两者不同是正常的，但必须说出来）。"""
+        if real_stroke_mm is None or real_stroke_mm <= 0:
+            return (
+                f"仿真夹爪行程 {self.stroke_mm:.1f} mm（site.yaml 里没读到真机行程，"
+                f"无法对比）"
+            )
+        same = abs(real_stroke_mm - self.stroke_mm) < 0.5
+        head = f"仿真行程 {self.stroke_mm:.1f} mm vs 真机 {real_stroke_mm:.1f} mm"
+        if same:
+            return head + "：一致。"
+        return (
+            head + "：**不一致**（不同型号的夹爪，这是正常的）。\n"
+            "    开口毫米采用绝对物理映射，所以同一个开口在两边几何上都对；\n"
+            "    但\"全开\"对应的开口不同，别把仿真里的全开当成真机的全开。\n"
+            "    力/触觉阈值也要按各自夹爪重新标定（见 R3.5）。"
+        )
+
+
+# ---------------------------------------------------------------------------
+# 解析与校验
+# ---------------------------------------------------------------------------
+def parse_mimics(entries: Sequence[str]) -> Dict[str, MimicRelation]:
+    """解析参数里的 mimic 表（每条 ``joint:source:multiplier:offset``）。
+
+    格式错就报错，不做"跳过这一条"的宽容处理：静默丢一条耦合关系，
+    结果是某个手指关节永远不动，而 Gazebo 不会抱怨。
+    """
+    out: Dict[str, MimicRelation] = {}
+    for raw in entries:
+        parts = str(raw).split(_MIMIC_SEP)
+        if len(parts) != 4:
+            raise GripperConfigError(
+                f"mimic 参数格式不对：{raw!r}\n"
+                f"  应为 '从动关节:主关节:multiplier:offset' 四段。"
+            )
+        joint, source, mult_s, off_s = (p.strip() for p in parts)
+        if not joint or not source:
+            raise GripperConfigError(f"mimic 参数里关节名为空：{raw!r}")
+        try:
+            mult, off = float(mult_s), float(off_s)
+        except ValueError as exc:
+            raise GripperConfigError(
+                f"mimic 参数里的 multiplier/offset 不是数字：{raw!r}"
+            ) from exc
+        if joint == source:
+            raise GripperConfigError(
+                f"mimic 参数里从动关节与主关节同名（{joint}）—— 描述有问题。"
+            )
+        if joint in out:
+            raise GripperConfigError(
+                f"mimic 参数里关节 {joint!r} 出现了两次，无法确定用哪条。"
+            )
+        out[joint] = MimicRelation(joint, source, mult, off)
+    return out
+
+
+def gripper_joint_order(
+    controllers_file: Path, controller: str = DEFAULT_CONTROLLER
+) -> List[str]:
+    """从 controllers.yaml 里读 ``<controller>.ros__parameters.joints``。
+
+    为什么解析而不是让调用方再配一份顺序：顺序是
+    ``JointGroupPositionController`` 的硬契约，配两份必然漂移，
+    而漂移的症状是"角度发给了别的关节"、不报错。
+    """
+    path = Path(controllers_file)
+    try:
+        data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
+    except OSError as exc:
+        raise GripperConfigError(f"读不到控制器配置 {path}：{exc}") from exc
+    except yaml.YAMLError as exc:
+        raise GripperConfigError(f"控制器配置 {path} 不是合法 YAML：{exc}") from exc
+
+    section = (data.get(controller) or {}).get("ros__parameters")
+    if not isinstance(section, dict):
+        raise GripperConfigError(
+            f"{path} 里没有 {controller}.ros__parameters 段。\n"
+            f"  仿真夹爪要靠这个控制器的 joints 顺序下发命令，缺了就无法工作。"
+        )
+    joints = section.get("joints")
+    if not isinstance(joints, list) or not joints:
+        raise GripperConfigError(
+            f"{path} 的 {controller}.ros__parameters 里 joints 为空。"
+        )
+    names = [str(j).strip() for j in joints]
+    if len(set(names)) != len(names):
+        raise GripperConfigError(
+            f"{path} 的 {controller} joints 里有重复项：{names}"
+        )
+    return names
+
+
+def joint_limits(urdf_xml: str, joint_name: str) -> Tuple[float, float]:
+    """从 URDF 文本里取某个关节的 (lower, upper)。取不到就报错。
+
+    为什么不从参数再传一遍限位：限位是描述里的**唯一真相**（也就是 Gazebo
+    里真实的行程）。再配一份等于给自己一个"改了描述没改配置"的机会，
+    而症状是开口悄悄不按预期走。
+    """
+    # 只认 <joint name="..."> 那一段，避免匹配到别的关节。
+    pat = re.compile(
+        r"<joint\b[^>]*\bname\s*=\s*[\"']%s[\"'][^>]*>(.*?)</joint>"
+        % re.escape(joint_name),
+        re.DOTALL,
+    )
+    m = pat.search(urdf_xml or "")
+    if not m:
+        raise GripperConfigError(
+            f"在 robot_description 里找不到关节 {joint_name!r}。\n"
+            f"  常见原因：描述没带夹爪（load_gripper:=false），或关节名前缀不对"
+            f"（仿真是 sim_ 前缀）。"
+        )
+    body = m.group(1)
+    if re.search(r"<mimic\b", body):
+        raise GripperConfigError(
+            f"关节 {joint_name!r} 自己是从动关节（带 <mimic>），不能当主关节。"
+        )
+    lm = re.search(r"<limit\b[^>]*>", body)
+    if not lm:
+        raise GripperConfigError(f"关节 {joint_name!r} 的 <limit> 标签缺失。")
+
+    def _attr(name: str) -> Optional[float]:
+        am = re.search(r"\b%s\s*=\s*[\"']([^\"']+)[\"']" % name, lm.group(0))
+        if not am:
+            return None
+        try:
+            return float(am.group(1))
+        except ValueError:
+            return None
+
+    lower, upper = _attr("lower"), _attr("upper")
+    if lower is None or upper is None:
+        # URDF 允许省略 limit 的上下限（continuous 关节就没有）。夹爪的宽度
+        # 关节必须有，否则我们无从知道"全开"是多少 —— 这时报错比猜好。
+        raise GripperConfigError(
+            f"关节 {joint_name!r} 的 <limit> 缺 lower/upper，"
+            f"无法确定开口行程。原始标签：{lm.group(0)}"
+        )
+    if upper <= lower:
+        raise GripperConfigError(
+            f"关节 {joint_name!r} 的限位不合法：lower={lower} upper={upper}"
+        )
+    return lower, upper
+
+
+def layout_from_parts(
+    order: Sequence[str],
+    mimics: Mapping[str, MimicRelation],
+    urdf_xml: str,
+) -> GripperLayout:
+    """把三份数据拼成 :class:`GripperLayout`，并做全部一致性校验。
+
+    校验点（每一条都对应一种"不报错但行为错"的故障）：
+
+    * ``mimics`` 里的从动关节必须在 ``order`` 里 —— 否则控制器根本没声明它，
+      我们发出去的这一路会被忽略；
+    * 从动关节引用的 ``source`` 也必须在 ``order`` 里 —— 链上有个关节没被
+      控制器声明的话，那一级的角度就没人驱动；
+    * 主关节（不带 mimic 的那个）必须**唯一** —— 多个候选说明推断逻辑不成立，
+      这时静默挑第一个会挑错；
+    * 每条耦合链都必须能走到主关节（不终止、成环都报错）—— 只校验开头几层
+      的话，链尾错一处只表现为"某个指尖角度一直不对"。
+    """
+    order = [str(j) for j in order]
+    if not order:
+        raise GripperConfigError("夹爪控制器没有声明任何关节。")
+
+    followers = dict(mimics)
+    unknown = sorted(set(followers) - set(order))
+    if unknown:
+        raise GripperConfigError(
+            f"mimic 表里的关节不在控制器 joints 列表里：{unknown}\n"
+            f"  控制器声明的是：{order}\n"
+            f"  两边必须一致，否则我们算出来的角度会被控制器忽略。"
+        )
+    missing_src = sorted({r.source for r in followers.values()} - set(order))
+    if missing_src:
+        raise GripperConfigError(
+            f"mimic 表里有关节的**主关节**不在控制器 joints 列表里：{missing_src}\n"
+            f"  控制器声明的是：{order}\n"
+            f"  耦合链上少一环，下面所有关节的角度都会算错。"
+        )
+
+    candidates = [j for j in order if j not in followers]
+    if len(candidates) != 1:
+        raise GripperConfigError(
+            f"无法唯一确定主关节：控制器里有 {len(candidates)} 个关节不带 mimic "
+            f"（{candidates}），期望恰好 1 个。\n"
+            f"  通常是描述里夹爪的 mimic 表没传进来（load_gripper 关了？），"
+            f"或者上游描述换了耦合方式。"
+        )
+    source = candidates[0]
+
+    lower, upper = joint_limits(urdf_xml, source)
+    layout = GripperLayout(order, followers, source, lower, upper)
+
+    # 逐条把耦合链走一遍：成环 / 不终止在这里就报出来，别等到运行期。
+    for joint in followers:
+        layout.value_for(joint, 0.0)
+
+    # 开口曲线要在 layout 之后建：它调 layout.value_for 拿每个关节的角度，
+    # 而 value_for 是耦合链的**唯一**权威实现 —— 曲线里再抄一份迟早会漂移。
+    # （所以是"先建后回填"，不是构造参数。）
+    layout.curve, layout.kin_note = _build_aperture_curve(layout, urdf_xml)
+    return layout
+
+
+def _build_aperture_curve(
+    layout: GripperLayout, urdf_xml: str
+) -> Tuple[Optional[ApertureCurve], str]:
+    """建"开口毫米 ↔ 主关节值"的精确曲线；建不出来就退回线性并**说明白**。
+
+    返回 ``(曲线或 None, 给日志的一句话)``。
+
+    为什么允许退回：换一个夹爪时，如果它的指尖 link 命名不符合约定，
+    硬报错会让整个仿真起不来；而线性近似虽然中段有几毫米偏差，但
+    "能不能动、方向对不对"仍然可验。所以退回，但**必须在日志里说清楚
+    口径变成了近似** —— 否则有人会拿它当毫米精度的夹爪去标定触觉阈值。
+    """
+    try:
+        tree = UrdfTree(urdf_xml)
+        tips = find_tip_pair(tree)
+        if tips is None:
+            return None, (
+                "⚠ 没在 URDF 里认出左右指尖 link（按 *left*finger*tip* / "
+                "*right*finger*tip* 约定找）→ 开口退回**线性近似**"
+                "（中段偏差可达 4 mm/100mm 行程，别拿它标触觉阈值）"
+            )
+        curve = ApertureCurve(
+            tree,
+            tips,
+            angles_of=lambda v: {j: layout.value_for(j, v) for j in layout.order},
+            lower=layout.lower,
+            upper=layout.upper,
+        )
+        return curve, (
+            f"开口由 URDF 正运动学反解（{tips[0]} / {tips[1]}；"
+            f"合拢时间距 {curve.closed_separation_mm:.2f} mm）"
+        )
+    except KinematicsError as exc:
+        return None, (
+            f"⚠ 正运动学不可用（{exc}）→ 开口退回**线性近似**"
+            f"（中段偏差可达 4 mm/100mm 行程）"
+        )
+
+
+# ---------------------------------------------------------------------------
+# 夹住判据
+# ---------------------------------------------------------------------------
+#: 判"夹住了"的默认容差（**毫米开口**）。GN01 全行程 100 mm，2 mm 的开口差
+#: 只可能是被挡住了，不可能是控制精度问题。
+DEFAULT_GRASP_TOLERANCE_MM = 2.0
+
+#: 判"还在动"的默认容差（毫米开口）。比上面小：它只是用来区分 MOVING / IDLE。
+DEFAULT_MOVING_TOLERANCE_MM = 0.5
+
+
+def is_grasping(
+    target_aperture_mm: float,
+    actual_aperture_mm: float,
+    tolerance_mm: float = DEFAULT_GRASP_TOLERANCE_MM,
+) -> bool:
+    """由"指令到了合拢位、实际开口却更大"推断夹住了东西。
+
+    真机（DH）有状态字可以问 "object caught"，仿真没有。但仿真里有一个等价
+    且更本质的信号：位置控制器把目标压到合拢位，若两指之间夹着东西，
+    关节就**到不了**那个位置，稳态开口会停在工件宽度附近。
+
+    两个条件同时成立才算夹住：
+
+    1. 目标在合拢侧（``target_aperture_mm`` 小于容差）—— 否则"有误差"只是没走到；
+    2. 实际开口比目标大出容差 —— 被挡住了。
+
+    ⚠️ 口径统一用**毫米开口**，不是主关节值（米）。因为开口↔关节值在
+    FK 口径下是非线性的（见 sim_gripper_kin.py），拿米当口径的话"2 mm 容差"
+    在不同开度下含义不同，标定出来的阈值换台夹爪就废了。
+
+    这是**推断**，不是测量：工件被夹在两指之间、但关节仍走到位的特殊情况
+    （例如工件很薄且被压进夹爪根部）会漏判。所以 P6 的最终判据仍然用触觉
+    力（见 R3.1），这里只提供 ``GripperStatus.is_gripped`` 这个粗信号。
+    """
+    near_closed = float(target_aperture_mm) <= float(tolerance_mm)
+    blocked = (
+        float(actual_aperture_mm) - float(target_aperture_mm)
+    ) > float(tolerance_mm)
+    return bool(near_closed and blocked)

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/sim_gripper_adapter.py

@@ -0,0 +1,515 @@
+"""仿真夹爪适配器：``GripperExecute`` 服务 → ``JointGroupPositionController``。
+
+=============================================================================
+它在链路上的位置
+=============================================================================
+    应用层（P6 状态机 / 现场脚本）
+        │  服务 <ns>/open | <ns>/close | <ns>/move | <ns>/rotate
+        │       （arm_msgs/srv/GripperExecute，与真机**同名同类型**）
+        ▼
+    本节点
+        │  ① 开口毫米 → 主关节值（绝对物理映射，见 sim_gripper.py 文件头）
+        │  ② 按 mimic 耦合算出 6 个从动关节的目标角
+        │  ③ 按 controllers.yaml 的 joints 顺序拼成数组
+        ▼
+    话题 <controller>/commands   (std_msgs/Float64MultiArray)
+        ▼
+    JointGroupPositionController（gz_ros2_control，在 Gazebo 进程里）
+        ▼
+    Gazebo 里的 7 个 sim_* 夹爪关节
+
+     同时订阅 /joint_states，把**反馈关节**的实际值换算回开口毫米，
+     发布到 <ns>/status（arm_msgs/GripperStatus）。
+     注意不是主关节 —— 主关节在 Gazebo 里压根不动，见下面第 4 个坑。
+
+=============================================================================
+为什么必须由适配器驱动全部 7 个关节
+=============================================================================
+sdformat 把 URDF 的 ``<mimic>`` 整个丢掉，gz-sim 也不实现 mimic ——
+6 个从动关节在仿真里不会跟随主关节。所以适配器必须自己算。
+这件事同时解决了一个 TF 隐患：``robot_state_publisher`` 是按 /joint_states
+里的**真实值**发布 TF 的，只要 7 个关节都被驱动、且值符合耦合公式，
+RViz/TF 与 Gazebo 就一致（详见 urdf_normalize.py 里那段说明）。
+
+=============================================================================
+⚠️ 仿真夹爪与真机夹爪不是同一个夹爪
+=============================================================================
+仿真是官方 Flexiv-GN01（行程 100 mm），现场是 DH PGEA-100-20-W
+（``site.yaml`` 的 ``gripper.stroke_mm`` = 20 mm）。开口用绝对物理映射，
+所以同一句"开 8 mm"两边都对；但"全开"对应的开口不同。启动时会把两者
+都打出来，差异大时会明确提示（见 :meth:`GripperLayout.stroke_note`）。
+
+=============================================================================
+三个必须知道的坑
+=============================================================================
+1) **命令数组的顺序**取自 controllers.yaml，不是本节点里的常量。
+   顺序错不会报错，只会把角度发给别的关节 —— 表现成"夹爪动得很奇怪"。
+   所以本节点启动时先把顺序读出来并校验（从动关节必须在列表里）。
+
+2) **``rotate`` 在平行夹爪上没有对应自由度**。真机那边 RGI 才有。这里明确
+   返回 ``success=false`` 并说明原因，而不是假装成功 —— 假装成功会让上层
+   的"拧入"流程在仿真里走到一个永远不动的分支上。
+
+3) **``status`` 的 ``is_gripped`` 是推断值**（指令到合拢位但实际值没跟上），
+   不是测量值。真机有 DH 状态字，仿真没有。最终夹持判据仍应看触觉力，
+   见 sim_gripper.is_grasping 的说明。
+
+4) **主关节 ``finger_width_joint`` 在 Gazebo 里不会动**。上游描述把它挂在
+   孤立的 ``unused_finger_width_link`` 上（那个 link 的注释写着 "cannot have
+   0 mass"，就是为了让 URDF 合法），它和手指没有任何机械连接。
+   实测：命令到 0.100，它的 /joint_states 值恒为 ~0，而 6 个从动关节准确到位。
+   所以：
+     - 命令数组里**仍然要带上它**（controllers.yaml 的 joints 含它，少一个会错位）；
+     - 但**反馈绝不能取它**，否则 status 永远报"开口 0 mm"而夹爪明明开着。
+   适配器改用"与主关节直接相连、|multiplier| 最大的那个从动关节"反推开口
+   （见 GripperLayout._pick_feedback / aperture_from_feedback）。
+
+5) **仿真里的夹持靠不住，需要一把"抓取锁"**。仿真的夹持力不是设出来的，
+   而是位置误差 × 接触刚度：规站着不动够用，机械臂一走连续轨迹（S01 快转
+   150°、pre_lift 60~100mm）就在指间打滑 —— 实测工具长度 0.243m → 0.285m、
+   规歪 62°，S07 随后拿到的几何全错。真机不会（46N 握力 + µ≈0.8 + 0.27kg）。
+   所以本节点在**确认夹住之后**用 DetachableJoint 把物体固定到手指上，
+   **指令要求张开时**解锁（详见 sim_grasp_lock.py；为什么不拿 is_gripped
+   当解锁条件，见 :meth:`_sync_grasp_lock`）。
+"""
+from __future__ import annotations
+
+from pathlib import Path
+from typing import List, Optional
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float64MultiArray
+
+from arm_msgs.msg import GripperStatus
+from arm_msgs.srv import GripperExecute
+from sensor_msgs.msg import JointState
+
+from .sim_grasp_lock import GraspLockSpec, publish_lock_event
+from .sim_gripper import (
+    DEFAULT_CONTROLLER,
+    DEFAULT_CONTROLLERS_REL,
+    DEFAULT_GRASP_TOLERANCE_MM,
+    DEFAULT_MOVING_TOLERANCE_MM,
+    GripperConfigError,
+    GripperLayout,
+    gripper_joint_order,
+    is_grasping,
+    layout_from_parts,
+    parse_mimics,
+)
+
+#: 主关节状态多少秒没更新就算"数据不新鲜"（仿真时间，不是墙钟）。
+#: 控制器 100Hz、JSB 也在这个量级，1 秒足够宽松。
+STATE_STALE_S = 1.0
+
+#: 服务后缀 → GripperCommand.command 的映射。前四个就是 capabilities.py
+#: 里声明的接口名；execute 是通用入口（与 dual_gripper_driver 对齐）。
+_SERVICE_COMMANDS = ("open", "close", "move", "rotate")
+
+
+def _site_stroke_mm() -> Optional[float]:
+    """从 site.yaml 读真机夹爪行程（只为启动时打一句对比，读不到返回 None）。
+
+    读不到不影响工作 —— 它只是提示信息，不该让仿真起不来。
+    """
+    try:
+        from arm_interfaces import load_site  # 延迟导入：只需要它一处
+    except Exception:
+        return None
+    try:
+        data = load_site()
+    except Exception:
+        return None
+    grip = data.get("gripper") if isinstance(data, dict) else None
+    if not isinstance(grip, dict):
+        return None
+    for key in ("stroke_mm", "stroke"):
+        try:
+            val = float(grip.get(key))
+        except (TypeError, ValueError):
+            continue
+        if val > 0:
+            return val
+    return None
+
+
+class SimGripperAdapter(Node):
+    """把 ``GripperExecute`` 服务翻译成 Gazebo 里 7 个夹爪关节的目标位置。"""
+
+    def __init__(self) -> None:
+        super().__init__("sim_gripper_adapter")
+
+        # 参数：全部由 launch 从**同一份** prepare_urdf 结果与 controllers.yaml
+        # 派生，避免这里自己再展开一次 xacro 造成两边不一致。
+        self.declare_parameter("controllers_file", "")
+        self.declare_parameter("controller", DEFAULT_CONTROLLER)
+        self.declare_parameter("mimics", [""])
+        # ⚠️ 不要 declare_parameter("use_sim_time", ...)：launch 会作为参数文件
+        #    传进来，rclpy 那时已经自动声明过，再声明会抛异常让节点起不来。
+        self.declare_parameter("namespace", "/gripper")
+        self.declare_parameter("tolerance_mm", DEFAULT_GRASP_TOLERANCE_MM)  # 夹住判据容差（毫米开口）
+        self.declare_parameter("default_speed_pct", 50.0)
+        self.declare_parameter("default_force_pct", 20.0)
+        # 仿真抓取锁（见 sim_grasp_lock.py）。默认**关**：参数化之后，launch 才是
+        # 唯一决定它开不开的地方 —— 单跑本节点做调试时不该莫名其妙去动 Gazebo。
+        self.declare_parameter("grasp_lock_enabled", False)
+        self.declare_parameter("grasp_lock_object", "gauge")
+        self.declare_parameter("grasp_lock_attach_topic", "/sim/grasp_lock/attach")
+        self.declare_parameter("grasp_lock_detach_topic", "/sim/grasp_lock/detach")
+        self.declare_parameter("grasp_lock_timeout_ms", 3000)
+
+        controller = str(self.get_parameter("controller").value or DEFAULT_CONTROLLER)
+        raw_ctrl = str(self.get_parameter("controllers_file").value or "")
+        controllers_file = Path(raw_ctrl) if raw_ctrl else Path.cwd() / DEFAULT_CONTROLLERS_REL
+
+        # mimics 参数传进来的是字符串数组；空字符串要滤掉（ROS 对空数组不友好）。
+        raw_mimics = [
+            str(m).strip()
+            for m in (self.get_parameter("mimics").value or [])
+            if str(m).strip()
+        ]
+
+        urdf_xml = self._robot_description()
+        try:
+            order = gripper_joint_order(controllers_file, controller)
+            relations = parse_mimics(raw_mimics)
+            self.layout: GripperLayout = layout_from_parts(order, relations, urdf_xml)
+        except GripperConfigError as exc:
+            # 配置错就让节点直接失败：一个"什么都不动"的夹爪适配器比没有更糟 ——
+            # 它看起来在工作，而上层会一直等一个永远不来的状态变化。
+            self.get_logger().error(f"仿真夹爪配置有问题：\n{exc}")
+            raise
+
+        self._controller = controller
+        self._ns = str(self.get_parameter("namespace").value or "/gripper").rstrip("/")
+        self._tolerance_mm = float(self.get_parameter("tolerance_mm").value)
+        self._speed = float(self.get_parameter("default_speed_pct").value)
+        self._force = float(self.get_parameter("default_force_pct").value)
+
+        # 当前目标（主关节值，米）与"最近一次是否有状态"。
+        self._target = self.layout.aperture_to_source(self.layout.stroke_mm)  # 默认全开
+        self._feedback_state: Optional[float] = None
+        self._state_stamp: Optional[float] = None
+        # ⚠️ 反馈取 layout.feedback，**不是**主关节：上游的 finger_width_joint
+        #    挂在孤立的 unused_finger_width_link 上，在 Gazebo 里根本不动
+        #    （实测：命令 0.100，状态恒为 0）。拿它当反馈会永远报"开口 0 mm"。
+        #    详见 sim_gripper.GripperLayout._pick_feedback 的说明。
+        self._state_name = f"{self.layout.feedback}"
+
+        self._pub_cmd = self.create_publisher(
+            Float64MultiArray, f"/{controller}/commands", 10
+        )
+        self._pub_status = self.create_publisher(GripperStatus, f"{self._ns}/status", 10)
+        self.create_subscription(JointState, "/joint_states", self._on_joint_state, 10)
+
+        self._srv: List[object] = []
+        for name in _SERVICE_COMMANDS:
+            self._srv.append(
+                self.create_service(
+                    GripperExecute, f"{self._ns}/{name}", self._make_handler(name)
+                )
+            )
+        self._srv.append(
+            self.create_service(GripperExecute, f"{self._ns}/execute", self._on_execute)
+        )
+
+        self.get_logger().info(f"夹爪：{self.layout.describe()}")
+        if self.layout.feedback != self.layout.source:
+            self.get_logger().info(
+                f"夹爪开口反馈取自 {self.layout.feedback}（反推主关节 "
+                f"{self.layout.source}；主关节本身在 Gazebo 里不跟随命令）"
+            )
+        self.get_logger().info(
+            f"夹爪行程口径：{self.layout.stroke_note(_site_stroke_mm())}"
+        )
+        self.get_logger().info(
+            f"服务 {self._ns}/{{{'|'.join(_SERVICE_COMMANDS)}|execute}} → "
+            f"话题 /{controller}/commands"
+        )
+
+        # 先把夹爪摆到默认全开，免得机器人一出场就是捏着的状态。
+        self._publish_target(self._target)
+
+        # 抓取锁：规格从参数来（launch 从 grasp_lock.yaml 派生），状态用三态：
+        #   None = 不知道 / False = 已解锁 / True = 已上锁
+        # 之所以要 None 而不是 bool + 计数器：发布失败时要能**重试**，
+        # 而"发布失败"恰恰表现为"发出去没人听"（话题名错、进程起不来），
+        # 只有把"未确认"单独表示出来才可能重试。
+        self._lock = GraspLockSpec(
+            enabled=bool(self.get_parameter("grasp_lock_enabled").value),
+            object=str(self.get_parameter("grasp_lock_object").value or "gauge"),
+            object_link="",
+            robot_model="",
+            robot_link="",
+            attach_topic=str(self.get_parameter("grasp_lock_attach_topic").value or ""),
+            detach_topic=str(self.get_parameter("grasp_lock_detach_topic").value or ""),
+            state_topic="",
+            publish_timeout_ms=int(self.get_parameter("grasp_lock_timeout_ms").value),
+        )
+        self._locked: Optional[bool] = None
+        if self._lock.enabled:
+            self.get_logger().info(
+                f"抓取锁已启用：夹住后把 {self._lock.object} 用固定关节锁到手指上，"
+                f"张开时解锁（attach={self._lock.attach_topic}）。\n"
+                "  它补的是 Gazebo 缺的那块物理（刚性接触没有静摩擦迟滞、也不模拟"
+                "指垫贴合）——\n"
+                "  真机 46N 握力 + µ≈0.8 本来就夹得住，仿真的位置控制夹持则会在"
+                "连续运动时打滑。"
+            )
+
+        self.create_timer(0.1, self._tick_status)
+
+    # --- 参数与描述 -------------------------------------------------------
+    def _robot_description(self) -> str:
+        """取归一化后的 robot_description（launch 与 RSP 用的是同一份）。"""
+        if not self.has_parameter("robot_description"):
+            self.declare_parameter("robot_description", "")
+        xml = str(self.get_parameter("robot_description").value or "")
+        if not xml.strip():
+            raise GripperConfigError(
+                "没拿到 robot_description 参数，无法确定夹爪关节的限位。\n"
+                "  正常由 gazebo_sim.launch.py 传入（它与发给 robot_state_publisher "
+                "的是同一份归一化描述）。单独手跑本节点时要显式给：\n"
+                "    ros2 run arm_sim_gazebo sim_gripper_adapter "
+                "--ros-args -p robot_description:=\"$(xacro ...)\""
+            )
+        return xml
+
+    # --- 下发 -------------------------------------------------------------
+    def _publish_target(self, source_value: float) -> None:
+        """把一整组关节目标（主 + 从动）按顺序发出去。"""
+        self._target = source_value
+        msg = Float64MultiArray()
+        msg.data = [float(v) for v in self.layout.command_vector_from_source(source_value)]
+        self._pub_cmd.publish(msg)
+
+    def _move_to_aperture(self, aperture_mm: float) -> None:
+        self._publish_target(self.layout.aperture_to_source(aperture_mm))
+
+    # --- 状态 -------------------------------------------------------------
+    def _on_joint_state(self, msg: JointState) -> None:
+        for name, pos in zip(msg.name, msg.position):
+            if name == self._state_name:
+                self._feedback_state = float(pos)
+                self._state_stamp = self._now()
+                return
+
+    def _now(self) -> float:
+        return self.get_clock().now().nanoseconds * 1e-9
+
+    def _fresh_state(self) -> Optional[float]:
+        """反馈关节的当前值；太久没更新或从没收到过就返回 None。"""
+        if self._feedback_state is None or self._state_stamp is None:
+            return None
+        if (self._now() - self._state_stamp) > STATE_STALE_S:
+            return None
+        return self._feedback_state
+
+    def _status_msg(self) -> GripperStatus:
+        state = self._fresh_state()
+        msg = GripperStatus()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        # frame_id 留空：真机那边填的是 "<side>_gripper"，而仿真里没有任何东西
+        # 发布那个坐标系（TF 只来自 robot_state_publisher）。
+        # 填一个查不到的 frame 比留空更糟 —— tf2 会一直报 frame 不存在。
+        msg.header.frame_id = ""
+        msg.name = "gn01_sim"
+        msg.gripper_type = "parallel"
+        msg.connected = state is not None
+        msg.initialized = state is not None
+        msg.velocity = self._speed
+        msg.force = self._force
+
+        if state is None:
+            # 没状态就别编：position 保持 0 会让上层以为夹爪是合拢的。
+            msg.state = "ERROR"
+            msg.error_msg = (
+                f"{STATE_STALE_S:.1f}s 内没收到 {self._state_name} 的 /joint_states，"
+                f"无法反馈开口。检查 joint_state_broadcaster 是否 active。"
+            )
+            return msg
+
+        aperture = self.layout.aperture_from_feedback(state)
+        target_aperture = self.layout.source_to_aperture(self._target)
+        # 判"还在动"用**开口口径**（毫米），不用主关节值：开口↔关节值在 FK
+        # 口径下是非线性的，拿米当口径的话同一个容差在不同开度下含义不同。
+        moving = abs(aperture - target_aperture) > DEFAULT_MOVING_TOLERANCE_MM
+        msg.position = float(aperture)
+        # 真机这里填 DH 的千分比；仿真的"宽度关节"本身就是千分比语义
+        #（0.100 m ≙ 1000‰），所以直接换算，口径与真机一致。
+        msg.finger_position_per_mille = float(
+            max(0.0, min(1000.0, aperture / max(1e-9, self.layout.stroke_mm) * 1000.0))
+        )
+        msg.is_moving = bool(moving)
+        # 判据：指令在合拢侧 + 实际开口比指令大出容差（被工件挡住）。
+        msg.is_gripped = is_grasping(
+            target_aperture, aperture, self._tolerance_mm
+        )
+        if msg.is_gripped:
+            msg.state = "GRIPPING"
+        elif moving:
+            msg.state = "MOVING"
+        else:
+            msg.state = "IDLE"
+        return msg
+
+    def _tick_status(self) -> None:
+        msg = self._status_msg()
+        self._pub_status.publish(msg)
+        self._sync_grasp_lock(bool(msg.is_gripped))
+
+    # --- 抓取锁（仿真专用）-------------------------------------------------
+    def _sync_grasp_lock(self, gripped: bool) -> None:
+        """夹持状态 → 抓取锁。只在"确认夹住"时上锁，绝不自动解锁。
+
+        **为什么不在这里因为"判到没夹住"而解锁**：
+            本适配器的 ``is_gripped`` 是**推断值**（指令到合拢位 + 实际开口被顶住），
+            抓取锁要解决的恰恰是"规在指间打了滑"的情形 —— 打滑会让触觉接触
+            闪断、``is_gripped`` 跟着抖。若拿它当解锁条件，就会得到
+            "一抖就解锁 → 规真掉下去 → 更读不到夹持"的自毁循环。
+            解锁只认**指令**（open / 开口变大的 move），那才是松手的真实原因。
+        """
+        spec = self._lock
+        if not spec.enabled or not gripped or self._locked is True:
+            return
+        if publish_lock_event(spec, True, logger=self.get_logger()):
+            self._locked = True
+            self.get_logger().info(
+                f"抓取锁：已把 {spec.object} 锁到手指上（此后机械臂怎么动都不会滑）。"
+            )
+        else:
+            # 失败不抛异常（锁只是补救措施），但要留痕：现象是"规掉了"而看不到真因。
+            self._locked = None
+
+    def _release_grasp_lock(self, why: str) -> None:
+        """松开抓取锁。只在**指令明确要张开**时调用。"""
+        spec = self._lock
+        if not spec.enabled or self._locked is False:
+            return
+        if publish_lock_event(spec, False, logger=self.get_logger()):
+            self._locked = False
+            self.get_logger().info(f"抓取锁：已解锁（{why}）")
+        else:
+            self._locked = None
+
+    # --- 服务 -------------------------------------------------------------
+    def _make_handler(self, verb: str):
+        def handler(request: GripperExecute.Request, response: GripperExecute.Response):
+            # 服务名本身就是命令；但请求里也可能带 command（通用入口风格）。
+            # 若两者冲突，以**请求里的**为准并记一条 info —— 免得上层以为
+            # 服务名能覆盖请求内容。
+            req_verb = str(getattr(request.command, "command", "") or "").strip()
+            if req_verb and req_verb != verb:
+                self.get_logger().info(
+                    f"{self._ns}/{verb} 收到 command={req_verb!r}，按请求执行。"
+                )
+                return self._execute_verb(req_verb, request, response)
+            return self._execute_verb(verb, request, response)
+
+        return handler
+
+    def _on_execute(
+        self, request: GripperExecute.Request, response: GripperExecute.Response
+    ):
+        verb = str(getattr(request.command, "command", "") or "").strip()
+        if not verb:
+            return self._fail(response, "execute 需要 request.command.command（如 open/close/move）")
+        return self._execute_verb(verb, request, response)
+
+    def _execute_verb(
+        self,
+        verb: str,
+        request: GripperExecute.Request,
+        response: GripperExecute.Response,
+    ):
+        cmd = request.command
+        try:
+            if verb == "open":
+                self._release_grasp_lock("收到 open 指令")
+                self._move_to_aperture(self.layout.stroke_mm)
+                note = f"全开（{self.layout.stroke_mm:.1f} mm）"
+            elif verb == "close":
+                self._move_to_aperture(0.0)
+                note = "全闭合（开口 0 mm）"
+            elif verb == "move":
+                want = float(cmd.position)
+                clamped = self.layout.is_clamped(want)
+                # 「开口变大」才叫松手。判据用**实测开口**而不是上一次指令：
+                # 指令可以是"先夹到 30 再挪到 32"这种，比指令会误判成开爪。
+                state = self._fresh_state()
+                now_mm = (
+                    self.layout.aperture_from_feedback(state)
+                    if state is not None
+                    else None
+                )
+                if now_mm is not None and want > now_mm + self._tolerance_mm:
+                    self._release_grasp_lock(f"开口 {now_mm:.1f} → {want:.1f} mm")
+                self._move_to_aperture(want)
+                note = f"开口 → {want:.2f} mm"
+                if clamped:
+                    note += (
+                        f"（**被限位截断**到 "
+                        f"{min(max(want, 0.0), self.layout.stroke_mm):.2f} mm）"
+                    )
+            elif verb == "rotate":
+                return self._fail(
+                    response,
+                    "仿真夹爪（GN01）是平行夹爪，没有旋转自由度；rotate 只在真机 RGI 上有效。",
+                )
+            elif verb == "stop":
+                # 停在当前位置：把"当前实际值"当目标发回去。
+                state = self._fresh_state()
+                if state is None:
+                    return self._fail(response, "没有关节状态，stop 无法保持位置")
+                # state 是**反馈关节**的值，得先换算回主关节口径再下发。
+                self._publish_target(self.layout.source_from_feedback(state))
+                note = (
+                    "保持当前位置（"
+                    f"{self.layout.aperture_from_feedback(state):.2f} mm）"
+                )
+            else:
+                return self._fail(
+                    response,
+                    f"仿真夹爪不支持命令 {verb!r}；支持 "
+                    f"{list(_SERVICE_COMMANDS) + ['stop']}。",
+                )
+        except GripperConfigError as exc:  # pragma: no cover - 构造期已挡住
+            return self._fail(response, str(exc))
+
+        response.success = True
+        response.message = note
+        response.status = self._status_msg()
+        self.get_logger().info(f"{self._ns}/{verb}：{note}")
+        return response
+
+    def _fail(self, response: GripperExecute.Response, why: str):
+        response.success = False
+        response.message = why
+        # status 仍然填：调用方拿到 success=false 时也需要知道当前开口，
+        # 否则只能靠猜（真机的返回结构也是这个语义）。
+        response.status = self._status_msg()
+        self.get_logger().warn(f"{self._ns}：{why}")
+        return response
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = None
+    try:
+        node = SimGripperAdapter()
+        rclpy.spin(node)
+    except GripperConfigError:
+        rclpy.shutdown()
+        raise SystemExit(2)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/sim_gripper_kin.py

@@ -0,0 +1,368 @@
+"""由 URDF 做夹爪正运动学，把"开口毫米"换算成**真的**关节角（纯逻辑，不起 ROS）。
+
+=============================================================================
+为什么需要它：上游的 <mimic> 是线性近似
+=============================================================================
+GN01 的描述用一条线性耦合把 6 个从动关节挂在宽度关节上：
+
+    从动关节 = 9.404 × 主关节 − 0.155
+
+系数 9.404 是按**两端**拟合出来的，所以两端准、中段不准 —— 因为真实的
+手指不是"平移"，而是"绕着 knuckle 转 + 指掌再转"，指垫的间距
+``|tip_L(θ) − tip_R(θ)|`` 是 θ 的非线性函数。
+
+实测（2026-09-24，Gazebo Fortress，见 test_sim_gripper_kin.py 里的黄金表）：
+
+    命令开口     指尖间距实测     与命令之差
+        0 mm      27.09 mm         （固有间距，指垫此时接触）
+        8 mm      35.76 mm        +0.67 mm
+       20 mm      48.84 mm        +1.75 mm
+       50 mm      80.96 mm        +3.87 mm
+      100 mm     127.02 mm        −0.07 mm
+
+也就是说：**命令"开 20 mm"，两指实际相距 21.75 mm**。抓一个 20 mm 宽的
+通止规时，指垫之间会空出 1.75 mm —— 没有接触、没有接触力，触觉那一步
+在仿真里直接失效。这和真机（DH，毫米制、开多少就是多少）的口径不一致，
+仿真就失去了"拿真几何试算法"的意义。
+
+所以本模块用 URDF 自己算正运动学，把 mm 反解成关节角：
+
+    aperture_to_source(20.0) → 令 |tip_L − tip_R| − 闭合间距 == 20.0 的关节值
+
+这样"开口 20 mm"在仿真里就是**物理上真的 20 mm**，与真机口径一致。
+
+=============================================================================
+为什么可靠
+=============================================================================
+本文件的正运动学与 Gazebo（DART 物理 + sdformat 转换后的模型）在
+0~100 mm 全程 **吻合到 0.01 mm**（上面那张表就是两边对比的结果）。
+两者完全独立：一个是我们的矩阵乘法，一个是物理引擎。这个一致性同时证明：
+
+    - URDF → SDF 的转换没有丢掉这些 link/joint 的偏移；
+    - 我们对耦合公式的理解与 Gazebo 实际执行的一致。
+
+=============================================================================
+通用性（换夹爪时）
+=============================================================================
+本模块不写死 GN01 的名字：
+    - 树结构完全从 URDF 里读；
+    - 两个"指尖"link 按命名约定找（``*left*finger*tip*`` / ``*right*...``），
+      找不到就让调用方退回线性近似（并在日志里**明确说出口径变了**）；
+    - 计算指尖间距用的参考系取两者的**最近公共祖先**，不需要知道夹爪 base
+     叫什么名字（GN01 是 ``grav_base_link``，别的夹爪叫别的）。
+"""
+from __future__ import annotations
+
+import math
+import re
+import xml.etree.ElementTree as ET
+from dataclasses import dataclass
+from typing import Callable, Dict, List, Mapping, Optional, Sequence, Tuple
+
+Mat3 = Tuple[Tuple[float, float, float], ...]
+Vec3 = Tuple[float, float, float]
+
+
+class KinematicsError(ValueError):
+    """URDF 结构与预期不符（缺 link、链断开、不是树等）。"""
+
+
+@dataclass(frozen=True)
+class JointNode:
+    """URDF 里的一条 joint（父 → 子），带固定偏移与转动轴。"""
+
+    name: str
+    type: str
+    parent: str
+    child: str
+    xyz: Vec3
+    rpy: Vec3
+    axis: Vec3
+
+
+# ---------------------------------------------------------------------------
+# 3×3 矩阵小工具（只有旋转，位置单独带着走 —— 比 4×4 好读）
+# ---------------------------------------------------------------------------
+def _rot_rpy(rpy: Vec3) -> Mat3:
+    r, p, y = rpy
+    cr, sr = math.cos(r), math.sin(r)
+    cp, sp = math.cos(p), math.sin(p)
+    cy, sy = math.cos(y), math.sin(y)
+    return (
+        (cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr),
+        (sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr),
+        (-sp, cp * sr, cp * cr),
+    )
+
+
+def _rot_axis(axis: Vec3, angle: float) -> Mat3:
+    x, y, z = axis
+    norm = math.sqrt(x * x + y * y + z * z)
+    if norm < 1e-12:
+        raise KinematicsError("关节 axis 是零向量，无法确定转动轴")
+    x, y, z = x / norm, y / norm, z / norm
+    c, s, k = math.cos(angle), math.sin(angle), 1.0 - math.cos(angle)
+    return (
+        (x * x * k + c, x * y * k - z * s, x * z * k + y * s),
+        (y * x * k + z * s, y * y * k + c, y * z * k - x * s),
+        (z * x * k - y * s, z * y * k + x * s, z * z * k + c),
+    )
+
+
+def _mat_mul(a: Mat3, b: Mat3) -> Mat3:
+    return tuple(
+        tuple(sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3))
+        for i in range(3)
+    )
+
+
+def _mat_vec(a: Mat3, v: Vec3) -> Vec3:
+    return tuple(sum(a[i][k] * v[k] for k in range(3)) for i in range(3))  # type: ignore[return-value]
+
+
+def _dist(a: Vec3, b: Vec3) -> float:
+    return math.sqrt(sum((x - y) ** 2 for x, y in zip(a, b)))
+
+
+# ---------------------------------------------------------------------------
+# URDF 树
+# ---------------------------------------------------------------------------
+def _floats(text: Optional[str], default: Sequence[float]) -> Vec3:
+    if not text:
+        return tuple(default)  # type: ignore[return-value]
+    parts = text.split()
+    if len(parts) != 3:
+        raise KinematicsError(f"URDF 里的三元组长度不对: {text!r}")
+    return (float(parts[0]), float(parts[1]), float(parts[2]))
+
+
+class UrdfTree:
+    """URDF 的 joint 树；只关心运动学（不碰 inertial / visual / collision）。"""
+
+    def __init__(self, xml: str) -> None:
+        root = ET.fromstring(xml)
+        self.by_child: Dict[str, JointNode] = {}
+        self.parent_of: Dict[str, str] = {}
+        for j in root.findall("joint"):
+            origin = j.find("origin")
+            axis_el = j.find("axis")
+            parent_el, child_el = j.find("parent"), j.find("child")
+            if parent_el is None or child_el is None or j.get("name") is None:
+                raise KinematicsError("URDF 里有缺 name/parent/child 的 joint")
+            node = JointNode(
+                name=j.get("name", ""),
+                type=j.get("type", "fixed"),
+                parent=parent_el.get("link", ""),
+                child=child_el.get("link", ""),
+                xyz=_floats(origin.get("xyz") if origin is not None else None, (0, 0, 0)),
+                rpy=_floats(origin.get("rpy") if origin is not None else None, (0, 0, 0)),
+                axis=_floats(axis_el.get("xyz") if axis_el is not None else None, (1, 0, 0)),
+            )
+            if node.child in self.by_child:
+                raise KinematicsError(f"link {node.child!r} 有两条父关节，不是树")
+            self.by_child[node.child] = node
+            self.parent_of[node.child] = node.parent
+
+    # --- 结构查询 ---------------------------------------------------------
+    def ancestors(self, link: str) -> List[str]:
+        """从 link 一路向上到根（含自身）。"""
+        out: List[str] = []
+        seen = set()
+        cur = link
+        while True:
+            out.append(cur)
+            if cur not in self.parent_of:
+                return out
+            if cur in seen:
+                raise KinematicsError(f"URDF 的 link 链里有环（{cur}）")
+            seen.add(cur)
+            cur = self.parent_of[cur]
+
+    def common_ancestor(self, a: str, b: str) -> str:
+        """最近公共祖先 link（用 A 的祖先链去查 B 的，第一个命中的就是）。"""
+        b_chain = set(self.ancestors(b))
+        for link in self.ancestors(a):
+            if link in b_chain:
+                return link
+        raise KinematicsError(f"{a!r} 与 {b!r} 没有公共祖先（URDF 断了？）")
+
+    def chain(self, link: str, root: str) -> List[JointNode]:
+        """root → link 的关节序列。"""
+        out: List[JointNode] = []
+        cur = link
+        while cur != root:
+            if cur not in self.by_child:
+                raise KinematicsError(f"从 {link!r} 上溯不到 {root!r}")
+            node = self.by_child[cur]
+            out.append(node)
+            cur = node.parent
+        return list(reversed(out))
+
+    # --- 正运动学 ---------------------------------------------------------
+    def position(self, link: str, root: str, angles: Mapping[str, float]) -> Vec3:
+        """link 原点在 root 坐标系里的位置。angles 缺省视为 0。"""
+        rot: Mat3 = ((1.0, 0, 0), (0, 1.0, 0), (0, 0, 1.0))
+        pos: Vec3 = (0.0, 0.0, 0.0)
+        for node in self.chain(link, root):
+            offset = _mat_vec(rot, node.xyz)
+            pos = (pos[0] + offset[0], pos[1] + offset[1], pos[2] + offset[2])
+            rot = _mat_mul(rot, _rot_rpy(node.rpy))
+            if node.type != "fixed":
+                rot = _mat_mul(rot, _rot_axis(node.axis, float(angles.get(node.name, 0.0))))
+        return pos
+
+
+# ---------------------------------------------------------------------------
+# 找指尖 link（按命名约定，不写死 GN01）
+# ---------------------------------------------------------------------------
+_LEFT_TIP_PATTERNS = (
+    r".*left.*finger.*tip$",
+    r".*left.*tip$",
+    r".*left.*tcp$",
+)
+_RIGHT_TIP_PATTERNS = (
+    r".*right.*finger.*tip$",
+    r".*right.*tip$",
+    r".*right.*tcp$",
+)
+
+
+def _pick(links: Sequence[str], patterns: Sequence[str]) -> Optional[str]:
+    for pat in patterns:
+        rx = re.compile(pat)  # 大小写敏感：URDF 里普遍是小写
+        hits = sorted(link for link in links if rx.match(link))
+        if hits:
+            # 多个命中时取名字最长的（``sim_left_finger_tip`` 比 ``left_tip``
+            # 更可能是真正的指垫），并保持确定性（已排序）。
+            return max(hits, key=len)
+    return None
+
+
+def find_tip_pair(tree: UrdfTree) -> Optional[Tuple[str, str]]:
+    """按命名约定找左右指垫 link；找不到返回 None（调用方退回线性近似）。"""
+    links = sorted(set(tree.by_child))
+    left = _pick(links, _LEFT_TIP_PATTERNS)
+    right = _pick(links, _RIGHT_TIP_PATTERNS)
+    if left is None or right is None or left == right:
+        return None
+    return (left, right)
+
+
+# ---------------------------------------------------------------------------
+# 开口曲线：mm ↔ 关节值
+# ---------------------------------------------------------------------------
+class ApertureCurve:
+    """由正运动学给出的"关节值 → 两指间距(mm)"曲线，以及它的反解。
+
+    ``joint_value`` 是**主关节**的值（GN01 就是宽度关节，单位米）。
+    调用方通过 ``angles_of(joint_value)`` 告诉本类"主关节取这个值时，
+    每个关节分别是多少" —— 耦合关系归调用方（sim_gripper.py）管，
+    本类只管几何，这样换夹爪/换耦合方式都不用动几何代码。
+    """
+
+    #: 反解用的二分次数（40 次足够把 100 mm 行程收敛到 1e-9 mm 以内）。
+    BISECT_ITERS = 60
+
+    def __init__(
+        self,
+        tree: UrdfTree,
+        tips: Tuple[str, str],
+        angles_of: Callable[[float], Mapping[str, float]],
+        lower: float,
+        upper: float,
+    ) -> None:
+        self.tree = tree
+        self.tips = tips
+        self.angles_of = angles_of
+        self.lower = float(lower)
+        self.upper = float(upper)
+        self.root = tree.common_ancestor(tips[0], tips[1])
+        # 闭合位置（主关节到下限）的指尖间距 —— 指垫此时贴在一起，
+        # 所以"开口"一律相对它算，这样开口 0 就是"真的合拢"。
+        self._closed = self._raw_separation(self.lower)
+        self._stroke = self._raw_separation(self.upper) - self._closed
+        # ⚠️ 这里不能只判 `stroke <= 0`。实测过一次：把右侧 knuckle 的符号写反时，
+        #    两指**同步**摆动，间距始终 45.0000000000 mm —— 差值是 4e-13，
+        #    一个裸的 `<= 0` 会放过它。而放过的后果是**静默失效**：开口曲线是
+        #    水平的，`joint_value()` 的二分会一路收敛到下限，
+        #    于是"开 20 mm"变成"不动"，且没有任何报错。
+        #    所以阈值取"名义行程的 1%"：真实夹爪不可能只有这么点行程，
+        #    而浮点噪声远小于它。
+        nominal_mm = abs(self.upper - self.lower) * 1000.0
+        min_stroke = max(1e-6, 0.01 * nominal_mm)
+        if self._stroke <= min_stroke:
+            raise KinematicsError(
+                f"正运动学算出两指行程只有 {self._stroke:.6f} mm"
+                f"（名义 {nominal_mm:.1f} mm）—— 耦合方向或 tip link 选错了："
+                f"关节在动，但两指间距几乎不变"
+            )
+        self._check_monotonic()
+
+    # --- 基础量 -----------------------------------------------------------
+    def _raw_separation(self, joint_value: float) -> float:
+        angles = self.angles_of(joint_value)
+        a = self.tree.position(self.tips[0], self.root, angles)
+        b = self.tree.position(self.tips[1], self.root, angles)
+        return 1000.0 * _dist(a, b)
+
+    def _check_monotonic(self, samples: int = 24) -> None:
+        """间距必须随主关节单调增 —— 否则反解会给出"看起来对但跳变"的结果。
+
+        真实夹爪（含 GN01）都满足；不满足说明耦合表填错了（比如某条
+        multiplier 正负号写反），这时**必须报错**而不是继续，
+        否则抓取时开口会忽大忽小。
+        """
+        prev = None
+        for i in range(samples + 1):
+            v = self.lower + (self.upper - self.lower) * i / samples
+            cur = self._raw_separation(v)
+            if prev is not None and cur < prev - 1e-6:
+                raise KinematicsError(
+                    f"指尖间距不随主关节单调变化（{v:.4f} 处 {cur:.3f} mm < "
+                    f"前一点 {prev:.3f} mm）—— 检查耦合表的正负号"
+                )
+            prev = cur
+
+    # --- 对外 -------------------------------------------------------------
+    @property
+    def closed_separation_mm(self) -> float:
+        """合拢时两侧 tip frame 之间的固有间距（GN01 约 27 mm）。
+
+        注意它不是开口：指垫此时贴在一起。验收脚本要拿它把"间距"换成
+        "开口"，见 scripts/check_sim_gripper.py。
+        """
+        return self._closed
+
+    @property
+    def stroke_mm(self) -> float:
+        """两指的**实测**行程（毫米）。GN01 ≈ 99.93，而不是名义的 100.0。
+
+        这个差不是误差：上游把宽度关节的限位定在 0~0.100 m，而线性耦合
+        让指尖实际只走了 99.93 mm。启动日志里报的是这个实测值 ——
+        报名义值会让"全开时开口读数与几何对不上"变成一个查不出的矛盾。
+        """
+        return self._stroke
+
+    def aperture_mm(self, joint_value: float) -> float:
+        """主关节值 → 开口（毫米）。
+
+        超出限位时按限位截断（位置控制器本来也会截断，这里提前对齐口径）。
+        """
+        v = min(max(float(joint_value), self.lower), self.upper)
+        return self._raw_separation(v) - self._closed
+
+    def joint_value(self, aperture_mm: float) -> float:
+        """开口（毫米）→ 主关节值。二分反解，超出行程按端点截断。"""
+        want = min(max(float(aperture_mm), 0.0), self._stroke)
+        lo, hi = self.lower, self.upper
+        for _ in range(self.BISECT_ITERS):
+            mid = 0.5 * (lo + hi)
+            if self.aperture_mm(mid) < want:
+                lo = mid
+            else:
+                hi = mid
+        return 0.5 * (lo + hi)
+
+    def is_clamped(self, aperture_mm: float) -> bool:
+        """请求的开口是否超出这个夹爪能开的范围（用来提醒上层）。"""
+        return not (0.0 <= float(aperture_mm) <= self._stroke)

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/sim_home_service.py

@@ -0,0 +1,125 @@
+#!/usr/bin/env python3
+"""仿真里的 ``/home`` —— 把真机 Flexiv 驱动的**同名服务**补齐，契约一致。
+
+为什么需要它
+------------
+真机上 ``/home``（``std_srvs/Trigger``）由 ``arm_driver_flexiv`` 提供，
+它的实现就是"切工具系（可选）+ 转发给 ``/set_named_pose``"。
+仿真里没有 Flexiv 驱动，于是通止规装配（S01 回 home、S11 复位）在 Gazebo 里
+第一步就过不了自检 —— 而 S01 本身在两种环境下**没有区别**。
+
+所以这里的做法不是"让上层代码判断自己在不在仿真"，而是**补一份同样的契约**：
+``/home`` → ``/set_named_pose``（``arm_msgs/srv/SetNamedPose``）。
+
+有意保留的差异（写在这里，免得有人以为两边完全一样）：
+
+* 真机的 ``/home`` 会顺带切工具系（``switch_tool_on_home``）；
+  仿真里没有工具坐标系的概念，**不切**（S00 也把 ``/switch_tool`` 列为选检）。
+* 真机 ``/home`` 转发时用的 ``home_pose`` / ``home_speed`` 来自 ``site.yaml``
+  （``go_home.pose`` / ``go_home.speed``）；这里用同名参数，默认值与 site 一致，
+  launch 不必再传。
+
+用法（由 ``gazebo_sim.launch.py`` 拉起）::
+
+    ros2 service call /home std_srvs/srv/Trigger '{}'
+"""
+
+from __future__ import annotations
+
+import time
+
+import rclpy
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from std_srvs.srv import Trigger
+
+from arm_msgs.srv import SetNamedPose
+
+
+class SimHomeService(Node):
+    """``/home`` → ``/set_named_pose``。"""
+
+    def __init__(self) -> None:
+        super().__init__("sim_home_service")
+        # 与 site.yaml 的 go_home 段保持一致：仿真与真机用**同一份默认值**，
+        # 免得"仿真里回零是 4 秒、真机是 1 秒"这种口径差异。
+        self.declare_parameter("home_pose", "home")
+        self.declare_parameter("home_speed", 0.25)
+        self.declare_parameter("set_named_pose_service", "/set_named_pose")
+        #: 等 ``/set_named_pose`` 出现的超时 [s]。仿真栈刚起来时它可能还没就绪，
+        #: 直接失败会让人误以为"服务没接线"。
+        self.declare_parameter("wait_service_s", 5.0)
+        #: 等 `</set_named_pose` 回应的超时 [s]。回零是慢动作，别设太小。
+        self.declare_parameter("call_timeout_s", 60.0)
+
+        self._pose = str(self.get_parameter("home_pose").value or "home")
+        self._speed = float(self.get_parameter("home_speed").value or 0.25)
+        self._timeout = float(self.get_parameter("call_timeout_s").value)
+        # Reentrant + 多线程：本节点的回调里要**同步等**另一个服务的应答，
+        # 非重入的互斥组会自锁（真机驱动里踩过，注释同样留着）。
+        self._cb = ReentrantCallbackGroup()
+        self._named = self.create_client(
+            SetNamedPose,
+            str(self.get_parameter("set_named_pose_service").value),
+            callback_group=self._cb,
+        )
+        self.create_service(Trigger, "/home", self._on_home, callback_group=self._cb)
+        self.get_logger().info(
+            f"仿真 /home 就绪 → {self._named.srv_name} 位姿[{self._pose}] "
+            f"速度 {self._speed}（真机由 Flexiv 驱动提供同名服务）"
+        )
+
+    def _on_home(self, request: Trigger.Request, response: Trigger.Response):
+        wait_s = float(self.get_parameter("wait_service_s").value)
+        if not self._named.wait_for_service(timeout_sec=wait_s):
+            response.success = False
+            response.message = (
+                f"{self._named.srv_name} 不在 —— 命名位姿服务没起来。"
+                "仿真是 gazebo_sim.launch.py 里的 named_pose_server，"
+                "真机是 arm_bringup/stack.launch.py"
+            )
+            self.get_logger().error(response.message)
+            return response
+
+        req = SetNamedPose.Request()
+        req.pose_name = self._pose
+        req.velocity_scale = self._speed
+        fut = self._named.call_async(req)
+        deadline = time.monotonic() + self._timeout
+        while not fut.done():
+            if time.monotonic() > deadline:
+                response.success = False
+                response.message = f"回位姿[{self._pose}]跟丢：{self._timeout:.0f}s 内没结果"
+                self.get_logger().error(response.message)
+                return response
+            # 这里是**回调线程**（MultiThreadedExecutor），可以 sleep；
+            # 不能 spin（会与执行器抢同一节点的回调）。
+            time.sleep(0.02)
+        res = fut.result()
+        response.success = bool(res.success)
+        response.message = res.message or f"已回位姿[{self._pose}]"
+        (self.get_logger().info if response.success else self.get_logger().error)(
+            f"/home → {response.message}"
+        )
+        return response
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = SimHomeService()
+    executor = MultiThreadedExecutor(num_threads=4)
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        executor.shutdown()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/sim_live_pose.py

@@ -0,0 +1,159 @@
+#!/usr/bin/env python3
+"""仿真里"物体**现在到底在哪**"的唯一来源：``/world/<world>/pose/info``。
+
+为什么要有这个模块（2026-09-24 的教训，别再让真值按名义位姿算）
+----------------------------------------------------------------
+真值路径（``perception.source=truth``）最初是**按 scene.yaml 的名义位姿**算目标点
+与孔位的 —— 也就是它假设"物体没动过"。实测这个假设站不住：
+
+1. 物体是"在名义位姿上方 spawn、再落下来"的，**落地就偏 2~3mm**（实测 2.6mm）；
+2. 工件骑在台面那 36mm 通槽上，静置中还会缓慢蠕动（实测十几分钟里又挪了 1.9mm）。
+
+而 2mm 的位姿误差在 S05 的像素上约 5px，在 S06 对孔上就是**规尖（φ6）进不了
+φ7 的孔** —— 现象是"S07 压到工件面、但孔没进去"，接着 S08 一转就掉件。
+现场看起来像"柔顺参数不对/夹持力不够"，其实几何从一开始就指错了两毫米。
+
+所以：**真值就该用实测位姿**，名义位姿只用来做"物体是不是被搬走了/掉下去了"的
+告警。这件事被两个地方需要（真值感知的目标点、二阶段定位的孔位），
+所以实现只放这一份（R5.8）。
+
+⚠️ 实测位姿是 Gazebo 的**真值**（不是测量），所以"按实测位姿算"比"按配置算"
+   更接近真机上的视觉：真机也是量到哪就是哪。
+"""
+
+from __future__ import annotations
+
+import math
+from dataclasses import replace
+from typing import Any, Dict, Iterable, Mapping, Optional, Tuple
+
+from arm_sim_gazebo import sim_scene
+
+#: 位姿（名 → ``(x, y, z, yaw)``，单位米/弧度）。缺的物体不在字典里。
+LivePoses = Dict[str, Tuple[float, float, float, float]]
+
+
+def yaw_from_quat_xyzw(quat: Iterable[float]) -> float:
+    """四元数 ``(x, y, z, w)`` → 绕 Z 的 yaw [rad]。
+
+    四元数→矩阵**不在这里实现**：复用全仓库唯一那份 ``handeye_lib.quat_xyzw_to_R``
+    （它有回环测试钉着）。重复实现的下场见 ``sim_truth_perception`` 的模块 docstring。
+    """
+    from arm_application.handeye_lib import quat_xyzw_to_R
+
+    R = quat_xyzw_to_R(list(quat))
+    return math.atan2(float(R[1, 0]), float(R[0, 0]))
+
+
+def poses_from_tf(msg: Any) -> LivePoses:
+    """``/world/<world>/pose/info``（``tf2_msgs/TFMessage``）→ 名 → ``(x,y,z,yaw)``。
+
+    每个模型的 ``child_frame_id`` 就是模型名（ros_gz_bridge 的约定）。
+
+    **纯函数**（只要求对象有 ``transforms``，不依赖 ROS 类型）：这条解析错的表现是
+    "实测位姿永远是空的"，于是真值退回名义位姿 —— 又是那个两毫米的错，
+    没有任何一处日志会提。所以它必须能脱离节点被钉住。
+    """
+    out: LivePoses = {}
+    for tr in getattr(msg, "transforms", None) or []:
+        name = str(getattr(tr, "child_frame_id", "") or "").strip().lstrip("/")
+        if not name:
+            continue
+        t = tr.transform.translation
+        q = tr.transform.rotation
+        out[name] = (
+            float(t.x),
+            float(t.y),
+            float(t.z),
+            yaw_from_quat_xyzw((float(q.x), float(q.y), float(q.z), float(q.w))),
+        )
+    return out
+
+
+def spec_with_live_poses(
+    spec: sim_scene.SceneSpec,
+    poses: Mapping[str, Tuple[float, float, float, float]],
+    names: Optional[Iterable[str]] = None,
+) -> sim_scene.SceneSpec:
+    """把场景**副本**里物体的位姿换成实测值；查不到实测位姿的物体保持原样。
+
+    为什么只换位姿、不换别的：``SceneSpec`` 里的几何（模型形状、孔环位置、
+    台面）都是配置里的事实，只有"物体摆在哪"是会变的。
+
+    ``names`` 给定时只换这几个（真值只依赖工件，没必要动别的）。
+    返回**副本**（``dataclasses.replace``）：调用方别把"实测位姿"写回配置，
+    否则名义位姿就永远消失了，而它正是判断"有没有被搬走"的基准。
+    """
+    want = None if names is None else {str(n) for n in names}
+    out = []
+    changed = 0
+    for obj in spec.objects:
+        p = poses.get(obj.name)
+        if p is None or (want is not None and obj.name not in want):
+            out.append(obj)
+            continue
+        out.append(replace(obj, x=float(p[0]), y=float(p[1]), z=float(p[2]), yaw=float(p[3])))
+        changed += 1
+    if changed == 0:
+        return spec
+    return replace(spec, objects=out)
+
+
+class LiveScenePose:
+    """订阅世界模型位姿，随时给出"物体现在在哪"。仿真专有。
+
+    用法（两个节点都一样，所以放在这里而不是各写一份）::
+
+        live = LiveScenePose(self, topic)          # topic 来自 scene.yaml 的 pose_info
+        spec_live = live.apply(spec, [obj_name])   # 换上位姿的副本
+        if live.ready: ...                         # 收到过帧没有
+
+    没配话题 / 没收到帧时 ``apply`` 原样返回 —— 调用方仍能用名义位姿干活，
+    但必须**说清楚**（``LiveScenePose.why_not``），否则"退回名义位姿"这件事
+    又会变成没有人知道的静默行为。
+    """
+
+    def __init__(self, node: Any, topic: str, qos_depth: int = 10) -> None:
+        self._node = node
+        self._topic = str(topic or "").strip()
+        self._poses: LivePoses = {}
+        self._seen = 0
+        self._sub = None
+        if not self._topic:
+            return
+        from tf2_msgs.msg import TFMessage
+
+        self._sub = node.create_subscription(TFMessage, self._topic, self._on_msg, qos_depth)
+
+    def _on_msg(self, msg: Any) -> None:
+        got = poses_from_tf(msg)
+        if got:
+            self._seen += 1
+            self._poses.update(got)
+
+    @property
+    def topic(self) -> str:
+        return self._topic
+
+    @property
+    def ready(self) -> bool:
+        """收到过至少一帧（一帧都没有说明话题名/桥接有问题，不是"物体没动"）。"""
+        return self._seen > 0
+
+    def pose(self, name: str) -> Optional[Tuple[float, float, float, float]]:
+        return self._poses.get(name)
+
+    def apply(
+        self,
+        spec: sim_scene.SceneSpec,
+        names: Optional[Iterable[str]] = None,
+    ) -> sim_scene.SceneSpec:
+        return spec_with_live_poses(spec, self._poses, names)
+
+    def why_not(self, name: str) -> str:
+        """给"为什么还在用名义位姿"一个能直接贴进日志的句子。"""
+        if not self._topic:
+            return "scene.yaml 的 pose_info 未配（没有世界模型位姿话题）"
+        if not self.ready:
+            return f"话题 {self._topic} 一帧都没收到（桥接没起来？话题名/世界名对不上？）"
+        return f"实测位姿里没有物体 {name!r}（模型名与 scene.yaml 的 name 不一致？）"

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/sim_pose_service.py

@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+"""把 Gazebo 里的模型**搬到指定位姿**（`/world/<world>/set_pose`）。
+
+谁在用
+------
+* ``scripts/sim_load_gauge.py``：加载通止规前把场景物体复位；
+* ``arm_sim_gazebo/sim_truth_perception.py``：启动时把真值依赖的工件复位
+  （见那里的 `_maybe_startup_reset`）。
+
+**实现只有这一份**：这个请求字符串以前抄过一遍，而它有一个很坑的细节
+（下面 `--reqtype` 的注释），第二份抄错的人不会知道。
+
+为什么走命令行 `ign service` 而不是 ROS 服务桥
+--------------------------------------------
+仿真的场景加载本来就是用命令行 `ros_gz_sim create` 做的（见 `sim_scene.py`），
+口径一致、少一个需要维护的服务桥；而且 `ros_gz_bridge` 对**服务**的桥接需要
+额外配置，桥不上时的表现是"服务一直在、调用永远超时"。
+"""
+
+from __future__ import annotations
+
+import math
+import re
+import subprocess
+from typing import Optional, Sequence, Tuple
+
+#: Gazebo 侧的服务等待超时 [ms]。
+SERVICE_TIMEOUT_MS = 3000
+#: 命令行整体超时 [s]（要大于上面那个，否则会被外面先掐掉）。
+CMD_TIMEOUT_S = 10.0
+
+
+def world_from_gz_topic(topic: str) -> Optional[str]:
+    """从 ``/world/<world>/pose/info`` 里取出 world 名；取不到返回 ``None``。
+
+    world 名必须与 ``worlds/assembly.world`` 的文件名一致，而 scene.yaml 里
+    已经写着那条话题了 —— 所以**不要**再单独配一个 world 名参数：
+    两份写法迟早不一致，而症状是"服务调用超时"（看起来像 Gazebo 没起来）。
+    """
+    m = re.fullmatch(r"/world/([^/]+)/pose(?:/info)?/?", (topic or "").strip())
+    return m.group(1) if m else None
+
+
+def yaw_to_quat(yaw_rad: float) -> Tuple[float, float, float, float]:
+    """绕 Z 的 yaw [rad] → 四元数 ``(x, y, z, w)``。
+
+    只支持绕 Z：场景里的物体位姿本来就是"平面位姿 + yaw"（``SceneObject``
+    也只有 yaw）—— 这样写反而不会被"顺手支持一下全姿态"拖进来一堆没测过的路径。
+    """
+    return (0.0, 0.0, math.sin(float(yaw_rad) / 2.0), math.cos(float(yaw_rad) / 2.0))
+
+
+def _run(cmd: Sequence[str], timeout: float = CMD_TIMEOUT_S) -> Tuple[int, str]:
+    """跑一条外部命令，返回 ``(退出码, 合并输出)``。**不抛异常**（调用方自己判）。"""
+    try:
+        p = subprocess.run(list(cmd), capture_output=True, text=True, timeout=timeout, check=False)
+        return p.returncode, (p.stdout or "") + (p.stderr or "")
+    except FileNotFoundError:
+        return 127, f"找不到可执行文件：{cmd[0]}"
+    except subprocess.TimeoutExpired:
+        return 124, f"命令超时（{timeout}s）：{' '.join(cmd)}"
+
+
+def set_model_pose(
+    world: str,
+    name: str,
+    pos: Sequence[float],
+    quat: Sequence[float],
+    timeout_s: float = CMD_TIMEOUT_S,
+) -> Tuple[bool, str]:
+    """把模型 ``name`` 搬到世界位姿 ``pos`` / ``quat``（四元数 xyzw）。
+
+    返回 ``(成功?, 输出)``。**失败只返回 False，不抛异常**：调用方拿到的场景
+    可能只是"少了一个可选物体"，因为一个失败的复位让整个流程起不来不值得。
+
+    注意 ``--reqtype`` 必须写 ``ignition.msgs.Pose``：本环境实测写
+    ``gz.msgs.Pose`` 会报 "Unable to create request"，一个字母的差别、
+    报错信息完全没提原因。
+    """
+    req = (
+        f'name: "{name}", '
+        f"position: {{x: {float(pos[0]):.6f}, y: {float(pos[1]):.6f}, z: {float(pos[2]):.6f}}}, "
+        "orientation: {"
+        f"x: {float(quat[0]):.8f}, y: {float(quat[1]):.8f}, "
+        f"z: {float(quat[2]):.8f}, w: {float(quat[3]):.8f}"
+        "}"
+    )
+    code, out = _run(
+        [
+            "ign", "service",
+            "-s", f"/world/{world}/set_pose",
+            "--reqtype", "ignition.msgs.Pose",
+            "--reptype", "ignition.msgs.Boolean",
+            "--timeout", str(int(SERVICE_TIMEOUT_MS)),
+            "--req", req,
+        ],
+        timeout=timeout_s,
+    )
+    ok = code == 0 and "data: true" in out
+    return ok, out.strip()
+
+
+def set_model_pose_yaw(
+    world: str,
+    name: str,
+    pos: Sequence[float],
+    yaw_rad: float,
+    timeout_s: float = CMD_TIMEOUT_S,
+) -> Tuple[bool, str]:
+    """``set_model_pose`` 的平面版：只有 yaw。"""
+    return set_model_pose(world, name, pos, yaw_to_quat(yaw_rad), timeout_s)

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/sim_site.py

@@ -0,0 +1,220 @@
+"""读 `config/site.yaml` 的 `sim.*` 段。
+
+=============================================================================
+为什么需要这个模块
+=============================================================================
+`config/site.yaml` 的 `sim` 段（world / physics_enable / joint_prefix）与
+`devices/sim/gazebo/manifest.yaml` 的 `config_schema` 一直宣称"改这里就生效"，
+但**没有任何代码读它**：world 路径硬编码在 robot_description._WORLD_REL 里。
+
+于是产生最难受的一类 bug：用户按文档改了 `site.yaml`，启动后行为毫无变化，
+也不报错。既然文档与 manifest 都把它当契约，就必须真的读它。
+
+要读，但也不能假装所有键都接线了 —— 有些键（physics_enable / joint_prefix）
+目前确实还没接。所以本模块除了读值，还提供 unwired_warnings()：
+把"声明了、但当前实现并不消费"的差异显式报出来，
+把静默无效变成一条看得见的告警。等哪天真的接线了，告警自然消失。
+=============================================================================
+"""
+
+from __future__ import annotations
+
+import re
+from dataclasses import dataclass, field
+from pathlib import Path
+from typing import List, Optional
+
+import yaml
+
+SITE_REL = "config/site.yaml"
+SITE_EXAMPLE_REL = "config/site.example.yaml"
+
+# 与 manifest.yaml 的 config_schema 默认值保持一致。
+# 三处（site.yaml 注释 / manifest / 这里）都写了默认值，改动时必须同步 ——
+# 所以下面有用例盯着这里和 manifest 的一致性。
+DEFAULT_WORLD = ""
+DEFAULT_PHYSICS_ENABLE = True
+DEFAULT_JOINT_PREFIX = "sim_"
+
+
+class SimSiteError(RuntimeError):
+    """site.yaml 读不了或内容不合法。"""
+
+
+@dataclass
+class SimSite:
+    """`sim.*` 段的解析结果。"""
+
+    world: str = DEFAULT_WORLD
+    physics_enable: bool = DEFAULT_PHYSICS_ENABLE
+    joint_prefix: str = DEFAULT_JOINT_PREFIX
+    # 这些键在 site.yaml 里**显式**出现过吗？
+    # 用来区分"用户说了要关物理"和"用户没提，所以用默认值"——
+    # 前者值得告警，后者没啥可说。
+    present: set = field(default_factory=set)
+    source: Optional[Path] = None
+
+
+def load_sim_site(root: Optional[Path] = None) -> SimSite:
+    """读 `config/site.yaml` 的 sim 段。文件不存在则返回全默认值。
+
+    故意**不**在文件缺失时报错：仿真要能在没有站点的机器上跑起来
+    （site.yaml 里含真实设备 ID，本来就不该提交，见 README 的"要提交什么"）。
+    """
+    base = Path(root) if root is not None else repo_root()
+    path = base / SITE_REL
+
+    if not path.is_file():
+        return SimSite()
+
+    try:
+        doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
+    except yaml.YAMLError as exc:
+        raise SimSiteError(
+            f"{path} 解析失败: {exc}\n"
+            f"  这一行会让仿真启动不了。修好它，或临时改名为 site.yaml.bak 忽略。"
+        ) from exc
+
+    if not isinstance(doc, dict):
+        raise SimSiteError(f"{path} 的顶层不是映射（键值对）。")
+
+    raw = doc.get("sim")
+    if raw is None:
+        return SimSite(source=path)
+    if not isinstance(raw, dict):
+        raise SimSiteError(
+            f"{path} 的 sim 段不是映射（键值对），而是 {type(raw).__name__}。"
+            f"它应该像这样：\n    sim:\n      world: devices/sim/gazebo/worlds/assembly.world"
+        )
+
+    site = SimSite(source=path)
+    if "world" in raw:
+        site.world = str(raw.get("world") or "").strip()
+        site.present.add("world")
+    if "physics_enable" in raw:
+        site.physics_enable = bool(raw.get("physics_enable"))
+        site.present.add("physics_enable")
+    if "joint_prefix" in raw:
+        site.joint_prefix = str(raw.get("joint_prefix") or "").strip()
+        site.present.add("joint_prefix")
+    return site
+
+
+def repo_root() -> Path:
+    """仓库根。**实现只有一份**：``robot_description.repo_root()``。
+
+    这里曾经也是 ``parents[3]`` —— 源码树里对、安装后错（install_sim 是拷贝）。
+    两处各写一份还会各自漂移，所以都改成委托（见 sim_scene.repo_root 的注释）。
+    """
+    from arm_sim_gazebo.robot_description import repo_root as _robust_root
+
+    return _robust_root()
+
+
+def resolve_world(
+    cli_world: str = "",
+    root: Optional[Path] = None,
+    fallback: Optional[Path] = None,
+) -> tuple[Path, str]:
+    """决定用哪个 world 文件。返回 (路径, 来源说明)。
+
+    优先级：命令行 --world  >  site.yaml 的 sim.world  >  内置默认。
+
+    为什么要有优先级而不是"只认 site.yaml"：site.yaml 是站点级的，而调试时
+    经常要临时换一个世界（比如 in.sdf 里复制一份改着试）。命令行必须能压过它。
+    """
+    base = Path(root) if root is not None else repo_root()
+
+    if cli_world and cli_world.strip():
+        p = Path(cli_world.strip())
+        # 相对路径按仓库根解析：用户从任意目录启动，写相对路径的直觉都是"相对仓库"。
+        if not p.is_absolute():
+            p = base / p
+        return p, "命令行 --world"
+
+    site = load_sim_site(base)
+    if site.world:
+        p = Path(site.world)
+        if not p.is_absolute():
+            p = base / p
+        return p, f"{SITE_REL} 的 sim.world"
+
+    if fallback is None:
+        raise SimSiteError(
+            "没能确定 world 文件：site.yaml 里 sim.world 为空，也没给内置默认。"
+        )
+    fb = Path(fallback)
+    # 内置默认也要跟前面两条一样解析成绝对路径：调用方（启动器 / launch）
+    # 拿到的必须是绝对路径，否则从别的目录启动时会被解析到错误的位置，
+    # 而那会表现成"找不到 world 文件"，看起来像配置写错了。
+    if not fb.is_absolute():
+        fb = base / fb
+    return fb, "内置默认（site.yaml 未指定 sim.world）"
+
+
+def unwired_warnings(
+    site: SimSite,
+    actual_joint_prefix: Optional[str] = None,
+    physics_in_world: Optional[bool] = None,
+) -> List[str]:
+    """把"site.yaml 里声明了、但当前实现并不消费"的配置报出来。
+
+    这是本模块存在的一半理由：与其让 `physics_enable: false` 静默无效，
+    不如明说"这个键还没接线"。下面两个键目前确实没接：
+
+      physics_enable  世界文件里的重力/步长是写死的，没有按它开关。
+      joint_prefix    仿真关节前缀由 arm.sim.urdf.xacro 的 arm_prefix 决定，
+                      且 controllers.yaml 里硬编码了 sim_joint1..7；
+                      改 site.yaml 不会传导过去。
+
+    等它们真的接线了，从这里删掉对应分支，告警就自然消失。
+    """
+    out: List[str] = []
+
+    if "physics_enable" in site.present and physics_in_world is not None:
+        want_on = bool(site.physics_enable)
+        if want_on != bool(physics_in_world):
+            out.append(
+                f"site.yaml 写了 sim.physics_enable={site.physics_enable}，"
+                f"但世界文件当前是物理{'开启' if physics_in_world else '关闭'}状态，"
+                f"而这个键**还没有接线** —— 世界文件里的重力/步长是写死的。\n"
+                f"      想让物理开关生效，得改 world 文件里的 <physics>，"
+                f"或者先把这个键接进启动流程（见 sim_site.py 顶部说明）。"
+            )
+
+    if "joint_prefix" in site.present and actual_joint_prefix is not None:
+        want = site.joint_prefix
+        if want and want != actual_joint_prefix:
+            out.append(
+                f"site.yaml 写了 sim.joint_prefix={want!r}，"
+                f"但实际用的前缀是 {actual_joint_prefix!r}，"
+                f"而这个键**还没有接线**。\n"
+                f"      前缀来自 arm.sim.urdf.xacro 的 arm_prefix，"
+                f"且 config/controllers.yaml 里硬编码了 {actual_joint_prefix}joint1..7 —— "
+                f"三处必须一致，改一处不够。"
+            )
+
+    return out
+
+
+def physics_enabled_in_world(world_file: Path) -> Optional[bool]:
+    """世界文件里的物理到底开没开。判据是重力是不是零向量。
+
+    gz 里"关物理"的常见做法就是把 <gravity> 置零（比删掉整个 <physics>
+    更常用，因为 <physics> 还带着步长等其它必需项）。没有 <physics> /
+    <gravity> 时按"开着"算 —— gz 的默认就是开。
+    """
+    try:
+        text = Path(world_file).read_text(encoding="utf-8", errors="replace")
+    except OSError:
+        return None
+    m = re.search(r"<gravity>\s*([^<]+)\s*</gravity>", text)
+    if not m:
+        return True
+    try:
+        vals = [float(x) for x in m.group(1).split()]
+    except ValueError:
+        return None
+    if len(vals) != 3:
+        return None
+    return any(abs(v) > 1e-12 for v in vals)

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/sim_tactile.py

@@ -0,0 +1,317 @@
+"""仿真触觉的配置解析、轴向投影与基线跟踪。
+
+=============================================================================
+为什么单独一个模块
+=============================================================================
+把 Gazebo 的关节力传感器变成 arm_msgs/TactileStatus，真正有内容的是三件事：
+
+    1. 轴向投影   关节坐标系的三向力 → (fnormal, fshearx, fsheary)
+    2. 基线相减   静止时关节要承担手指自重，不减掉就是一个恒定偏置
+    3. 接触判据   接触传感器是**事件式**发布，必须靠超时判"已离开"
+
+这三件事都是纯数据变换，不需要 ROS 跑起来就能验。所以实现放在这里、用普通
+函数/类表达，节点（sim_tactile_adapter.py）只做订阅/发布这层壳。
+好处是它们能被单测钉死 —— 轴向映射搞反、基线算错这类错误，
+在"力有数值变化"的直观观察下完全看不出异常，只能靠测试兜住。
+
+轴映射的实测依据与口径见 devices/sim/gazebo/config/tactile.yaml 的注释。
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Dict, List, Optional, Sequence, Tuple
+
+import yaml
+
+# 默认配置位置（相对仓库根）。与 cameras.yaml 同目录，属"物料而非代码"。
+DEFAULT_TACTILE_REL = "devices/sim/gazebo/config/tactile.yaml"
+
+# 轴向只认这三根轴。写成别的一律报错 —— 静默地把 'Y' 当成未知轴处理，
+# 会让力的语义整体错位而没有任何提示。
+_VALID_AXES = ("x", "y", "z")
+
+
+class TactileConfigError(RuntimeError):
+    """触觉配置有问题时抛出，消息面向使用者。"""
+
+
+@dataclass(frozen=True)
+class AxisSpec:
+    """把关节力传感器的三根轴映射成 Orisys 的三路力。
+
+    ``normal`` 是**指尖法向**（压下去的方向）；另两根是切向。
+    符号单独给：实测确认两指压同一个物体读数同号，但保留符号项是为了
+    换夹爪/换传感器时不必改代码。
+    """
+
+    normal: str = "y"
+    shear_x: str = "z"
+    shear_y: str = "x"
+    sign_normal: float = 1.0
+    sign_shear_x: float = 1.0
+    sign_shear_y: float = 1.0
+
+    def project(self, force: Sequence[float], scale: float = 1.0) -> Tuple[float, float, float]:
+        """三向力（关节坐标系，牛顿）→ (fnormal, fshearx, fsheary)。
+
+        ``scale`` 是 config 里的 force_scale，见该文件头的「单位口径」。
+        """
+        idx = {a: i for i, a in enumerate(_VALID_AXES)}
+        fx, fy, fz = (float(v) for v in force)
+        raw = {"x": fx, "y": fy, "z": fz}
+        return (
+            self.sign_normal * raw[self.normal] * scale,
+            self.sign_shear_x * raw[self.shear_x] * scale,
+            self.sign_shear_y * raw[self.shear_y] * scale,
+        )
+
+    def shear_indices(self) -> Tuple[int, int]:
+        """两个切向轴在 (x,y,z) 里的下标，用于把接触点转成 centroid_u/v。"""
+        idx = {a: i for i, a in enumerate(_VALID_AXES)}
+        return idx[self.shear_x], idx[self.shear_y]
+
+
+@dataclass(frozen=True)
+class TactileSide:
+    """一"指"的声明。"""
+
+    side: str
+    wrench_topic: str
+    contact_topic: str
+    frame: str
+
+    # --- ROS 侧话题名：与真机 tactile_node 完全一致，应用层不用改 -----------
+    @property
+    def status_topic(self) -> str:
+        return f"/tactile/{self.side}/status"
+
+    @property
+    def image_topic(self) -> str:
+        return f"/tactile/{self.side}/image"
+
+    @property
+    def vector_topic(self) -> str:
+        return f"/tactile/{self.side}/vector"
+
+    # --- Gazebo 侧：桥接的输入 ---------------------------------------------
+    @property
+    def gz_wrench_msg(self) -> str:
+        return "gz.msgs.Wrench"
+
+    @property
+    def gz_contact_msg(self) -> str:
+        return "gz.msgs.Contacts"
+
+
+@dataclass(frozen=True)
+class TactileSpec:
+    """整个仿真触觉的声明。"""
+
+    sides: Tuple[TactileSide, ...]
+    axis: AxisSpec
+    publish_hz: float
+    baseline_seconds: float
+    rebaseline_when_idle: bool
+    rebaseline_tau_s: float
+    contact_timeout_s: float
+    min_contact_points: int
+    force_scale: float
+    image_hz: float
+    image_width: int
+    image_height: int
+    unit: str = "N"
+
+    def by_side(self, side: str) -> Optional[TactileSide]:
+        for s in self.sides:
+            if s.side == side:
+                return s
+        return None
+
+
+def load_tactile(path: Optional[Path] = None, root: Optional[Path] = None) -> TactileSpec:
+    """读触觉配置。找不到或写错时抛 TactileConfigError，消息里给出修法。"""
+    if path is None:
+        base = root if root is not None else Path.cwd()
+        path = Path(base) / DEFAULT_TACTILE_REL
+    path = Path(path)
+
+    if not path.is_file():
+        raise TactileConfigError(
+            f"触觉配置不存在: {path}\n"
+            f"  它是仿真相机之外另一份「物料配置」：话题名、轴向映射、force_scale、\n"
+            f"  基线口径全在这里（桥接与适配器都从它派生）。缺了它仿真里的触觉\n"
+            f"  完全没有输出。确认仓库根下存在 {DEFAULT_TACTILE_REL}。"
+        )
+
+    try:
+        doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
+    except yaml.YAMLError as exc:
+        raise TactileConfigError(f"触觉配置 YAML 解析失败: {path}\n  原因: {exc}") from exc
+
+    raw_sides = doc.get("sides")
+    if not isinstance(raw_sides, list) or not raw_sides:
+        raise TactileConfigError(
+            f"{path} 里没有 sides 列表（或为空）。双指方案要写 left 与 right 两项 ——\n"
+            f"  单指无法复现「旋拧时切向力从哪一侧来」这个判据。"
+        )
+
+    sides: List[TactileSide] = []
+    seen: Dict[str, str] = {}
+    for i, item in enumerate(raw_sides):
+        where = f"{path} 的 sides[{i}]"
+        if not isinstance(item, dict):
+            raise TactileConfigError(f"{where} 不是映射（key: value 形式）。")
+        missing = [
+            k for k in ("side", "wrench_topic", "contact_topic", "frame")
+            if not str(item.get(k, "")).strip()
+        ]
+        if missing:
+            raise TactileConfigError(
+                f"{where} 缺字段: {', '.join(missing)}。\n"
+                f"  wrench_topic/contact_topic 必须与 arm.sim.sensors.xacro 里写的\n"
+                f"  <topic> 完全一致 —— 对不上时适配器只是订阅了一个不存在的话题，\n"
+                f"  不报错、status 永远是零。"
+            )
+        side = str(item["side"]).strip()
+        if side in seen:
+            raise TactileConfigError(f"{where} 的 side 重复: {side}")
+        seen[side] = where
+        sides.append(
+            TactileSide(
+                side=side,
+                wrench_topic=str(item["wrench_topic"]).strip(),
+                contact_topic=str(item["contact_topic"]).strip(),
+                frame=str(item["frame"]).strip(),
+            )
+        )
+
+    raw_axis = doc.get("axis") or {}
+    raw_sign = doc.get("sign") or {}
+    if not isinstance(raw_axis, dict) or not isinstance(raw_sign, dict):
+        raise TactileConfigError(f"{path} 的 axis / sign 必须是映射。")
+
+    axis = AxisSpec(
+        normal=_axis(raw_axis.get("normal", "y"), path, "axis.normal"),
+        shear_x=_axis(raw_axis.get("shear_x", "z"), path, "axis.shear_x"),
+        shear_y=_axis(raw_axis.get("shear_y", "x"), path, "axis.shear_y"),
+        sign_normal=_num(raw_sign.get("normal", 1.0), path, "sign.normal"),
+        sign_shear_x=_num(raw_sign.get("shear_x", 1.0), path, "sign.shear_x"),
+        sign_shear_y=_num(raw_sign.get("shear_y", 1.0), path, "sign.shear_y"),
+    )
+    if len({axis.normal, axis.shear_x, axis.shear_y}) != 3:
+        raise TactileConfigError(
+            f"{path} 的 axis 里三根轴必须互不相同（当前 normal={axis.normal}, "
+            f"shear_x={axis.shear_x}, shear_y={axis.shear_y}）。\n"
+            f"  重复意味着有两个分量取自同一根轴，另有一根轴被丢掉 —— "
+            f"力的语义会静默错位。"
+        )
+
+    spec = TactileSpec(
+        sides=tuple(sides),
+        axis=axis,
+        publish_hz=_num(doc.get("publish_hz", 20.0), path, "publish_hz", positive=True),
+        baseline_seconds=_num(doc.get("baseline_seconds", 2.0), path, "baseline_seconds"),
+        rebaseline_when_idle=bool(doc.get("rebaseline_when_idle", True)),
+        rebaseline_tau_s=_num(doc.get("rebaseline_tau_s", 3.0), path, "rebaseline_tau_s"),
+        contact_timeout_s=_num(
+            doc.get("contact_timeout_s", 0.25), path, "contact_timeout_s", positive=True
+        ),
+        min_contact_points=int(doc.get("min_contact_points", 1)),
+        force_scale=_num(doc.get("force_scale", 1.0), path, "force_scale"),
+        image_hz=_num(doc.get("image_hz", 0.0), path, "image_hz"),
+        image_width=int(doc.get("image_width", 64)),
+        image_height=int(doc.get("image_height", 64)),
+        unit=str(doc.get("unit", "N")),
+    )
+    return spec
+
+
+def _axis(value: object, path: Path, key: str) -> str:
+    axis = str(value).strip().lower()
+    if axis not in _VALID_AXES:
+        raise TactileConfigError(
+            f"{path} 的 {key} 必须是 x/y/z 之一，当前是 {value!r}。"
+        )
+    return axis
+
+
+def _num(value: object, path: Path, key: str, positive: bool = False) -> float:
+    try:
+        out = float(value)
+    except (TypeError, ValueError) as exc:
+        raise TactileConfigError(f"{path} 的 {key} 不是数字: {value!r}") from exc
+    if positive and out <= 0.0:
+        raise TactileConfigError(f"{path} 的 {key} 必须为正，当前是 {out}")
+    return out
+
+
+def bridge_args(spec: TactileSpec) -> List[str]:
+    """生成 ros_gz_bridge parameter_bridge 的话题规则。
+
+    只做**传输**：gz 消息类型 → ROS 消息类型，话题名保持 gz 侧原样。
+    改名与语义（轴向、基线、单位）全在适配器里做 —— 桥接器改不了 frame_id，
+    也做不了基线相减，越权只会把"在哪一层改"这件事弄乱。
+    """
+    args: List[str] = []
+    for s in spec.sides:
+        args.append(
+            f"{s.wrench_topic}@geometry_msgs/msg/Wrench[{s.gz_wrench_msg}"
+        )
+        args.append(
+            f"{s.contact_topic}@ros_gz_interfaces/msg/Contacts[{s.gz_contact_msg}"
+        )
+    return args
+
+
+class BaselineTracker:
+    """按"相对静止基线取增量"的口径跟踪零力读数。
+
+    两段式：
+        预热期（前 baseline_seconds 秒）：等权均值。用均值而不是 EMA 是因为
+            一开始没有任何先验，EMA 从 0 爬升需要很久。
+        稳态期：仅在**确认无接触**时用时间常数 tau 做慢跟踪。
+            为什么需要慢跟踪而不是"一次性基线"：force_torque 读的是关节传递力，
+            静止时它等于远端那段手指的自重，而**这个自重项随机械臂姿态变化** ——
+            一次性基线会在换姿态后留下残余。只在无接触时更新，是为了不把真实
+            接触当成新基线吃掉（那会让接触力永远回到零，症状是"夹住了但力为 0"）。
+    """
+
+    def __init__(self, baseline_seconds: float, tau_s: float, enabled: bool = True) -> None:
+        self._warmup = max(0.0, float(baseline_seconds))
+        self._tau = max(0.0, float(tau_s))
+        self._enabled = bool(enabled)
+        self._sum: Dict[str, float] = {}
+        self._count: Dict[str, int] = {}
+        self._value: Dict[str, float] = {}
+        self._elapsed = 0.0
+
+    @property
+    def warmed_up(self) -> bool:
+        return self._elapsed >= self._warmup
+
+    def add(self, key: str, raw: float, dt: float, in_contact: bool) -> None:
+        """喂一个原始读数。``dt`` 是距上次的秒数。"""
+        if in_contact:
+            # 接触期间既不预热也不跟踪：接触力绝不能进基线。
+            return
+        if not self.warmed_up:
+            self._sum[key] = self._sum.get(key, 0.0) + float(raw)
+            self._count[key] = self._count.get(key, 0) + 1
+            self._value[key] = self._sum[key] / max(1, self._count[key])
+            return
+        if not self._enabled or self._tau <= 0.0 or key not in self._value:
+            return
+        alpha = min(1.0, max(0.0, dt) / self._tau)
+        self._value[key] += (float(raw) - self._value[key]) * alpha
+
+    def advance(self, dt: float) -> None:
+        """推进预热计时。必须在每轮循环调用一次（即使这一轮没有读数）。"""
+        self._elapsed += max(0.0, float(dt))
+
+    def value(self, key: str) -> float:
+        return self._value.get(key, 0.0)
+
+    def delta(self, key: str, raw: float) -> float:
+        """原始读数 → 相对基线的增量。"""
+        return float(raw) - self.value(key)

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/sim_tactile_adapter.py

@@ -0,0 +1,389 @@
+"""仿真触觉适配器：Gazebo 的关节力/接触传感器 → arm_msgs/TactileStatus。
+
+=============================================================================
+它在链路上的位置
+=============================================================================
+    gz  /tactile/<side>/wrench  (gz.msgs.Wrench,  持续 ~100Hz)
+    gz  /tactile/<side>/raw     (gz.msgs.Contacts, 只在接触时)
+        │
+        ├─ ros_gz_bridge parameter_bridge（只搬类型，不改语义）
+        │
+        └─ 本节点
+             ├ 轴向投影    三向力 → fnormal/fshearx/fsheary（映射实测，见 config）
+             ├ 基线相减    静止时关节要承担手指自重，不减就是一个恒定偏置
+             ├ 接触判据    超时判"已离开"（接触传感器是事件式发布）
+             └ 变送        /tactile/<side>/status  （arm_msgs/TactileStatus）
+
+话题名、消息类型、字段口径全部与真机 tactile_node 对齐，所以下游
+（compliant_descent、stage3_search_node）**一行都不用改**就能吃仿真的数据，
+这正是"同一套接口、换后端"的契约。
+
+=============================================================================
+三个必须知道的坑（都会静默）
+=============================================================================
+1) 接触传感器的消息是**事件式**的：不接触时一条都不发。
+   若适配器只在收到消息时才发布 status，那么"离开工件"期间下游会认为
+   话题断流（stage3_search_node 的 _tac_fresh 会直接 SearchAbort）。
+   所以本节点**按 publish_hz 恒定发布**，接触状态由"最近是否收到过"决定。
+   真机 Orisys 也是恒定发布的，这里必须与真机一致。
+
+2) 力的话题来自 force_torque 传感器，而它依赖 world 里的
+   forcetorque-system 插件被显式声明（只要 world 声明了任何 <plugin>，
+   gz 就不再加载默认系统列表）。插件漏了的话**话题根本不出现**、
+   日志一片空白。所以本节点在启动几秒后若一个 wrench 都没收到，
+   会明确报出来并指出该查哪里。
+
+3) 单位不是牛顿等价值就可能是错的：Gazebo 给的是牛顿，真机给的是形变
+   任意单位。换算靠 config 里的 force_scale，见 tactile.yaml 文件头的
+   「单位口径」。选错不报错，只表现为阈值永远不触发或一碰就炸。
+"""
+from __future__ import annotations
+
+import time
+from pathlib import Path
+from typing import Dict, Optional, Tuple
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from rclpy.time import Time
+
+from arm_msgs.msg import TactileStatus
+from geometry_msgs.msg import Wrench
+from sensor_msgs.msg import Image
+from tf2_ros import Buffer, TransformListener
+
+# ros_gz_interfaces 只在仿真里用得到，而且是 conda 装的（不在本工作区）。
+# 万一它的 typesupport 加载不了（见下），我们只丢掉"接触点"这一路信息 ——
+# 力本身走 geometry_msgs/Wrench，不受影响。所以这里**容错导入**：
+# 宁可用退化模式跑，也不要整个适配器起不来。
+_contacts_err: Optional[str] = None
+try:
+    from ros_gz_interfaces.msg import Contacts
+except Exception as _exc:  # pragma: no cover - 取决于运行环境
+    Contacts = None  # type: ignore[assignment]
+    _contacts_err = f"{type(_exc).__name__}: {_exc}"
+
+from .sim_tactile import (
+    DEFAULT_TACTILE_REL,
+    BaselineTracker,
+    TactileConfigError,
+    TactileSide,
+    TactileSpec,
+    load_tactile,
+)
+
+# 启动后多久检查"到底有没有收到力数据"。要给桥接器与 gz 一点时间。
+TOPIC_CHECK_DELAY_S = 8.0
+
+
+def _depth_placeholder(width: int, height: int) -> np.ndarray:
+    """占位深度图：32FC1、全 NaN。
+
+    为什么是 NaN 而不是全 0：全 0 在深度语义里是"距离 0 米"，会被下游当成
+    一个贴脸的平面；NaN 是"无读数"，不会伪装成有效数据。
+    """
+    return np.full((height, width), np.nan, dtype=np.float32)
+
+
+def _vector_placeholder(width: int, height: int) -> np.ndarray:
+    """占位力矢量图：32FC2（每像素 dx, dy），全 NaN。"""
+    return np.full((height, width, 2), np.nan, dtype=np.float32)
+
+
+class SimTactileAdapter(Node):
+    def __init__(self) -> None:
+        super().__init__("sim_tactile_adapter")
+
+        self.declare_parameter("tactile_file", "")
+        # ⚠️ 不要 declare_parameter("use_sim_time", ...)：launch 会把它作为
+        #    参数文件传进来，rclpy 那时**已经自动声明过**，再声明一次会直接抛
+        #    ParameterAlreadyDeclaredException 让节点起不来（实测踩过）。
+        #    相机适配器同样只声明自己的参数。
+
+        raw_path = str(self.get_parameter("tactile_file").value or "")
+        path = Path(raw_path) if raw_path else Path.cwd() / DEFAULT_TACTILE_REL
+        try:
+            self.spec: TactileSpec = load_tactile(path)
+        except TactileConfigError as exc:
+            # 配置错误必须让节点直接失败：静默启动一个"永远零力"的适配器
+            # 比不启动更糟 —— 它看起来在工作。
+            self.get_logger().error(f"触觉配置有问题：\n{exc}")
+            raise
+
+        self.get_logger().info(
+            f"触觉：单位={self.spec.unit} force_scale={self.spec.force_scale} "
+            f"发布率={self.spec.publish_hz}Hz 基线={self.spec.baseline_seconds}s"
+            f" 轴向 normal={self.spec.axis.normal} "
+            f"shear_x={self.spec.axis.shear_x} shear_y={self.spec.axis.shear_y}"
+        )
+
+        self._tf_buffer = Buffer()
+        self._tf_listener = TransformListener(self._tf_buffer, self)
+
+        # 每指的状态。键都是 side。
+        self._wrench: Dict[str, Tuple[float, float, float]] = {}
+        self._wrench_stamp: Dict[str, float] = {}
+        self._contact_point: Dict[str, Optional[Tuple[float, float, float]]] = {}
+        self._contact_stamp: Dict[str, float] = {}
+        self._got_wrench: Dict[str, bool] = {}
+        self._got_contact: Dict[str, bool] = {}
+
+        self._baseline = BaselineTracker(
+            self.spec.baseline_seconds,
+            self.spec.rebaseline_tau_s,
+            enabled=self.spec.rebaseline_when_idle,
+        )
+        self._last_tick = time.monotonic()
+
+        self._pub_status = {}
+        self._pub_image: Dict[str, Tuple[object, object, float, float]] = {}
+        self._contact_unavailable = False
+        for s in self.spec.sides:
+            self._got_wrench[s.side] = False
+            self._got_contact[s.side] = False
+            self._pub_status[s.side] = self.create_publisher(
+                TactileStatus, s.status_topic, 10
+            )
+            self.create_subscription(
+                Wrench,
+                s.wrench_topic,
+                self._make_wrench_cb(s.side),
+                10,
+            )
+            if Contacts is not None:
+                self.create_subscription(
+                    Contacts,
+                    s.contact_topic,
+                    self._make_contact_cb(s.side),
+                    10,
+                )
+            else:
+                self._contact_unavailable = True
+            if self.spec.image_hz > 0.0:
+                self._pub_image[s.side] = (
+                    self.create_publisher(Image, s.image_topic, 3),
+                    self.create_publisher(Image, s.vector_topic, 3),
+                    self.spec.image_hz,
+                    0.0,
+                )
+                self.get_logger().warn(
+                    f"仿真里没有弹性体成像：{s.image_topic} 与 {s.vector_topic} "
+                    f"发布的是**占位图**（32FC1 / 32FC2 全 NaN），话题与消息结构"
+                    f"与真机一致，但内容没有物理含义。需要真实图像时请关掉 "
+                    f"image_hz（设为 0）。"
+                )
+
+        period = 1.0 / max(1e-3, self.spec.publish_hz)
+        self.create_timer(period, self._tick)
+        self._check_at = time.monotonic() + TOPIC_CHECK_DELAY_S
+        self._checked = False
+
+        if self._contact_unavailable:
+            detail = f"（导入失败：{_contacts_err}）" if _contacts_err else ""
+            self.get_logger().warn(
+                f"ros_gz_interfaces/msg/Contacts 用不了{detail}，"
+                f"本节点进入退化模式：\n"
+                f"  仍会发 TactileStatus（fnormal/fshearx/fsheary、connected、"
+                f"接触去基线都正常），\n"
+                f"  但 contact 恒为 False、centroid_u/v 恒为 0"
+                f"（这两项依赖接触点消息）。\n"
+                f"  三力判据（本项目 S07 用的那个）不受影响。\n"
+                f"  要修：在**干净 shell** 里 source scripts/sim_env.sh（它会清掉真机"
+                f"环境残留的 PYTHONPATH/LD_LIBRARY_PATH 并补上 $CONDA_PREFIX/lib）。"
+            )
+
+    # --- 订阅回调 ---------------------------------------------------------
+    def _make_wrench_cb(self, side: str):
+        def cb(msg: Wrench) -> None:
+            self._got_wrench[side] = True
+            self._wrench[side] = (
+                float(msg.force.x),
+                float(msg.force.y),
+                float(msg.force.z),
+            )
+            self._wrench_stamp[side] = time.monotonic()
+
+        return cb
+
+    def _make_contact_cb(self, side: str):
+        def cb(msg: Contacts) -> None:
+            pts = []
+            for c in msg.contacts:
+                for p in c.positions:
+                    pts.append((float(p.x), float(p.y), float(p.z)))
+            if len(pts) < max(1, self.spec.min_contact_points):
+                return
+            n = float(len(pts))
+            self._got_contact[side] = True
+            self._contact_point[side] = (
+                sum(p[0] for p in pts) / n,
+                sum(p[1] for p in pts) / n,
+                sum(p[2] for p in pts) / n,
+            )
+            self._contact_stamp[side] = time.monotonic()
+
+        return cb
+
+    # --- 主循环 -----------------------------------------------------------
+    def _tick(self) -> None:
+        now = time.monotonic()
+        dt = now - self._last_tick
+        self._last_tick = now
+        self._baseline.advance(dt)
+
+        for s in self.spec.sides:
+            raw = self._wrench.get(s.side)
+            in_contact = self._in_contact(s, now)
+
+            if raw is None:
+                # 还没收到过力数据。仍然发布（下游靠"有没有帧"判活），
+                # 但发的是零 + connected=False，让下游能区分"没数据"与"零力"。
+                self._publish(s.side, connected=False, contact=False,
+                              forces=(0.0, 0.0, 0.0), stamp=now)
+                continue
+
+            for i, axis in enumerate(("x", "y", "z")):
+                self._baseline.add(f"{s.side}_{axis}", raw[i], dt, in_contact)
+
+            delta = tuple(
+                self._baseline.delta(f"{s.side}_{axis}", raw[i]) for i, axis in enumerate(("x", "y", "z"))
+            )
+            fn, fsx, fsy = self.spec.axis.project(delta, self.spec.force_scale)
+            # 真机 fnormal 也是非负的（法向只能压不能拉），这里保持一致。
+            fn = max(0.0, fn)
+            self._publish(s.side, connected=True, contact=in_contact,
+                          forces=(fn, fsx, fsy), stamp=now, side_spec=s)
+
+        self._maybe_check_topics(now)
+
+    def _in_contact(self, s: TactileSide, now: float) -> bool:
+        """接触 = 最近 contact_timeout_s 内收到过接触消息。
+
+        ⚠️ 不能写成"收到消息就一直算接触"：接触传感器是事件式的，
+        离开工件后不会发"取消接触"的消息，那样 contact 会永远停在 True。
+        """
+        stamp = self._contact_stamp.get(s.side)
+        return stamp is not None and (now - stamp) <= self.spec.contact_timeout_s
+
+    def _centroid(self, s: TactileSide) -> Tuple[float, float]:
+        """接触点在指尖坐标系里沿两个切向轴的偏移（毫米）→ (u, v)。
+
+        真机的 centroid_u/v 是形变图的像素坐标；仿真没有成像，用"接触点在指腹
+        平面上的位置"表达同一件事 —— 同样是"接触发生在指腹的哪里"。
+        下游目前不消费这两个字段，所以量纲不同不会造成影响；
+        真要跨仿真/真机共用，这里是最需要先统一的一处。
+        """
+        pt = self._contact_point.get(s.side)
+        if pt is None:
+            return (0.0, 0.0)
+        try:
+            tr = self._tf_buffer.lookup_transform(s.frame, "world", Time())
+        except Exception:
+            return (0.0, 0.0)
+        t = tr.transform.translation
+        q = tr.transform.rotation
+        # world → frame：位置平移 + 用共轭四元数旋转
+        px, py, pz = pt[0] - t.x, pt[1] - t.y, pt[2] - t.z
+        # 逆旋转：q* v q
+        qx, qy, qz, qw = -q.x, -q.y, -q.z, q.w
+        ix = qw * px + qy * pz - qz * py
+        iy = qw * py + qz * px - qx * pz
+        iz = qw * pz + qx * py - qy * px
+        iw = -qx * px - qy * py - qz * pz
+        vx = ix * qw + iw * -qx + iy * -qz - iz * -qy
+        vy = iy * qw + iw * -qy + iz * -qx - ix * -qz
+        vz = iz * qw + iw * -qz + ix * -qy - iy * -qx
+        i_sx, i_sy = self.spec.axis.shear_indices()
+        comp = (vx, vy, vz)
+        return (comp[i_sx] * 1000.0, comp[i_sy] * 1000.0)
+
+    def _publish(self, side: str, connected: bool, contact: bool,
+                 forces: Tuple[float, float, float], stamp: float,
+                 side_spec: Optional[TactileSide] = None) -> None:
+        msg = TactileStatus()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = f"{side}_tactile"
+        msg.side = side
+        msg.connected = connected
+        msg.contact = contact
+        msg.fnormal = float(forces[0])
+        msg.fshearx = float(forces[1])
+        msg.fsheary = float(forces[2])
+        # 仿真没有光学帧率。填 0 而不是编一个 30 —— 假数据比缺数据更难查。
+        msg.fps = 0.0
+        if contact and side_spec is not None:
+            msg.centroid_u, msg.centroid_v = (float(v) for v in self._centroid(side_spec))
+        else:
+            msg.centroid_u = 0.0
+            msg.centroid_v = 0.0
+        msg.error_msg = ""
+        self._pub_status[side].publish(msg)
+
+        pub = self._pub_image.get(side)
+        if pub is not None:
+            p_img, p_vec, hz, last = pub
+            now = time.monotonic()
+            if now - last >= 1.0 / max(1e-3, hz):
+                self._pub_image[side] = (p_img, p_vec, hz, now)
+                self._publish_placeholders(side, p_img, p_vec)
+
+    def _publish_placeholders(self, side: str, p_img, p_vec) -> None:
+        stamp = self.get_clock().now().to_msg()
+        w, h = self.spec.image_width, self.spec.image_height
+
+        depth = _depth_placeholder(w, h)
+        img = Image()
+        img.header.stamp = stamp
+        img.header.frame_id = f"{side}_tactile"
+        img.height, img.width = h, w
+        img.encoding = "32FC1"
+        img.is_bigendian = False
+        img.step = w * 4
+        img.data = depth.tobytes()
+        p_img.publish(img)
+
+        vec = _vector_placeholder(w, h)
+        vimg = Image()
+        vimg.header.stamp = stamp
+        vimg.header.frame_id = f"{side}_tactile"
+        vimg.height, vimg.width = h, w
+        vimg.encoding = "32FC2"
+        vimg.is_bigendian = False
+        vimg.step = w * 8
+        vimg.data = vec.tobytes()
+        p_vec.publish(vimg)
+
+    def _maybe_check_topics(self, now: float) -> None:
+        """启动几秒后确认力话题真的有数据。
+
+        这一步存在的唯一理由：漏声明 forcetorque-system 插件时，gz 不报错、
+        话题不出现、本节点也照常发零力 —— 整条链路"看起来在工作"。
+        主动检查并指出可能原因是唯一能打破这种静默的地方。
+        """
+        if self._checked or now < self._check_at:
+            return
+        self._checked = True
+        missing = [s.side for s in self.spec.sides if not self._got_wrench[s.side]]
+        if not missing:
+            return
+        self.get_logger().error(
+            f"启动 {TOPIC_CHECK_DELAY_S:.0f}s 后仍未收到力数据（{'/'.join(missing)}）。\n"
+            f"  最可能的原因：world 文件里没有声明 forcetorque-system 插件。\n"
+            f"  只要 world 声明了任何 <plugin>，gz 就不再加载默认系统列表，于是\n"
+            f"  force_torque 传感器**静默失效**：话题不出现、日志空白。\n"
+            f"  检查 devices/sim/gazebo/worlds/assembly.world 里是否有\n"
+            f"  libignition-gazebo6-forcetorque-system.so 那一项。"
+        )
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = SimTactileAdapter()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/sim_truth_perception.py

@@ -0,0 +1,706 @@
+#!/usr/bin/env python3
+"""仿真"真值感知"：把 Gazebo 里**已知的真实位姿**当成视觉测量发给伺服。
+
+为什么要这个节点
+----------------
+真机的视觉链是 ``perception_node``（YOLO/ONNX）→ 话题
+``visual_servo/filtered_point``。仿真环境（ros_gz_sim conda）里没有
+torch/ultralytics，而且这台机器**离线**装不上，于是 perception_node 一启动就死。
+
+死法还很误导人：S00 只看"权重文件在不在"（在），S04 一阶段则一直等
+``/visual_servo/locked``，等满 150s 报"没锁定（视野里没工件？）"——
+现场看到的是"算法不收敛/标定不准"，真因是"仿真里根本没有推理后端"。
+
+仿真里其实**不需要推理**：工件摆在哪、长什么样都是已知的
+（``scene.yaml`` 的绝对位姿 + 模型自己的几何）。伺服要的那个"相机系下的
+目标点"可以直接算出来：
+
+    世界系目标点 = 物体世界 XY 轮廓中心 + 高度（模型顶面）  ＋ 可选局部偏移
+    相机系目标点 = T_cam_world · 世界系目标点                （T 由 TF 查，不自己算）
+
+    发布 → ``PointStamped(visual_servo/filtered_point)``
+          与 perception_node **同话题、同类型**，所以下游（servo_node、
+          整条装配流程）一个字都不用改。
+
+⚠️ 它验证的是**控制那一半**（坐标变换 / 手眼 / 伺服收敛 / 流程编排），
+   不是感知那一半。真值箱体没有像素噪声、没有遮挡、没有光照变化，也没有
+   "算法把孔认成别的"这类错。所以：
+
+     能跑通 = "控制侧与接口是对的"
+     不能跑通 ≠ "真机上也能跑通"          ← 规律写在
+     devices/sim/gazebo/README.md 的「仿真里哪些模块用真值替代」一节
+
+绝不做的事
+----------
+* 目标在相机背后 / 相机查不到 TF 时**不发假点**（和 perception_node 的
+  "没有可用深度就不发点"同一条规矩）。宁可让伺服看门狗超时，也不给它一个
+  幻影目标 —— 后者会让机械臂朝错误方向走，而且看起来像"标定坏了"。
+* **物体被撞走了就不再发点**。真值感知的整个前提是"物体就在 scene.yaml 写的位置"，
+  前提破了，算出来的就是**假测量**。所以本节点自己订阅 ``/world/<world>/pose/info``
+  （scene.yaml 的 ``pose_info`` 段）盯着部件的实际位姿，偏离名义位姿超过容差就停下
+  并**指名报错**。
+  为什么必须由这一侧盯（2026-09-24 的教训）：``perception.source=truth`` 是纯几何的，
+  它从不问 Gazebo 物体现在在哪。实测工件掉进台面通槽、落到地面之后，"真值"开始撒谎，
+  而现象全都指向别处（二阶段报"两孔间距 169mm 不像 k1/k2"、S07 一路下压不接触），
+  看起来像检测/标定/阈值坏了 —— 真正的原因只有一个，却没有任何一处日志说出来。
+  **谁假设的，谁负责查。**
+* **开局那一次例外**（scene.yaml 的 ``pose_info.reset_on_start``）：物体是"在名义位姿
+  上方 spawn、再落下来"的，落稳后实测偏 2.6mm（越过 2mm 容差）—— 那不是"被撞走"，
+  但看门狗分不出来，于是每轮开局都可能被这条假故障拦死。所以在**开局窗口内**
+  （``STARTUP_RESET_WINDOW_S``）把它 set_pose 回名义位姿，只做一次；
+  过了窗口再漂就是运行中被撞，必须报错停下、**绝不**搬（见
+  ``startup_reset_decision`` 与 ``_maybe_startup_reset``）。
+* 不自己算四元数→旋转矩阵（复用 ``handeye_lib.quat_xyzw_to_R``）。
+  这段换算以前在 4 个 launch 文件里各抄了一遍、错了 3 处，症状是
+  "总能到附近、永远差一点"。数学实现只能有一份。
+* 不自己算四元数→旋转矩阵（复用 ``handeye_lib.quat_xyzw_to_R``）。
+  这段换算以前在 4 个 launch 文件里各抄了一遍、错了 3 处，症状是
+  "总能到附近、永远差一点"。数学实现只能有一份。
+"""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Dict, Optional, Sequence, Tuple
+
+import numpy as np
+import rclpy
+from geometry_msgs.msg import PointStamped
+from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+from rclpy.time import Time
+from tf2_ros import Buffer, TransformListener
+from tf2_msgs.msg import TFMessage
+
+from arm_sim_gazebo import sim_live_pose, sim_pose_service, sim_scene
+
+#: 与 perception_node / servo_node 一致：点话题用 BEST_EFFORT + KEEP_LAST(5)。
+#: 匹配不上 QoS 的后果是"话题在、消息发了、订阅方一条也收不到"，
+#: 而两边单独看都正常 —— 这类问题排查成本极高，所以直接抄同一份配置。
+POINT_QOS = QoSProfile(
+    history=HistoryPolicy.KEEP_LAST,
+    depth=5,
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+)
+
+#: 目标点离相机光心太近（或跑到背后）就不发：z<=0 在相机系里是"背后"。
+MIN_CAM_Z_M = 0.01
+
+#: 启动复位的**时间窗口** [s]：只在这段时间内把工件搬回名义位姿。
+#: 过了这段时间还漂 = 运行中被撞走/被压走，那时**绝不能**悄悄搬回去
+#: （机器人可能正握着它，搬回去等于把现场证据抹掉并让真值开始撒谎），
+#: 必须照旧报错停下。整个理由见 ``_maybe_startup_reset``。
+STARTUP_RESET_WINDOW_S = 20.0
+
+
+class TruthError(RuntimeError):
+    """真值目标解不出来（场景 / 模型 / 参数有问题）。
+
+    单独一个异常类型是为了让"配置错了"和"TF 还没到"分开处理：
+    前者要直接起不来（早报），后者只该刷限流告警（等机器人动起来就有了）。
+    """
+
+
+#: ``startup_reset_decision`` 的决定：不动 / 再等等 / 现在搬 / 放弃。
+RESET_SKIP = "skip"
+RESET_WAIT = "wait"
+RESET_NOW = "reset"
+RESET_GIVE_UP = "give_up"
+
+
+def startup_reset_decision(
+    now_s: float,
+    deadline_s: Optional[float],
+    window_s: float,
+    pose_known: bool,
+    any_drift: bool,
+) -> Tuple[str, Optional[float]]:
+    """开局"要不要把物体搬回名义位姿"的判据 → ``(决定, 新的截止时刻)``。
+
+    **纯函数**：写错的表现两头都很糟 ——
+      * 判成"运行中也搬"：把"谁把它撞歪了"的现场抹掉，甚至把工件从爪里拽走，
+        现象会变成"孔位对不上/下压不接触"，比原故障更难查；
+      * 判成"永远等"：开局那 2.6mm 落地偏差每轮都把真值点拦死，
+        S04 报「一个视觉目标点都没收到」，看起来像真值节点根本没起来。
+
+    所以它必须能脱离节点被钉住（``test_sim_truth_perception.py``）。
+    """
+    dl = float(deadline_s) if deadline_s is not None else float(now_s) + float(window_s)
+    if now_s > dl:
+        return RESET_GIVE_UP, dl
+    if not pose_known:
+        # 还没收到 pose/info 帧：此时"有没有漂移"根本无从判断，
+        # 等下去（窗口到点会自己变成 give_up）。
+        return RESET_WAIT, dl
+    if not any_drift:
+        return RESET_SKIP, dl
+    return RESET_NOW, dl
+
+
+@dataclass(frozen=True)
+class TruthTarget:
+    """真值目标：一个**世界系**点，以及它是怎么算出来的（给人看）。"""
+
+    object_name: str
+    world_xyz: Tuple[float, float, float]
+    note: str
+
+
+def target_point_world(
+    spec: sim_scene.SceneSpec,
+    obj: sim_scene.SceneObject,
+    offset_m: Sequence[float] = (0.0, 0.0, 0.0),
+    root: Optional[Path] = None,
+) -> TruthTarget:
+    """物体顶面中心（世界系）＋ 物体局部系偏移。
+
+    为什么取"顶面中心"：真机 perception_node 默认 ``feature_mode: plane``，
+    发的是检测框在**工件上表面**上的几何中心。真值要想替得住它，就得发同一个
+    物理点；差一个板厚（12mm）会让对比结论完全反过来。
+
+    为什么用 ``sim_scene.footprint`` 而不是自己按 yaw 算：
+    轮廓换算里已经含了 yaw（``world_boxes`` 里有且只有一份实现），
+    自己再写一遍迟早不一致，而"哪个才是对的"很难判断。
+
+    ``offset_m`` 是**物体局部系**里的偏移（会跟着 yaw 转），用途例如：
+    把目标从"板中心"挪到 k1 孔 ``[0, -0.029, 0]``，用来单独验证二阶段对孔。
+    """
+    fp = sim_scene.footprint(spec, obj, root)
+    height = sim_scene.model_height(spec, obj, root)
+    if fp is None or height is None:
+        raise TruthError(
+            f"读不出 {obj.name!r} 的几何（模型 {obj.model}）。\n"
+            f"  期望：{sim_scene.model_sdf_path(spec, obj, root)} 里有 <collision>\n"
+            f"  修法：确认 models_dir 指对了、模型目录里有 model.sdf 且至少一个碰撞体。"
+        )
+    vals = [float(v) for v in offset_m]
+    if len(vals) != 3:
+        # 长度不对必须报错，不能"缺的当 0"：把 [0.03] 当成 (0.03,0,0) 还是
+        # (0,0.03,0) 全凭猜，而猜错的表现是"偏了一点"，很难归因。
+        raise TruthError(
+            f"offset_m 要给 3 个数（物体局部系 xyz，单位米），现在给的是 {list(offset_m)!r}。"
+        )
+    ox, oy, oz = vals
+    cx = 0.5 * (fp[0] + fp[1])
+    cy = 0.5 * (fp[2] + fp[3])
+    c, s = math.cos(float(obj.yaw)), math.sin(float(obj.yaw))
+    x = cx + c * ox - s * oy
+    y = cy + s * ox + c * oy
+    z = float(obj.z) + float(height) + oz
+    note = (
+        f"{obj.name}（模型 {obj.model}）"
+        f"轮廓中心 ({cx:.3f},{cy:.3f}) + 高 {height * 1000:.1f}mm"
+        f" + 局部偏移 ({ox * 1000:+.0f},{oy * 1000:+.0f},{oz * 1000:+.0f})mm"
+        f"  yaw={math.degrees(float(obj.yaw)):.1f}°"
+    )
+    return TruthTarget(object_name=obj.name, world_xyz=(x, y, z), note=note)
+
+
+def pick_object(spec: sim_scene.SceneSpec, name: str) -> sim_scene.SceneObject:
+    """按名字找物体；找不到时把**现有名字**列出来（改配置时不用再翻文件）。"""
+    want = (name or "").strip() or spec.operating.part_object
+    for obj in spec.objects:
+        if obj.name == want:
+            return obj
+    raise TruthError(
+        f"场景 {spec.source} 里没有名为 {want!r} 的物体。\n"
+        f"  现有物体：{', '.join(o.name for o in spec.objects) or '（空）'}\n"
+        f"  修法：改 site.yaml 的 perception.truth.object，"
+        f"或确认 scene.yaml 的 objects 里确实有它。"
+    )
+
+
+def load_truth_target(
+    scene_file: Optional[Path],
+    object_name: str,
+    offset_m: Sequence[float] = (0.0, 0.0, 0.0),
+    root: Optional[Path] = None,
+) -> Tuple[sim_scene.SceneSpec, TruthTarget]:
+    """读场景 + 解出真值目标。返回 ``(场景, 目标)``，场景用于启动日志。
+
+    ``object_name`` 空 = 用场景里的 ``operating.part_object``：**"哪个是工件"
+    是场景的知识，不是流程的知识**，所以默认跟着场景走，换场景不用改流程参数。
+    """
+    spec = sim_scene.load_scene(scene_file, root)
+    return spec, target_point_world(spec, pick_object(spec, object_name), offset_m, root)
+
+
+def apply_transform(T: np.ndarray, p: Sequence[float]) -> Tuple[float, float, float]:
+    """4×4 齐次变换作用于点。"""
+    q = np.asarray(T, dtype=float) @ np.array([p[0], p[1], p[2], 1.0], dtype=float)
+    return (float(q[0]), float(q[1]), float(q[2]))
+
+
+def matrix_from_transform(tr) -> np.ndarray:
+    """``TransformStamped`` → 4×4 齐次矩阵。
+
+    四元数→旋转矩阵**不在这里实现**：复用 ``handeye_lib.quat_xyzw_to_R``
+    （那是全仓库唯一一份，且有回环测试钉着）。重复实现的下场见模块 docstring。
+    """
+    from arm_application.handeye_lib import quat_xyzw_to_R
+
+    t = tr.transform.translation
+    q = tr.transform.rotation
+    T = np.eye(4, dtype=float)
+    T[:3, :3] = quat_xyzw_to_R([q.x, q.y, q.z, q.w])
+    T[0, 3] = float(t.x)
+    T[1, 3] = float(t.y)
+    T[2, 3] = float(t.z)
+    return T
+
+
+def pose_deviation(
+    obj: sim_scene.SceneObject,
+    p: Sequence[float],
+    quat: Sequence[float],
+) -> Tuple[float, float, float]:
+    """实际位姿相对名义位姿的偏差 → ``(平移 [m], 姿态夹角 [deg], 实测 yaw [rad])``。
+
+    **纯函数**：这条判据搞错了的表现是"监视器静默失声" ——
+    要么永远不报（物体明明被撞走了却没人说），要么天天误报（现场学会忽略它）。
+    两种情况都比没有监视更糟，所以它必须能脱离节点被钉住。
+    """
+    from arm_application.handeye_lib import quat_xyzw_to_R
+
+    d = math.dist((float(obj.x), float(obj.y), float(obj.z)), [float(v) for v in p[:3]])
+    R_now = quat_xyzw_to_R([float(v) for v in quat])
+    yaw_now = math.atan2(float(R_now[1, 0]), float(R_now[0, 0]))
+    # 姿态用**整段旋转差**的夹角，不只比 yaw：板被碰一下往往是"翘起来夹着转了一点"，
+    # 只比 yaw 会漏掉翘起，而翘起恰恰最影响孔的成像位置。
+    c, s = math.cos(float(obj.yaw)), math.sin(float(obj.yaw))
+    R_ref = np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
+    cosang = (float(np.trace(R_ref.T @ R_now)) - 1.0) / 2.0
+    ang = math.degrees(math.acos(max(-1.0, min(1.0, cosang))))
+    return (d, ang, yaw_now)
+
+
+def pose_drift(
+    obj: sim_scene.SceneObject,
+    p: Sequence[float],
+    quat: Sequence[float],
+    tol_m: float,
+    tol_deg: float,
+) -> Optional[str]:
+    """偏差超过容差时的**说明文字**；在容差内返回 ``None``（= 没漂移）。
+
+    给日志/告警用的人话版；数值判据在 :func:`pose_deviation`（同一个实现，
+    免得两处各算一遍距离）。
+    """
+    d, ang, yaw_now = pose_deviation(obj, p, quat)
+    if d <= tol_m and ang <= tol_deg:
+        return None
+    return (
+        f"名义 (x,y,z)=({obj.x:.4f},{obj.y:.4f},{obj.z:.4f}) "
+        f"yaw={math.degrees(float(obj.yaw)):.1f}°\n"
+        f"        实际 (x,y,z)=({p[0]:.4f},{p[1]:.4f},{p[2]:.4f}) "
+        f"yaw={math.degrees(yaw_now):.1f}°\n"
+        f"        偏差 |Δp|={d * 1000:.1f}mm、姿态差 {ang:.1f}°"
+        f"（容差 {tol_m * 1000:.1f}mm / {tol_deg:.1f}°）"
+    )
+
+
+class SimTruthPerception(Node):
+    """把真值目标点按固定频率发到 ``visual_servo/filtered_point``。"""
+
+    def __init__(self) -> None:
+        super().__init__("sim_truth_perception")
+        self.declare_parameter("scene_file", "")
+        """场景文件（空 = 仓库根下 devices/sim/gazebo/config/scene.yaml）。"""
+
+        self.declare_parameter("target_object", "")
+        """目标物体名（空 = 场景里的 operating.part_object）。"""
+
+        self.declare_parameter("offset_m", [0.0, 0.0, 0.0])
+        """物体局部系偏移 [m]，例如挪到某个孔：``[0.0, -0.029, 0.0]``。"""
+
+        self.declare_parameter("camera_frame", "")
+        """相机光学系（由 launch 按当前臂/相机传进来，不写死）。"""
+
+        self.declare_parameter("world_frame", "world")
+        """场景的世界系。Gazebo 里模型位姿就是相对它，TF 树上也有。"""
+
+        self.declare_parameter("point_topic", "visual_servo/filtered_point")
+        self.declare_parameter("publish_hz", 20.0)
+        """发布频率：不必跟着相机帧率，伺服 100Hz 控制会自己插值/外推。"""
+
+        self.declare_parameter("pose_info_topic", "")
+        """世界模型位姿话题（``tf2_msgs/TFMessage``）。
+
+        空 = 用 scene.yaml 的 ``pose_info.ros_topic``；场景里也没配 = 真值**按名义
+        位姿**算（几何会偏，且没有"物体被搬走"的告警）。"""
+
+        scene_file = str(self.get_parameter("scene_file").value or "").strip()
+        self._scene_file = Path(scene_file) if scene_file else None
+        self._offset = offset = self.get_parameter("offset_m").value or [0.0, 0.0, 0.0]
+        # 长度的校验在 target_point_world 里（纯函数，有测试钉着），这里不重复。
+        self._camera_frame = str(self.get_parameter("camera_frame").value or "").strip()
+        if not self._camera_frame:
+            raise TruthError(
+                "camera_frame 没给。它由 visual_servo.launch.py 按当前臂的相机传进来"
+                "（site.yaml → cameras/ perception.camera_frame）。"
+                "手工跑这个节点时必须显式指定，例如 "
+                "--ros-args -p camera_frame:=sim_d405_color_optical_frame"
+            )
+        self._world_frame = str(self.get_parameter("world_frame").value or "world").strip()
+
+        # 目标点在启动时算一次就够：场景是静态的（工件被推动后位置会变，
+        # 但那种情况下"真值"本身也就失效了 —— 此时应该重跑 gen_sim_handeye
+        # 或重启，而不是悄悄跟着一个被撞歪的位姿走）。
+        # ⚠️ 这句话在 2026-09-24 之前只是**注释**：工件真的被碰走了，没有任何人
+        #    发现（详见 scene.yaml 的 pose_info 段）。现在由 _setup_drift_watch
+        #    把它变成**运行时的判据**：一旦对上不，立刻停发并指名报错。
+        self._spec, self._target = load_truth_target(
+            self._scene_file,
+            str(self.get_parameter("target_object").value or ""),
+            offset,
+        )
+
+        self._pub = self.create_publisher(
+            PointStamped, str(self.get_parameter("point_topic").value), POINT_QOS
+        )
+        self._tf = Buffer()
+        TransformListener(self._tf, self, spin_thread=True)
+        self._sent = 0
+        self._warned_tf = 0
+        self._warned_drift = 0
+        self._t_start: Optional[float] = None
+        # 目标点按**实测**位姿算（见模块 docstring 与 sim_live_pose），
+        # 位姿没变就不重算（算一次要读模型 SDF，20Hz 下不能每拍都读）。
+        self._target_live: Optional[TruthTarget] = None
+        self._target_key: Optional[Tuple[float, ...]] = None
+        self._warned_nominal = 0
+        # 启动复位（scene.yaml 的 pose_info.reset_on_start）：见 _maybe_startup_reset。
+        self._reset_done = False
+        self._reset_deadline: Optional[float] = None
+        self._setup_drift_watch()
+
+        hz = max(1.0, min(100.0, float(self.get_parameter("publish_hz").value)))
+        self.create_timer(1.0 / hz, self._tick)
+        self.get_logger().warn(
+            "真值感知已启动（仿真专用）：不跑任何推理，直接把已知位姿当测量发出去。\n"
+            f"  场景 {self._spec.source}\n"
+            f"  目标 {self._target.note}\n"
+            f"  世界系点 ({self._target.world_xyz[0]:.3f}, "
+            f"{self._target.world_xyz[1]:.3f}, {self._target.world_xyz[2]:.3f})  "
+            f"→ 相机系 {self._camera_frame}（TF: {self._world_frame} → "
+            f"{self._camera_frame}）  频率 {hz:.0f}Hz\n"
+            f"  ⚠️ 它验证的是控制与接口，不是检测算法 —— 见 "
+            f"devices/sim/gazebo/README.md「仿真里哪些模块用真值替代」"
+        )
+
+    # ------------------------------------------------------------------
+    # 漂移监视：真值感知的"前提自检"
+    # ------------------------------------------------------------------
+    def _setup_drift_watch(self) -> None:
+        """订阅世界模型位姿，盯住"被真值当作几何基准"的物体现在在哪。
+
+        两件事一起做，因为它们是同一份数据：
+          * **量测**：给真值目标/孔位提供**实测**位姿（``sim_live_pose``）——
+            这才是真值该用的几何（见模块 docstring）；
+          * **告警**：实测位姿相对 ``scene.yaml`` 的名义位姿偏了多少。
+            超过 ``drift_tol_m`` **告警但继续发**（几何已经跟着实测走，
+            没有幻影目标）；超过 ``drift_stop_m`` 才**停发**并指名报错
+            （那个量级说明物体掉了/被搬走了，场景前提没了）。
+
+        为什么告警与停发要分开（2026-09-24 实测）：
+            物体 spawn 落地就偏 2.6mm、骑在通槽上还会缓慢再挪 1.9mm ——
+            这个量级**天天都会出现**，而它正好压在"2mm"这条线上。用同一条阈值
+            既停发又告警，结果是"每轮开局都可能被假故障拦死"，
+            而现场的正确反应是"忽略这条告警" —— 监视器一旦被学会忽略就废了。
+
+        只盯**工件**：它是二阶段孔位与伺服目标的几何基准。
+        通止规刻意不盯 —— 它本来就要被夹走、被搬动，那正是流程的一部分。
+        """
+        pi = self._spec.pose_info
+        topic = str(self.get_parameter("pose_info_topic").value or "").strip()
+        if not topic and pi is not None:
+            topic = pi.ros_topic
+        # 只盯工件：它是二阶段孔位与伺服目标的几何基准。
+        want = self._spec.operating.part_object
+        self._nominal_obj = next((o for o in self._spec.objects if o.name == want), None)
+        self._tol_m = float(pi.drift_tol_m if pi is not None else 0.002)
+        self._tol_deg = float(pi.drift_tol_deg if pi is not None else 2.0)
+        self._stop_m = float(getattr(pi, "drift_stop_m", 0.02) if pi is not None else 0.02)
+        # 启动复位要用 world 名（服务路径里），而 world 名从 gz_topic 推出来 ——
+        # 不另配一个参数：两份写法迟早不一致，症状是"服务调用超时"。
+        self._reset_world = (
+            sim_pose_service.world_from_gz_topic(pi.gz_topic) if pi is not None else None
+        )
+        self._reset_on_start = bool(pi.reset_on_start) if pi is not None else False
+        self._live = sim_live_pose.LiveScenePose(self, topic)
+
+        if self._nominal_obj is None:
+            self.get_logger().warn(
+                f"scene.yaml 的 operating.part_object={want!r} 不在 objects 里 —— "
+                f"实测位姿无从取得（只能按名义位姿算，几何会偏）。"
+            )
+        if not topic:
+            self.get_logger().info(
+                "没有世界模型位姿话题（scene.yaml 的 pose_info 未配）—— "
+                "真值将**按名义位姿**算目标点与孔位（物体一动就偏，实测偏 2~3mm "
+                "就会让规尖进不了孔），而且没有「物体被搬走」的告警。"
+            )
+            return
+        self.get_logger().info(
+            f"已订阅 {topic}：真值目标按**实测**位姿算；与该物体在 scene.yaml 的"
+            f"名义位姿相差超过 {self._tol_m * 1000:.1f}mm / {self._tol_deg:.1f}° 告警，"
+            f"超过 {self._stop_m * 1000:.0f}mm 停发。"
+        )
+        if self._reset_on_start:
+            if self._reset_world is None:
+                self.get_logger().error(
+                    f"pose_info.reset_on_start=true，但从 gz_topic "
+                    f"{pi.gz_topic!r} 里认不出 world 名 —— **启动复位不会执行**。\n"
+                    f"        期望形如 /world/<world>/pose/info，world 名 = "
+                    f"worlds/assembly.world 的文件名。\n"
+                    f"        后果：spawn 落地的 2~3mm 偏差会被当成"
+                    f"「场景被动过」，告警一直刷。"
+                )
+            else:
+                self.get_logger().info(
+                    f"启动复位已开：开局（{STARTUP_RESET_WINDOW_S:.0f}s 内）会把被盯的物体"
+                    f"搬回名义位姿（set_pose /world/{self._reset_world}/set_pose）。"
+                )
+
+    # ------------------------------------------------------------------
+    # 实测位姿 → 目标点
+    # ------------------------------------------------------------------
+    def _live_deviation(self) -> Optional[Tuple[float, float, str]]:
+        """实测位姿相对名义位姿的偏差 ``(d [m], ang [deg], 说明)``；没有实测返回 None。"""
+        obj = getattr(self, "_nominal_obj", None)
+        if obj is None:
+            return None
+        p = self._live.pose(obj.name)
+        if p is None:
+            return None
+        d, ang, _ = pose_deviation(obj, p[:3], sim_pose_service.yaw_to_quat(p[3]))
+        reason = (
+            pose_drift(obj, p[:3], sim_pose_service.yaw_to_quat(p[3]), self._tol_m, self._tol_deg)
+            or ""
+        )
+        return (d, ang, reason)
+
+    def _current_target(self) -> Tuple[TruthTarget, bool]:
+        """按实测位姿算目标点（位姿没变就复用）。返回 ``(目标, 是否按实测算的)``。
+
+        实测位姿拿不到时**退回名义位姿**并把这个事实报给调用方 ——
+        静默退回是这条链上最贵的错（几何偏两毫米、没有任何日志提起）。
+        """
+        obj = getattr(self, "_nominal_obj", None)
+        p = self._live.pose(obj.name) if obj is not None else None
+        if obj is None or p is None:
+            return self._target, False
+        key = tuple(round(float(v), 7) for v in p)
+        if self._target_key == key and self._target_live is not None:
+            return self._target_live, True
+        spec_live = self._live.apply(self._spec, [obj.name])
+        live_obj = pick_object(spec_live, obj.name)
+        t = target_point_world(spec_live, live_obj, self._offset)
+        self._target_key = key
+        self._target_live = t
+        return t, True
+
+    # ------------------------------------------------------------------
+    # 启动复位：把"落地的 2~3mm"与"被撞走"分开
+    # ------------------------------------------------------------------
+    def _maybe_startup_reset(self) -> None:
+        """开局把被盯的物体**实际搬到**名义位姿（只做一次，且有时间窗）。
+
+        为什么需要（2026-09-24 实测，别再当玄学）：
+            物体是"在名义位姿上方 spawn、再落下来"的，落稳后实测偏 **2.6mm**
+            （(0.5175, 0.0008) vs 名义 (0.5200, 0.0000)），正好越过 2mm 容差 ——
+            它不是"被撞走"，但看门狗**分不出来**，于是每一轮开局都可能被这条
+            假故障拦死：S04 等不到目标点、报「一个视觉目标点都没收到」。
+            而把它 set_pose 回名义位姿之后，它**停在那儿不动**（实测）——
+            说明名义位姿本身就是稳定平衡点，差的只是 spawn 那一下的落点。
+
+        为什么只在窗口内做、且**只做一次**：
+            运行中漂移的唯一合法解释是"被机器人撞了/压了"。那时悄悄搬回去等于
+            抹掉现场证据（而现象会变成"孔位对不上/下压不接触"，更难查），
+            还可能把工件从爪里拽出来。所以过了窗口就照旧报错停下。
+
+        判据本身在纯函数 ``startup_reset_decision`` 里（有单测钉着），
+        这里只负责"按决定去调服务、并把话说明白"。
+        """
+        if self._reset_done or not self._reset_on_start or self._reset_world is None:
+            return
+        now = self.get_clock().now().nanoseconds * 1e-9
+        dev = self._live_deviation()
+        decision, deadline = startup_reset_decision(
+            now_s=now,
+            deadline_s=self._reset_deadline,
+            window_s=STARTUP_RESET_WINDOW_S,
+            pose_known=self._live.ready,
+            any_drift=bool(dev is not None and (dev[2] or dev[0] > self._tol_m)),
+        )
+        self._reset_deadline = deadline
+        if decision == "wait":
+            return
+        if decision == "skip":
+            self._reset_done = True  # 本来就在位（或还没量到足够信息）：不必搬
+            return
+        if decision == "give_up":
+            # 窗口内没成功（比如 set_pose 服务不通）→ 放弃并说清楚，
+            # 之后就是告警/停发的正常行为（真值几何已经跟着实测算，不影响发点）。
+            self._reset_done = True
+            self.get_logger().warn(
+                f"启动复位窗口（{STARTUP_RESET_WINDOW_S:.0f}s）内没能把物体搬回名义位姿 —— "
+                f"放弃复位；真值几何按实测位姿算（不受影响），"
+                f"但告警会一直刷「与名义位姿不一致」。"
+            )
+            return
+
+        self._reset_done = True
+        obj = getattr(self, "_nominal_obj", None)
+        if obj is None:
+            return
+        reason = (dev[2] if dev else "") or ""
+        ok, out = sim_pose_service.set_model_pose_yaw(
+            self._reset_world, obj.name, (obj.x, obj.y, obj.z), float(obj.yaw)
+        )
+        if ok:
+            self.get_logger().warn(
+                f"启动复位：{obj.name!r} 不在名义位姿"
+                f"（{reason.splitlines()[-1].strip() if reason else ''}）"
+                f"—— 已 set_pose 回 ({obj.x:.4f},{obj.y:.4f},{obj.z:.4f}) "
+                f"yaw={math.degrees(float(obj.yaw)):.1f}°。\n"
+                f"        这是**开局**的正常现象（spawn 落地会偏 2~3mm），不是故障；"
+                f"复位后实测位姿等于名义位姿。"
+            )
+        else:
+            self.get_logger().error(
+                f"启动复位失败：{obj.name!r} 没能 set_pose 回名义位姿 —— {out[:200]}\n"
+                f"        影响有限：真值几何按实测位姿算，仍能发点；但告警会一直刷。"
+            )
+
+    def _tick(self) -> None:
+        # 前提自检的**活性检查**：订阅了话题却一帧都没收到，等于没有监视 ——
+        # 而那正是"以为有保护、实际裸奔"。所以这里必须把这种情况说出来。
+        if self._live.topic and not self._live.ready:
+            if self._t_start is None:
+                self._t_start = self.get_clock().now().nanoseconds * 1e-9
+            elif self.get_clock().now().nanoseconds * 1e-9 - self._t_start > 8.0:
+                self.get_logger().warn(
+                    "盯物体位姿的话题**一帧都没收到** —— 真值只能退回**名义位姿**"
+                    "（几何会偏，实测偏 2~3mm 就能让规尖进不了孔）。\n"
+                    "        常见原因：gazebo_sim.launch.py 里的 pose_info_bridge 没起来，"
+                    "或 scene.yaml 的 pose_info.gz_topic 与 world 名对不上\n"
+                    "        （必须等于 /world/<world>/pose/info，world 名 = "
+                    "worlds/assembly.world 的文件名）。",
+                    throttle_duration_sec=10.0,
+                )
+        # 先给"开局落地偏差"一次纠偏机会（见 _maybe_startup_reset）。
+        self._maybe_startup_reset()
+
+        dev = self._live_deviation()
+        if dev is None and self._nominal_obj is not None and self._live.ready:
+            # 话题有帧，就是没有这个模型 → 名字对不上，说出来。
+            self.get_logger().warn(
+                f"实测位姿里没有物体 {self._nominal_obj.name!r}："
+                f"{self._live.why_not(self._nominal_obj.name)}",
+                throttle_duration_sec=10.0,
+            )
+        elif dev is not None:
+            d, ang, reason = dev
+            if d > self._stop_m:
+                # 这个量级说明物体掉了/被搬走了：场景前提没了，**不发点**。
+                self._warned_drift += 1
+                if self._warned_drift in (1, 5, 30) or self._warned_drift % 300 == 0:
+                    self.get_logger().error(
+                        f"仿真物体 {self._nominal_obj.name!r} 已经不在 scene.yaml 说的地方了 —— "
+                        f"**停止发真值点**（偏了 {d * 1000:.0f}mm，超过 "
+                        f"{self._stop_m * 1000:.0f}mm 这条线）。\n"
+                        f"        {reason}\n"
+                        f"        ⚠ 为什么停：物体掉了/被搬走之后，装配就没有意义了 ——"
+                        f"再发点等于指挥机械臂去装一个不存在的工件。\n"
+                        f"        修法：把 {self._nominal_obj.name!r} 放回 scene.yaml 的位置"
+                        f"（真机现场就改 scene.yaml），或重启仿真让它按场景重新 spawn。\n"
+                        f"        排查提示：台面沿 X 有一道 36mm 通槽（|y| ≤ 18mm）；"
+                        f"工件若跨槽时被压翻就会掉下去。"
+                    )
+                return
+            if reason:
+                # 2~3mm 这个量级**天天都有**（spawn 落地 + 通槽上缓慢蠕动），
+                # 而几何已经跟着实测走，所以只告警、**继续发点**。
+                self.get_logger().warn(
+                    f"物体 {self._nominal_obj.name!r} 与 scene.yaml 的名义位姿不一致 —— "
+                    f"真值几何已按**实测**位姿算，不受影响；只是提醒你场景被动过。\n"
+                    f"        {reason}\n"
+                    f"        （超过 {self._stop_m * 1000:.0f}mm 才停发）",
+                    throttle_duration_sec=30.0,
+                )
+
+        target, live = self._current_target()
+        if not live and self._warned_nominal < 3:
+            self._warned_nominal += 1
+            self.get_logger().warn(
+                f"真值目标暂时按**名义位姿**算（{self._live.why_not(target.object_name)}）—— "
+                f"物体一动几何就偏，实测偏 2~3mm 就会让规尖进不了孔。"
+            )
+        try:
+            tr = self._tf.lookup_transform(self._camera_frame, self._world_frame, Time())
+        except Exception as exc:  # noqa: BLE001 —— tf2 的异常类型很杂，统一当"还没有"
+            # 前几拍必然查不到（TF 要等 robot_state_publisher 起来），
+            # 所以只刷限流告警，不报错。
+            self._warned_tf += 1
+            if self._warned_tf in (1, 20, 100) or self._warned_tf % 200 == 0:
+                self.get_logger().warn(
+                    f"还查不到 TF {self._world_frame} → {self._camera_frame}"
+                    f"（已试 {self._warned_tf} 次）：{exc}",
+                    throttle_duration_sec=5.0,
+                )
+            return
+        p = apply_transform(matrix_from_transform(tr), target.world_xyz)
+        if p[2] < MIN_CAM_Z_M:
+            # 目标在相机背后或贴到镜头上：不发！发出去伺服会朝反方向走。
+            self.get_logger().warn(
+                f"真值目标不在相机前方（相机系 z={p[2] * 1000:.0f}mm < "
+                f"{MIN_CAM_Z_M * 1000:.0f}mm）—— 不发点。"
+                f"世界系点 ({target.world_xyz[0]:.3f}, "
+                f"{target.world_xyz[1]:.3f}, {target.world_xyz[2]:.3f})"
+                f" 不在 {self._camera_frame} 视野里；先让臂摆到能看到它的位置。",
+                throttle_duration_sec=5.0,
+            )
+            return
+
+        msg = PointStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = self._camera_frame
+        msg.point.x, msg.point.y, msg.point.z = p
+        self._pub.publish(msg)
+        self._sent += 1
+        if self._sent == 1:
+            self.get_logger().info(
+                f"首个真值目标点已发出：相机系 ({p[0]:.4f}, {p[1]:.4f}, {p[2]:.4f}) m"
+            )
+        elif self._sent % 200 == 0:
+            self.get_logger().debug(f"已发 {self._sent} 个真值点")
+
+
+def main(argv=None) -> int:
+    import traceback
+
+    rclpy.init(args=argv)
+    node = None
+    try:
+        node = SimTruthPerception()
+        rclpy.spin(node)
+        return 0
+    except TruthError as exc:
+        # 配置错：把原因原样打出来（它是给人看的），别让 ROS 的异常栈盖住。
+        print(f"真值感知起不来：{exc}", flush=True)
+        return 2
+    except Exception:  # noqa: BLE001
+        traceback.print_exc()
+        return 1
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/sim_wrist_ft.py

@@ -0,0 +1,159 @@
+"""仿真腕部力扭矩（wrist force/torque）的配置与桥接规则。
+
+这一层只做两件事，别的都不做：
+
+    1. 从 ``devices/sim/gazebo/config/wrist_ft.yaml`` 读话题与关节名
+    2. 生成 ros_gz_bridge 的参数（gz 消息 → ROS 消息）
+
+**为什么不需要适配器节点**（与触觉不同）：
+    触觉必须有 ``sim_tactile_adapter``，因为要做的语义工作很多 —— 轴向映射、
+    基线相减、单位换算、接触超时、把 ``Contacts`` 折算成 contact 标志。
+    腕部 FT 不需要任何这些：Gazebo 的 force_torque 直接给的就是**牛顿**，
+    坐标系是 ``child`` 连杆系（工具竖直时 z 即轴向），而 ros_gz_bridge
+    **原生支持** gz.msgs.Wrench → geometry_msgs/msg/WrenchStamped（实测确认）。
+    于是仿真的输出与真机 ``/<sn>/ft_sensor_wrench`` **同名同类型同单位**，
+    应用层一行分支都不用写。
+
+    少一层就少一个"配错了不报错"的地方，所以这里刻意不加节点。
+
+与真机的对应：
+    真机  /<sn>/ft_sensor_wrench   geometry_msgs/WrenchStamped（法兰系，牛顿）
+    仿真  /sim/ft_sensor_wrench    geometry_msgs/WrenchStamped（link7 系，牛顿）
+                                   ← 名字由 site.yaml 的 robot.arms[sim].sn 决定
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any, Dict, List, Optional
+
+import yaml
+
+#: 相对仓库根的默认配置路径。
+DEFAULT_WRIST_FT_REL = "devices/sim/gazebo/config/wrist_ft.yaml"
+
+
+class WristFtConfigError(RuntimeError):
+    """腕部 FT 配置缺失或写错。消息里必须给出修法（现场就靠这句话）。"""
+
+
+@dataclass(frozen=True)
+class WristFtSpec:
+    """腕部 FT 的一份完整描述。"""
+
+    topic: str
+    joint: str
+    ros_type: str
+    gz_type: str
+    publish_hz: float
+
+
+def load_wrist_ft(
+    path: Optional[Path] = None, root: Optional[Path] = None
+) -> WristFtSpec:
+    """读腕部 FT 配置。找不到或写错时抛 WristFtConfigError，消息里给出修法。"""
+    if path is None:
+        base = root if root is not None else Path.cwd()
+        path = Path(base) / DEFAULT_WRIST_FT_REL
+    path = Path(path)
+
+    if not path.is_file():
+        raise WristFtConfigError(
+            f"腕部 FT 配置不存在: {path}\n"
+            f"  它声明了仿真腕部力扭矩的话题名与挂载关节（桥接从它派生）。\n"
+            f"  缺了它，S07 的接触判据在仿真里永远没有数据源。\n"
+            f"  确认仓库根下存在 {DEFAULT_WRIST_FT_REL}。"
+        )
+
+    try:
+        doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
+    except yaml.YAMLError as exc:
+        raise WristFtConfigError(
+            f"腕部 FT 配置 YAML 解析失败: {path}\n  原因: {exc}"
+        ) from exc
+
+    if not isinstance(doc, dict):
+        raise WristFtConfigError(f"{path} 的顶层应该是映射（key: value 形式）。")
+
+    missing = [
+        k for k in ("topic", "joint", "ros_type", "gz_type") if not str(doc.get(k, "")).strip()
+    ]
+    if missing:
+        raise WristFtConfigError(
+            f"{path} 缺字段: {', '.join(missing)}。\n"
+            f"  topic 必须与 arm.sim.sensors.xacro 里 wrist_ft_topic 的默认值\n"
+            f"  完全一致，也要与应用层按 sn 推出的名字一致 —— 对不上时桥接器只是\n"
+            f"  订阅了一个不存在的话题，不报错，而 S07 会一直判不到接触。"
+        )
+
+    try:
+        hz = float(doc.get("publish_hz", 100.0))
+    except (TypeError, ValueError) as exc:
+        raise WristFtConfigError(
+            f"{path} 的 publish_hz 不是数字: {doc.get('publish_hz')!r}"
+        ) from exc
+    if hz <= 0.0:
+        raise WristFtConfigError(f"{path} 的 publish_hz 必须为正，收到 {hz}")
+
+    return WristFtSpec(
+        topic=str(doc["topic"]).strip(),
+        joint=str(doc["joint"]).strip(),
+        ros_type=str(doc["ros_type"]).strip(),
+        gz_type=str(doc["gz_type"]).strip(),
+        publish_hz=hz,
+    )
+
+
+def bridge_args(spec: WristFtSpec) -> List[str]:
+    """生成 ros_gz_bridge parameter_bridge 的话题规则。
+
+    只做**传输**：gz 类型 → ROS 类型，话题名两侧保持同名。
+    这条规则的终点就是应用层真正订阅的那个话题，所以话题名必须一次写对 ——
+    桥接器没有"改名"能力，写错只会得到"话题上没数据"。
+    """
+    return [f"{spec.topic}@{spec.ros_type}[{spec.gz_type}"]
+
+
+def check_wrist_ft_sensor(
+    urdf_text: str, spec: WristFtSpec
+) -> List[str]:
+    """描述级静态检查：URDF 里到底有没有那个腕部 FT 传感器。
+
+    为什么需要（与 ``check_tactile_collisions`` 同一个理由）：
+        Gazebo 对**挂错关节**的传感器不报错 —— 传感器会被静默丢弃，
+        话题永远没有数据，现象是"S07 判不到接触"。光看 launch 日志完全看不出来
+        （桥接器和 Gazebo 都说一切正常）。这里在启动时就把它指出来。
+
+    返回告警列表（空 = 没问题）；**不抛异常** —— 缺了它仿真仍能跑，
+    只是 S07 的判据要退回触觉兜底，那是降级不是崩溃。
+    """
+    warnings: List[str] = []
+    if not urdf_text:
+        return ["URDF 文本为空，无法核对腕部 FT 传感器"]
+
+    if f'reference="{spec.joint}"' not in urdf_text:
+        near = [ln for ln in urdf_text.splitlines() if "gazebo reference=" in ln and "joint" in ln]
+        warnings.append(
+            f"URDF 里没有 <gazebo reference=\"{spec.joint}\"> —— "
+            f"腕部 FT 传感器挂错关节了，它会被 Gazebo 静默丢弃、话题永远没数据。\n"
+            f"  期望挂在 {spec.joint}（最后一个 revolute 关节：它下游挂着整个腕部以下"
+            f" 的载荷，这才是工程意义上的腕部 FT）。\n"
+            f"  注意不能挂 fixed 关节（如 flange_joint）——物理引擎不把它当独立关节"
+            f"  求解，传感器永远没输出且不报错。\n"
+            f"  当前 URDF 里的 <gazebo reference> 关节有: {', '.join(sorted(set(near))) or '（无）'}"
+        )
+
+    if spec.topic not in urdf_text:
+        warnings.append(
+            f"URDF 里找不到腕部 FT 的话题 {spec.topic} —— 传感器的话题名与配置不一致。\n"
+            f"  两侧必须一致：config 的 topic、arm.sim.sensors.xacro 的 wrist_ft_topic、\n"
+            f"  以及应用层按 sn 推出的 /{{sn}}/ft_sensor_wrench（仿真 sn = sim）。"
+        )
+
+    if "force_torque" not in urdf_text:
+        warnings.append(
+            "URDF 里没有任何 type=\"force_torque\" 传感器 —— 腕部 FT 不会被创建。"
+        )
+
+    return warnings

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/truth_holes.py

@@ -0,0 +1,247 @@
+#!/usr/bin/env python3
+"""真值孔位：把场景里**已知的孔几何**投影成二阶段要的那两个像素点。
+
+为什么需要它
+------------
+二阶段（S05 精拍 → 对准）原本的输入是 ``hole_detect_hough`` 在图上找**亮环**。
+那条路要求工件检测模型先给出工件框（ROI），否则连"找哪儿"都不知道。
+
+仿真的处境是：合成图过不了真机训练出来的工件模型（实测最高分 0.017），
+于是 S05 只能退回"整幅画面当 ROI"—— 而整幅画面里有**夹爪、通止规、台面**，
+HoughCircles 会在那儿找到"看起来像一对孔"的东西（实测两孔间距 108mm，
+真值是 58mm），随后被间距门槛丢掉，每拍都丢，最后超时。
+
+真值路径直接把"孔在图上应该在哪"算出来：
+
+    孔世界点 = 物体世界位姿（scene.yaml）+ 环在物体局部系的位置（模型 <visual>）
+    孔像素   = K · T_cam_world · 孔世界点
+
+它验证的是**控制那一半**（坐标变换 / 手眼 / 伺服收敛 / 流程编排），
+不是感知那一半 —— 与 ``sim_truth_perception`` 同一条设计（见
+``devices/sim/gazebo/README.md`` 的「仿真里哪些模块用真值替代」）。
+
+绝不做的事
+----------
+* 孔在相机背后（z<=0）时**不返回**结果。给伺服一个幻影目标会让机械臂朝错误
+  方向走，而且看起来像"标定坏了" —— 和 ``sim_truth_perception`` 同一条规矩。
+* 不自己算四元数→旋转矩阵：T 由调用方（节点）用 TF 查好传进来。
+  本模块只做**纯几何**，因此在没有 ROS 的环境里也能测。
+"""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, field
+from pathlib import Path
+from typing import List, Optional, Sequence, Tuple
+
+import numpy as np
+
+from arm_sim_gazebo import sim_scene
+
+#: 孔（环）离相机光心太近或跑到背后就不返回：z 太小除法会放大误差，
+#: z<=0 在相机系里就是"背后"。取 2cm，与真值感知节点的取值同量级。
+MIN_CAM_Z_M = 0.02
+
+#: 两种孔排序的判据：图像上 |dx| 大于这个比例倍数就按 x 排，否则按 y 排。
+#: 与 ``hole_detect_hough._detect_insert_pair`` 的规则一致（k1=左/上，
+#: k2=右/下）—— 换后端不该换语义，否则"哪边是 k1"会随环境变。
+_PAIR_HORIZONTAL_RATIO = 1.0
+
+
+@dataclass(frozen=True)
+class HoleTruth:
+    """一次真值定位的结果（``ok=False`` 时只有 ``why`` 有意义）。"""
+
+    ok: bool
+    why: str = ""
+    hole_points: List[Tuple[float, float]] = field(default_factory=list)
+    #: 两孔的像素半径（环的物理半径按透视缩放；Hough 路径给的是它自己的估计，
+    #: 真值路径给它一个**可信的**值 —— 上下游只把它当"尺度参考"用）
+    hole_radii: List[float] = field(default_factory=list)
+    #: 两孔**环心的世界坐标**（米），与 ``hole_points`` 一一对应、同序。
+    #:
+    #: 为什么真值路径要把三维也带出来（2026-09-24 的教训）：
+    #:     原来只给像素，调用方（stage2）再拿**深度图**反投影回三维。仿真里
+    #:     那一步是错的，而且错法很隐蔽：孔是**贯通孔**，孔位正下方就是台面
+    #:     通槽 —— 深度相机在孔位看到的是**槽底/地面**（实测 0.62m），
+    #:     而工件面其实只有 0.21m。拿 0.21m 的像素尺度去乘 0.62m 的深度，
+    #:     三角化出的 k1/k2 间距变成 169mm（真值 58mm），被间距门槛丢掉。
+    #:     真值路径**本来就知道**孔在世界系的精确位置，没有理由再绕一圈深度图。
+    hole_world: List[Tuple[float, float, float]] = field(default_factory=list)
+    #: 工件**上表面中心**的世界坐标（粗居中用；同样是"真值就该直接用"）。
+    part_center_world: Optional[Tuple[float, float, float]] = None
+    #: 工件上表面的世界 z（= 二阶段的对孔基准面）。
+    part_top_z: Optional[float] = None
+    #: 工件在画面里占的框（孔检测的 ROI 用；也是"工件中心"的来源）
+    bbox: Tuple[int, int, int, int] = (0, 0, 0, 0)
+    #: 给人看的说明（进了日志才知道这个数是哪来的）
+    note: str = ""
+
+
+def _object_world_xy(obj: sim_scene.SceneObject, lx: float, ly: float) -> Tuple[float, float]:
+    """物体局部 (x,y) → 世界 (x,y)（含物体自身 yaw）。"""
+    c, s = math.cos(float(obj.yaw)), math.sin(float(obj.yaw))
+    return (float(obj.x) + c * lx - s * ly, float(obj.y) + s * lx + c * ly)
+
+
+def project_point(
+    T_cam_world: np.ndarray, intr: Sequence[float], xyz: Sequence[float]
+) -> Optional[Tuple[float, float, float]]:
+    """世界点 → ``(u, v, z_cam)``；在相机背后/太近时返回 None。"""
+    T = np.asarray(T_cam_world, dtype=float)
+    p = T @ np.array([float(xyz[0]), float(xyz[1]), float(xyz[2]), 1.0], dtype=float)
+    z = float(p[2])
+    if not math.isfinite(z) or z <= MIN_CAM_Z_M:
+        return None
+    fx, fy, cx, cy = (float(intr[0]), float(intr[1]), float(intr[2]), float(intr[3]))
+    return (fx * float(p[0]) / z + cx, fy * float(p[1]) / z + cy, z)
+
+
+def object_bbox_px(
+    T_cam_world: np.ndarray,
+    intr: Sequence[float],
+    boxes: Sequence[Sequence[float]],
+    image_shape: Tuple[int, int],
+) -> Optional[Tuple[int, int, int, int]]:
+    """物体各碰撞体世界 AABB 的角点投影 → 像素包围框（裁剪到画面内）。
+
+    返回 None = 一个角点都投不出来（跑到背后去了），调用方应当放弃这一拍。
+    """
+    h, w = int(image_shape[0]), int(image_shape[1])
+    us: List[float] = []
+    vs: List[float] = []
+    for b in boxes:
+        for x in (b[0], b[1]):
+            for y in (b[2], b[3]):
+                for z in (b[4], b[5]):
+                    got = project_point(T_cam_world, intr, (x, y, z))
+                    if got is None:
+                        continue
+                    us.append(got[0])
+                    vs.append(got[1])
+    if not us:
+        return None
+    x1 = max(0, min(w - 1, int(math.floor(min(us)))))
+    y1 = max(0, min(h - 1, int(math.floor(min(vs)))))
+    x2 = max(0, min(w - 1, int(math.ceil(max(us)))))
+    y2 = max(0, min(h - 1, int(math.ceil(max(vs)))))
+    if x2 <= x1 or y2 <= y1:
+        return None
+    return (x1, y1, x2, y2)
+
+
+def locate_holes(
+    T_cam_world: Optional[np.ndarray],
+    intr: Optional[Sequence[float]],
+    image_shape: Tuple[int, int],
+    *,
+    spec: Optional[sim_scene.SceneSpec] = None,
+    object_name: str = "",
+    root: Optional[Path] = None,
+) -> HoleTruth:
+    """算这一拍的两个孔（像素 + ROI 框）。
+
+    ``T_cam_world`` 为 None（TF 还没到）时返回 ``ok=False``，由调用方决定是
+    继续用检测器还是报错 —— 本函数不猜、不兜底。
+    """
+    if T_cam_world is None or intr is None:
+        return HoleTruth(ok=False, why="还没有 T_cam_world / 相机内参（TF 或 camera_info 未到）")
+    try:
+        spec = spec if spec is not None else sim_scene.load_scene(None, root)
+    except Exception as exc:  # noqa: BLE001 —— 场景读不出来要说清是场景的问题
+        return HoleTruth(ok=False, why=f"读不到场景配置：{exc}")
+    want = (object_name or "").strip() or spec.operating.part_object
+    obj = next((o for o in spec.objects if o.name == want), None)
+    if obj is None:
+        names = ", ".join(o.name for o in spec.objects) or "（空）"
+        return HoleTruth(ok=False, why=f"场景里没有物体 {want!r}（现有：{names}）")
+    rings = sim_scene.rings_local(spec, obj, root)
+    if len(rings) != 2:
+        return HoleTruth(
+            ok=False,
+            why=(
+                f"工件 {obj.name!r} 的模型里有 {len(rings)} 个嵌件亮环，"
+                f"真值孔位要求正好 2 个（模型 <visual name=\"*_ring\"> 的名字约定）"
+            ),
+        )
+
+    pts: List[Tuple[float, float]] = []
+    rad: List[float] = []
+    holes_world: List[Tuple[float, float, float]] = []
+    for ring in rings:
+        wx, wy = _object_world_xy(obj, ring.x, ring.y)
+        wz = float(obj.z) + float(ring.z)
+        got = project_point(T_cam_world, intr, (wx, wy, wz))
+        if got is None:
+            return HoleTruth(
+                ok=False,
+                why=f"孔 {ring.name} 投影到相机背后（z<=0）—— 相机没对着工件时不该出结果",
+            )
+        u, v, z = got
+        pts.append((u, v))
+        rad.append(float(intr[0]) * float(ring.radius) / z)
+        holes_world.append((wx, wy, wz))
+
+    # k1/k2 的左右（上下）规则与 Hough 路径一致：水平对按 x，竖直对按 y。
+    (p, q) = pts
+    horizontal = abs(p[0] - q[0]) >= _PAIR_HORIZONTAL_RATIO * abs(p[1] - q[1])
+    key = 0 if horizontal else 1
+    if pts[0][key] > pts[1][key]:
+        pts = [pts[1], pts[0]]
+        rad = [rad[1], rad[0]]
+        holes_world = [holes_world[1], holes_world[0]]
+
+    boxes = sim_scene.world_boxes(spec, obj, root)
+    bbox = object_bbox_px(T_cam_world, intr, boxes, image_shape)
+    if bbox is None:
+        return HoleTruth(ok=False, why="工件整体投影不出画面框（角点都在相机背后）")
+
+    # 工件上表面中心与上表面 z：从各碰撞体的世界 AABB 取（AABB 是轴对齐的，
+    # 只有 yaw 的物体上它正好等于"外接矩形"，对"粗居中"这个用途是合适的）。
+    part_center: Optional[Tuple[float, float, float]] = None
+    part_top: Optional[float] = None
+    if boxes:
+        xs = [float(v) for b in boxes for v in (b[0], b[1])]
+        ys = [float(v) for b in boxes for v in (b[2], b[3])]
+        zs = [float(v) for b in boxes for v in (b[4], b[5])]
+        part_top = max(zs)
+        part_center = (0.5 * (min(xs) + max(xs)), 0.5 * (min(ys) + max(ys)), part_top)
+
+    dist_px = math.hypot(pts[0][0] - pts[1][0], pts[0][1] - pts[1][1])
+    note = (
+        f"{obj.name}（模型 {obj.model}） 环心距 {dist_px:.1f}px "
+        f"环半径 {rad[0]:.1f}/{rad[1]:.1f}px 框 {bbox}"
+    )
+    return HoleTruth(
+        ok=True,
+        why="",
+        hole_points=[(float(pts[0][0]), float(pts[0][1])), (float(pts[1][0]), float(pts[1][1]))],
+        hole_radii=[float(rad[0]), float(rad[1])],
+        hole_world=holes_world,
+        part_center_world=part_center,
+        part_top_z=part_top,
+        bbox=bbox,
+        note=note,
+    )
+
+
+def draw(bgr: np.ndarray, res: HoleTruth) -> np.ndarray:
+    """把真值结果画在图上（叠图/日志用）。失败时只写一行原因。"""
+    import cv2
+
+    img = bgr.copy()
+    h, w = img.shape[:2]
+    if not res.ok:
+        cv2.putText(img, "TRUTH: n/a", (12, 28), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
+        return img
+    x1, y1, x2, y2 = res.bbox
+    cv2.rectangle(img, (x1, y1), (x2, y2), (0, 255, 255), 2)
+    for i, (u, v) in enumerate(res.hole_points):
+        r = int(round(res.hole_radii[i])) if i < len(res.hole_radii) else 8
+        cv2.circle(img, (int(round(u)), int(round(v))), max(3, r), (0, 0, 255), 2)
+        cv2.putText(
+            img, f"T{i + 1}", (int(u) + 8, int(v)), cv2.FONT_HERSHEY_SIMPLEX, 0.60, (0, 0, 255), 2
+        )
+    cv2.putText(img, "TRUTH", (12, min(h - 8, 28)), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
+    return img

install_sim/arm_sim_gazebo/lib/python3.11/site-packages/arm_sim_gazebo/urdf_normalize.py

@@ -0,0 +1,501 @@
+"""把厂商给的 URDF 归一化成"Gazebo 能接受"的形态。
+
+=============================================================================
+为什么需要这一步（全是实测踩出来的）
+=============================================================================
+官方 flexiv_description 是给 RViz / MoveIt 用的，直接喂 Gazebo 会被
+sdformat 拒掉。两类问题，都属于"RViz 容忍、Gazebo 严格"的差异，
+所以这个框架在有 Gazebo 之前一直没暴露：
+
+问题一：7 轴本体的质量/惯量是"孤儿标签"
+    rizon_arm.xacro 里这样调：
+        <link name="${prefix}base_link">
+          <xacro:get_inertias name="base_link" />
+    get_inertias 宏只吐 <origin> / <mass> / <inertia> 三个碎片，**外面没有
+    <inertial> 包裹**。URDF 规范里 <link> 不能直接有这三个子元素。
+        RViz/MoveIt   忽略不认识的子元素 → 看起来一切正常
+        sdformat      判定 "link has no <inertial> block"，
+                      然后把 **link 和它的父关节一起丢掉**
+    症状很误导：机械臂凭空消失或只剩夹爪，报错只有一句
+    "link[sim_link1] is not modeled in sdf"，看不出根因在上游描述文件里。
+
+问题二：夹爪有零质量 link，导致 sdformat 惯性叠加出 NaN
+    grav_macro.xacro 给 left_finger_tip / left_finger_tcp / grav_tcp 等
+    六个纯坐标系 dummy link 写了 mass=0、惯量全 0。
+    sdformat 对 fixed 关节会把子 link 的惯性"叠加(lump)"到父 link，
+    这个叠加要除以总质量 → 质量 0 就出 NaN。而报错指向的是**父** link：
+        Pose values must be finite.
+        link[sim_left_finger_mount]/inertial/pose
+    完全看不出是子 link 质量的问题。
+
+问题三：被 mimic 的关节 effort="0"，在 Gazebo 里出不了力
+    sdformat 转 SDF 时会**直接丢掉 <mimic>**（实测生成的 SDF 里
+    "mimic" 出现 0 次），gz-sim 也没有任何 mimic 实现，SDF 规范里
+    根本没有这个元素。于是夹爪那 6 个"靠机械耦合跟随"的手指关节
+    在 Gazebo 里变成互相独立的关节，谁都不跟随谁。
+    更麻烦的是它们的 effort="0"（上游语义：不由电机驱动）——
+    而 gz_ros2_control 的 position 接口是"位置误差 → 力"，
+    力被 effort 上限夹到 0，于是**永远出不了力**：
+    接口 [claimed]、控制器 active、话题有订阅、命令也发得出去，
+    但手指一动不动，只是自由地被重力拽着漂。
+    详见 MIMIC_EFFORT_FIX 处的注释。
+
+=============================================================================
+修法原则
+=============================================================================
+只做"重新分组 + 补极小量"，不发明任何物理数据，也不掩盖真问题：
+    - 孤儿三件套 → 只是把已有元素包进 <inertial>，一个数字都不改
+    - 零质量 → 补成 1e-4 kg（与夹爪其他配件同量级，0.1g 对动力学无影响），
+               惯量如果是全 0 也补成极小的对角值，避免张量退化
+若某个 link 上游真没给质量信息，问题一这里什么都不会包，
+sdformat 会照实报错 —— 这是我们想要的：归一化不是止错器。
+
+=============================================================================
+为什么在启动时做，而不是改上游文件
+=============================================================================
+1. 上游文件由 scripts/fetch_flexiv.sh 从 GitHub 获取，改了会被下次 fetch 覆盖
+2. 这个框架要支持"换成别的臂" —— 别的厂商描述大概率有别的坑，
+   在启动时统一过一道"归一化"，比逐个去改厂商文件更可持续
+3. 纯字符串→字符串的变换，可以脱开 ROS 单测（见 test/test_urdf_normalize.py）
+=============================================================================
+"""
+from __future__ import annotations
+
+import re
+from dataclasses import dataclass, field
+from typing import List, Tuple
+
+# 零质量 link 补成的质量。取 1e-4 kg：与夹爪上 other 配件（内外杆、指座）
+# 同量级，0.1g 在动力学上完全可忽略，但足以让惯性叠加不除零。
+ZERO_MASS_FIX_KG = "1e-4"
+
+# 惯量全 0 时补的对角值。极致小值只为让张量非退化，不引入可观测的转动惯量。
+ZERO_INERTIA_FIX = "1e-9"
+
+# 被 mimic 的关节在 Gazebo 里失去机械耦合，必须能自己出力，所以把 effort="0"
+# 补成这个值。取 80 Nm：与夹爪那个真正被驱动的 finger_width_joint 的 effort 一致
+# （grav_macro.xacro 里写的就是 80），所以不引入任何新的"凭空数据"，
+# 只是把"上游说这关节不由电机驱动"翻译成"仿真里它必须自己出力"。
+#
+# 为什么需要这条修复（实测，2026-09-23）：
+#   夹爪只有 finger_width_joint 是真关节（effort=80），其余 6 个手指关节
+#   全是 mimic、effort="0"（上游语义：它们靠机械耦合跟随，不由电机驱动）。
+#   而 sdformat **直接丢弃 <mimic>**，gz-sim 也不支持 mimic ——
+#   于是在 Gazebo 里它们是 7 个互相独立的关节，谁都不跟随谁。
+#
+#   症状极具误导性：命令下发成功（接口 [claimed]、控制器 active、话题有订阅），
+#   但手指一动不动，只是自由地被重力拽着漂。原因是 gz_ros2_control 的 position
+#   接口实现是"位置误差 → 力"（实测 position_proportional_gain=0.1），
+#   力被 effort 上限夹到 0，于是永远出不了力。
+#
+#   对照实验：同一套链路下 sim_joint1 收到 0.2 rad 的目标后精确到位（0.19998），
+#   所以控制链路本身没问题，差别只在这 6 个关节的 effort=0。
+MIMIC_EFFORT_FIX = "80"
+
+# =============================================================================
+# 夹爪出力上限：把 gz_ros2_control 的"位置控制"夹成"力控夹爪"
+# =============================================================================
+# 为什么需要（实测，2026-09-24）：
+#   gz_ros2_control 的 position 接口是"位置误差 → 力"，力只被 effort 上限夹。
+#   grav_macro 给手指关节写的 effort=80（单位在这里是 Nm），而指腹到指节的
+#   力臂只有 ~0.1m —— 于是"合爪"能压出**上千牛**：仿真里 30mm 的规被两指
+#   直接挤穿（实测关节走到 0 位=开口 27mm < 规径 30mm），规被弹出去、掉回规座，
+#   触觉却报着上百牛的力。表现像"夹爪抓不住"，根因是出力没有上限。
+#
+# 取值怎么来的（不用猜：从 URDF 自己的行程推）：
+#   夹爪是平行四边形连杆，指腹平动 100mm 时手指关节转 0.9404 rad
+#   （inner/outer_finger 的 limits：-0.7854..0.155），所以
+#       力臂 ≈ 行程 / 角行程 = 0.100 / 0.9404 ≈ 0.106 m
+#   真机 Grav 的握力档位是 20~50N，取 45N（覆盖应用层常用的 force=30）：
+#       手指关节 effort = 45 × 0.106 ≈ 4.8 Nm
+#   而 finger_width_joint 是 1:1 的"行程关节"（0..0.1m），它的 effort 就是握力
+#   本身，给 45（N）。
+#
+# ⚠ 这一条是**仿真专用**的物理量：真机的握力由 GripperExecute.force 走力控闭环，
+#   不靠关节 effort 上限。归一化只服务于 Gazebo，所以写在这里、不去动上游描述。
+GRIP_FORCE_N = 45.0
+GRIP_STROKE_M = 0.100
+GRIP_FINGER_TRAVEL_RAD = 0.9404
+GRIP_PAD_LEVER_M = GRIP_STROKE_M / GRIP_FINGER_TRAVEL_RAD      # ≈ 0.1064 m
+GRIP_FINGER_EFFORT_NM = round(GRIP_FORCE_N * GRIP_PAD_LEVER_M, 2)   # ≈ 4.79 Nm
+GRIP_WIDTH_EFFORT_N = GRIP_FORCE_N
+
+#: 名字里带这些字样的关节 = 夹爪的指节/指腹/行程关节（仿真里统一带 sim_ 前缀）
+_GRIP_JOINT_HINTS = ("finger", "knuckle")
+#: 行程关节（1:1，effort 单位就是 N）
+_GRIP_WIDTH_HINT = "finger_width"
+
+# =============================================================================
+# 为什么要把 <mimic> 整个从描述里删掉
+# =============================================================================
+# 除了上面的 effort 问题，<mimic> 留在描述里还有第二个隐患：
+#
+#   robot_state_publisher **支持** mimic —— 它会用"主关节的值 × multiplier + offset"
+#   覆盖从动关节的位置，而不是用 /joint_states 里广播的真实值。
+#   而 Gazebo 里这些关节是独立被我们驱动的，真实值与 mimic 公式算出来的值
+#   在**我们没按公式驱动时**（或主关节没跟上时）就不一样。
+#   后果是 TF 与 Gazebo 各说各话，而且不报错。
+#
+# 实测（2026-09-23）：当前版本的 RSP **没有**应用 mimic（TF 跟随真实关节值），
+# 所以现在两边碰巧一致。但这是 RSP 的实现细节，不是契约 —— 它一变，
+# TF 就会静默退回闭合位。
+#
+# 因此归一化时把 <mimic> 删掉，让描述与 Gazebo 的事实一致：
+#   "7 个独立关节，全部由我们显式驱动"。
+# 耦合关系（谁跟随谁、乘多少）由 sim_gripper_adapter 显式负责，
+# 它从**未归一化**的展开结果里读（见 robot_description.mimic_relations）。
+#
+# 删掉的信息不会丢：会记进 NormalizeReport.stripped_mimics 并被打印出来。
+# =============================================================================
+
+# 裸露的 origin + mass + inertia 三件套。
+#
+# ⚠️ 标签之间的空白必须是**可选**的（\s*），不能要求有换行 —— 这是个实测踩到的坑：
+#    xacro 的输出格式在不同环境里不一样。真机环境的 xacro 会缩进成多行：
+#        <origin .../>
+#        <mass .../>
+#        <inertia .../>
+#    而仿真环境（另一份 xacro 版本）走 xacro.process_file().toxml() 时**全部挤在一行**：
+#        <link ...><origin .../><mass .../><inertia .../><visual ...>
+#    早期这里的正则要求标签之间必须有 \n，于是 Python 路径下归一化静默地什么都没做
+#    （报告"包裹 0 处"），而 CLI 路径下却正常 —— 因为命令行会缩进。
+#    后果极严重：Gazebo 会丢掉整条机械臂，正是这个归一化本来要防的事。
+#    教训：这段代码不能对空白格式有任何假设。
+#
+# 夹爪的 <inertial> 内部也是这三个标签，所以必须按 link 块处理并跳过已含
+# <inertial> 的块，否则会二次包裹 —— sdformat 会报
+# "Inertial element must have a mass element"（已踩过）。
+_TRIPLE = re.compile(
+    r"(?P<ind>[ \t]*)<origin(?P<o>[^>]*?)/>"
+    r"\s*<mass(?P<m>[^>]*?)/>"
+    r"\s*<inertia(?P<i>[^>]*?)/>"
+)
+
+_LINK_BLOCK = re.compile(r"<link\b[^>]*>.*?</link>", re.S)
+
+_JOINT_BLOCK = re.compile(r"<joint\b[^>]*>.*?</joint>", re.S)
+_MIMIC_TAG = re.compile(r"<mimic\b")
+
+# <limit effort="0" .../>：只认数值 0（或 0.0 / 0.00…），不碰其它值。
+_LIMIT_ZERO_EFFORT = re.compile(r"<limit\b(?P<attrs>[^>]*?)/>")
+
+_MASS_TAG = re.compile(r"<mass(?P<attrs>[^>]*?)/>")
+_INERTIA_TAG = re.compile(r"<inertia(?P<attrs>[^>]*?)/>")
+_INERTIA_COMPONENTS = ("ixx", "ixy", "ixz", "iyy", "iyz", "izz")
+
+
+@dataclass
+class NormalizeReport:
+    """归一化做了什么。启动器会打印它 —— 静默修改用户描述是不可接受的，
+    必须让人一眼看到"我改了你给的文件里的哪几处、改成什么"。"""
+
+    wrapped_inertials: int = 0
+    fixed_zero_mass: List[str] = field(default_factory=list)
+    fixed_zero_inertia: List[str] = field(default_factory=list)
+    fixed_zero_effort: List[str] = field(default_factory=list)
+    #: 被删掉的 mimic 关系 (关节, 源关节, multiplier, offset)
+    stripped_mimics: List[Tuple[str, str, str, str]] = field(default_factory=list)
+    #: 被压到真机握力量级的夹爪关节 (关节, 原值, 新值)
+    clamped_grip_effort: List[Tuple[str, str, str]] = field(default_factory=list)
+
+    @property
+    def changed(self) -> bool:
+        return bool(
+            self.wrapped_inertials
+            or self.fixed_zero_mass
+            or self.fixed_zero_inertia
+            or self.fixed_zero_effort
+            or self.stripped_mimics
+            or self.clamped_grip_effort
+        )
+
+    def summary(self) -> str:
+        if not self.changed:
+            return "URDF 无需归一化"
+        bits = []
+        if self.wrapped_inertials:
+            bits.append(f"补 <inertial> 包裹 {self.wrapped_inertials} 处")
+        if self.fixed_zero_mass:
+            bits.append(
+                f"零质量 link 补 {ZERO_MASS_FIX_KG}kg {len(self.fixed_zero_mass)} 个"
+                f"({', '.join(self.fixed_zero_mass)})"
+            )
+        if self.fixed_zero_inertia:
+            bits.append(f"零惯量张量补极小对角 {len(self.fixed_zero_inertia)} 个")
+        if self.fixed_zero_effort:
+            bits.append(
+                f"被 mimic 关节补 effort={MIMIC_EFFORT_FIX} {len(self.fixed_zero_effort)} 个"
+                f"({', '.join(self.fixed_zero_effort)})"
+            )
+        if self.stripped_mimics:
+            bits.append(
+                f"删除 mimic 关系 {len(self.stripped_mimics)} 条"
+                f"（Gazebo 不支持 mimic，改由 sim_gripper_adapter 显式驱动）"
+            )
+        if self.clamped_grip_effort:
+            bits.append(
+                f"夹爪出力压到握力 {GRIP_FORCE_N:g}N 量级 {len(self.clamped_grip_effort)} 个"
+                f"({', '.join(f'{n}:{a}→{b}' for n, a, b in self.clamped_grip_effort)})"
+            )
+        return "；".join(bits)
+
+
+def _num(attr: str) -> float:
+    """把 URDF 里的数值属性转 float，失败返回 nan（调用方据此跳过）。"""
+    try:
+        return float(attr)
+    except (TypeError, ValueError):
+        return float("nan")
+
+
+def _attrs_of(tag: str) -> dict:
+    """从标签文本里取出 name="value" 形式的属性。"""
+    return dict(re.findall(r'([A-Za-z_][\w.-]*)\s*=\s*"([^"]*)"', tag))
+
+
+def _fix_zero_mass(block: str, report: NormalizeReport, link_name: str) -> str:
+    """把 <mass value="0"/> 补成极小正值。
+
+    两个前提：这个 link 有 <inertial>（否则不是"质量为零"而是"没有质量信息"，
+    不归这里管），且质量为数值 0。
+    """
+    if "<inertial>" not in block:
+        return block
+
+    m = _MASS_TAG.search(block)
+    if m is None:
+        return block
+    attrs = _attrs_of(m.group("attrs"))
+    if _num(attrs.get("value", "")) != 0.0:
+        return block
+
+    report.fixed_zero_mass.append(link_name)
+    fixed = f'<mass value="{ZERO_MASS_FIX_KG}"/>'
+    block = block[: m.start()] + fixed + block[m.end():]
+
+    # 惯量张量若也全是 0，补极小对角值，避免退化
+    im = _INERTIA_TAG.search(block)
+    if im is not None:
+        iattrs = _attrs_of(im.group("attrs"))
+        if all(_num(iattrs.get(c, "")) == 0.0 for c in _INERTIA_COMPONENTS):
+            report.fixed_zero_inertia.append(link_name)
+            new_attrs = " ".join(
+                f'{c}="{ZERO_INERTIA_FIX if c in ("ixx", "iyy", "izz") else "0"}"'
+                for c in _INERTIA_COMPONENTS
+            )
+            block = block[: im.start()] + f"<inertia {new_attrs}/>" + block[im.end():]
+
+    return block
+
+
+def _fix_mimic_joint(block: str, report: NormalizeReport, joint_name: str) -> str:
+    """处理带 <mimic> 的关节：记下耦合关系、补 effort、删掉 mimic 标签。
+
+    为什么三件事一起做：它们都是同一个事实（"Gazebo 不支持 mimic"）的直接后果，
+    分散在三处代码里迟早会有人只改一半。
+      - 记下关系 → 适配器要知道谁跟随谁（信息不能丢，只是搬家）
+      - 补 effort → 关节要能自己出力（见 MIMIC_EFFORT_FIX）
+      - 删 mimic → 让描述与 Gazebo 的事实一致（见文件头那段说明）
+    """
+    m = _MIMIC_TAG.search(block)
+    if m is None:
+        return block
+
+    # 1) 记录耦合关系（从 <mimic .../> 标签上抠出来）
+    tag_end = block.find(">", m.start())
+    tag = block[m.start() : tag_end + 1] if tag_end != -1 else ""
+    attrs = _attrs_of(tag)
+    report.stripped_mimics.append(
+        (
+            joint_name,
+            attrs.get("joint", "?"),
+            attrs.get("multiplier", "1"),
+            attrs.get("offset", "0"),
+        )
+    )
+
+    # 2) 删掉 <mimic .../>（含它前面的缩进，免得留下孤零零的空白行）
+    line_start = block.rfind("\n", 0, m.start()) + 1
+    if block[line_start : m.start()].strip() == "":
+        block = block[:line_start] + block[tag_end + 1 :].lstrip("\n")
+    else:
+        block = block[: m.start()] + block[tag_end + 1 :]
+
+    # 3) effort="0" → 可用值（只动这一个属性，限位是上游的物理事实，不碰）
+    lm = _LIMIT_ZERO_EFFORT.search(block)
+    if lm is not None:
+        lattrs = _attrs_of(lm.group("attrs"))
+        if _num(lattrs.get("effort", "")) == 0.0:
+            report.fixed_zero_effort.append(joint_name)
+            lattrs["effort"] = MIMIC_EFFORT_FIX
+            new = "<limit " + " ".join(f'{k}="{v}"' for k, v in lattrs.items()) + "/>"
+            block = block[: lm.start()] + new + block[lm.end() :]
+
+    return block
+
+
+def _clamp_grip_effort(block: str, report: NormalizeReport, joint_name: str) -> str:
+    """把夹爪关节的 effort 上限压到"真机握力量级"（见 GRIP_FORCE_N 的推导）。
+
+    为什么必须做：不做的话"合爪"能压出上千牛，把工件挤穿、弹飞，
+    看起来像"夹爪抓不住"，实际是出力没有上限（详见 GRIP_FORCE_N 处注释）。
+
+    只改 effort 一个属性；限位、速度是上游的物理事实，一律不碰。
+    """
+    low = joint_name.lower()
+    if not any(h in low for h in _GRIP_JOINT_HINTS):
+        return block
+    want = GRIP_WIDTH_EFFORT_N if _GRIP_WIDTH_HINT in low else GRIP_FINGER_EFFORT_NM
+    lm = _LIMIT_ZERO_EFFORT.search(block)
+    if lm is None:
+        return block
+    lattrs = _attrs_of(lm.group("attrs"))
+    cur = _num(lattrs.get("effort", ""))
+    if cur <= want:
+        return block          # 上游本来就比我们更保守：尊重它，不往上调
+    lattrs["effort"] = f"{want:g}"
+    report.clamped_grip_effort.append((joint_name, f"{cur:g}", f"{want:g}"))
+    new = "<limit " + " ".join(f'{k}="{v}"' for k, v in lattrs.items()) + "/>"
+    return block[: lm.start()] + new + block[lm.end() :]
+
+
+def normalize_urdf(urdf: str) -> Tuple[str, NormalizeReport]:
+    """归一化入口。返回 (新 URDF 文本, 报告)。
+
+    纯文本替换，不改动输入里其它任何字节 —— 重写整份 XML（如用
+    ElementTree 再序列化）会顺带改变空白、属性顺序甚至语义，风险不值得担。
+    """
+    report = NormalizeReport()
+    parts: List[str] = []
+    pos = 0
+
+    for m in _LINK_BLOCK.finditer(urdf):
+        parts.append(urdf[pos:m.start()])
+        block = m.group(0)
+        link_name = "?"
+
+        # --- 问题一：孤儿 inertial 三件套 ---
+        if "<inertial>" not in block:
+            def repl(mm: re.Match) -> str:
+                ind = mm.group("ind")
+                o, m_, i = mm.group("o"), mm.group("m"), mm.group("i")
+                # 原样保留输入的排版风格：多行输入就给多行输出，单行就给单行输出。
+                # 凭空插入或删除换行会让 diff 里全是噪声，也不必要。
+                if "\n" in mm.group(0):
+                    return (
+                        f"{ind}<inertial>\n"
+                        f"{ind}  <origin{o}/>\n"
+                        f"{ind}  <mass{m_}/>\n"
+                        f"{ind}  <inertia{i}/>\n"
+                        f"{ind}</inertial>"
+                    )
+                return f"{ind}<inertial><origin{o}/><mass{m_}/><inertia{i}/></inertial>"
+
+            block, n = _TRIPLE.subn(repl, block)
+            report.wrapped_inertials += n
+
+        # --- 问题二：零质量 link ---
+        lm = re.match(r'<link\b[^>]*name\s*=\s*"([^"]*)"', block)
+        if lm:
+            link_name = lm.group(1)
+        block = _fix_zero_mass(block, report, link_name)
+
+        parts.append(block)
+        pos = m.end()
+
+    parts.append(urdf[pos:])
+    out = "".join(parts)
+
+    # --- 问题三：被 mimic 的关节 effort="0" ---
+    # 放在 link 处理之后单独一遍：<joint> 不会嵌套在 <link> 里，顺序无关，
+    # 而且这样不必把两套正则揉进同一个循环。
+    def joint_repl(mm: re.Match) -> str:
+        block = mm.group(0)
+        jm = re.match(r'<joint\b[^>]*name\s*=\s*"([^"]*)"', block)
+        name = jm.group(1) if jm else "?"
+        if _MIMIC_TAG.search(block):
+            block = _fix_mimic_joint(block, report, name)
+        # 夹爪出力上限**放在 mimic 修复之后**：mimic 那条会把 effort=0 补成 80，
+        # 而这里要把它压到真机握力的量级。顺序反了就等于没夹。
+        return _clamp_grip_effort(block, report, name)
+
+    out = _JOINT_BLOCK.sub(joint_repl, out)
+    return out, report
+
+
+def mimic_relations(urdf: str) -> List[Tuple[str, str, str, str]]:
+    """从（**未归一化的**）URDF 文本里读出所有 mimic 关系。
+
+    返回 [(关节, 源关节, multiplier, offset)]，数值以字符串给出（原样保留，
+    由调用方决定怎么解释）。归一化会删掉 <mimic>，所以必须在归一化**之前**读 ——
+    这也是这个函数放在本模块而不是适配器里的原因：谁删的谁负责提供读取入口。
+    """
+    out: List[Tuple[str, str, str, str]] = []
+    for m in _JOINT_BLOCK.finditer(urdf):
+        block = m.group(0)
+        mm_ = _MIMIC_TAG.search(block)
+        if mm_ is None:
+            continue
+        jm = re.match(r'<joint\b[^>]*name\s*=\s*"([^"]*)"', block)
+        tag_end = block.find(">", mm_.start())
+        attrs = _attrs_of(block[mm_.start() : tag_end + 1] if tag_end != -1 else "")
+        out.append(
+            (
+                jm.group(1) if jm else "?",
+                attrs.get("joint", "?"),
+                attrs.get("multiplier", "1"),
+                attrs.get("offset", "0"),
+            )
+        )
+    return out
+
+
+def normalize_file(src: str, dst: str) -> NormalizeReport:
+    """读文件 → 归一化 → 写文件。"""
+    with open(src, "r", encoding="utf-8") as fh:
+        text = fh.read()
+    out, report = normalize_urdf(text)
+    with open(dst, "w", encoding="utf-8") as fh:
+        fh.write(out)
+    return report
+
+
+def main(argv: List[str] | None = None) -> int:
+    """命令行入口：只看归一化做了什么，不启动 Gazebo。
+
+    排查"Gazebo 说我的 link 不存在"时，先跑这个比直接起仿真快得多：
+        ros2 run arm_sim_gazebo urdf_normalize <urdf> [输出路径]
+    """
+    import argparse
+    import sys
+
+    parser = argparse.ArgumentParser(
+        prog="urdf_normalize",
+        description="把 URDF 归一化成 Gazebo 可接受的形态（补 <inertial> 包裹、补零质量）。",
+    )
+    parser.add_argument("src", help="输入 URDF 路径")
+    parser.add_argument(
+        "dst", nargs="?", help="输出路径（默认写到 stdout，便于直接 grep）"
+    )
+    parser.add_argument(
+        "--quiet", action="store_true", help="不打印归一化摘要"
+    )
+    args = parser.parse_args(argv)
+
+    with open(args.src, "r", encoding="utf-8") as fh:
+        text = fh.read()
+    out, report = normalize_urdf(text)
+
+    if not args.quiet:
+        print(f"[urdf_normalize] {report.summary()}", file=sys.stderr)
+
+    if args.dst:
+        with open(args.dst, "w", encoding="utf-8") as fh:
+            fh.write(out)
+    else:
+        sys.stdout.write(out)
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

install_sim/arm_sim_gazebo/share/arm_sim_gazebo/hook/ament_prefix_path.dsv

@@ -0,0 +1 @@
+prepend-non-duplicate;AMENT_PREFIX_PATH;

install_sim/arm_sim_gazebo/share/arm_sim_gazebo/hook/ament_prefix_path.ps1

@@ -0,0 +1,3 @@
+# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
+
+colcon_prepend_unique_value AMENT_PREFIX_PATH "$env:COLCON_CURRENT_PREFIX"

install_sim/arm_sim_gazebo/share/arm_sim_gazebo/hook/ament_prefix_path.sh

@@ -0,0 +1,3 @@
+# generated from colcon_core/shell/template/hook_prepend_value.sh.em
+
+_colcon_prepend_unique_value AMENT_PREFIX_PATH "$COLCON_CURRENT_PREFIX"

install_sim/arm_sim_gazebo/share/arm_sim_gazebo/hook/pythonpath.dsv

@@ -0,0 +1 @@
+prepend-non-duplicate;PYTHONPATH;lib/python3.11/site-packages

install_sim/arm_sim_gazebo/share/arm_sim_gazebo/hook/pythonpath.ps1

@@ -0,0 +1,3 @@
+# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
+
+colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX/lib/python3.11/site-packages"

install_sim/arm_sim_gazebo/share/arm_sim_gazebo/hook/pythonpath.sh

@@ -0,0 +1,3 @@
+# generated from colcon_core/shell/template/hook_prepend_value.sh.em
+
+_colcon_prepend_unique_value PYTHONPATH "$COLCON_CURRENT_PREFIX/lib/python3.11/site-packages"

install_sim/arm_sim_gazebo/share/arm_sim_gazebo/launch/gazebo_sim.launch.py

@@ -0,0 +1,781 @@
+"""启动 Gazebo 仿真：世界 + 机器人 + 控制器。
+
+用法（推荐走 scripts/run_sim_gz.sh，它负责先激活仿真环境）：
+    ros2 launch arm_sim_gazebo gazebo_sim.launch.py
+    ros2 launch arm_sim_gazebo gazebo_sim.launch.py world:=/abs/other.world headless:=false
+
+=============================================================================
+启动顺序为什么是这样（每一环都踩过）
+=============================================================================
+    1. robot_state_publisher  发布归一化后的 URDF 到 /robot_description
+    2. Gazebo（gz_sim）        加载世界
+    3. ros_gz_sim create       从 /robot_description spawn 机器人
+    4. controller spawner      拉起 joint_state_broadcaster + rizon_arm_controller
+
+两处顺序是硬要求，颠倒了只表现为"静默不工作"：
+
+  a) RSP 必须在 create 之前就绪
+     create 用 -topic robot_description 取 URDF。RSP 以 transient_local 发布
+     （latched），迟到的订阅者也能拿到，所以这里主要是容忍，不是硬依赖。
+
+  b) controller spawner 必须在 Gazebo 起来之后
+     controller_manager **跑在 Gazebo 进程里**（由 gz_ros2_control 的系统插件
+     拉起），不是独立的 ros2_control_node。Gazebo 没起来时 spawner 找不到
+     /controller_manager 服务，会一直重试直到 timeout。
+
+  c) 归一化后的 URDF 必须同时喂给 RSP 和 create
+     gz_ros2_control 是从 /robot_description 读 <ros2_control> 段落的
+     （SDF 里只存插件名和参数路径，接口声明不在 SDF —— 实测确认）。
+     两边不是同一份时，控制器注册的关节名与 Gazebo 里的对不上，
+     症状是 "Joint 'sim_joint1' not found in the robot model"。
+
+=============================================================================
+use_sim_time
+=============================================================================
+仿真里所有节点都要 use_sim_time=True，时间来自 /clock（gz → ROS 桥接）。
+不设的话 TF 时间戳会用墙上时钟，与 Gazebo 的仿真时间不同源，
+表现为 RViz 里模型抖动、tf2 报 "Lookup would require extrapolation"。
+/clock 的桥接在下面 clock_bridge 里，故意排在最前面。
+"""
+from __future__ import annotations
+
+import os
+from pathlib import Path
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    LogInfo,
+    OpaqueFunction,
+    TimerAction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+# 启动延迟（秒）。都是为了避开"服务还没起来就去调"的竞态。
+# 数值偏保守：多等几秒远好过让用户对着一个静默失败的界面调试。
+SPAWN_DELAY = 4.0
+CONTROLLER_DELAY = 8.0
+
+CONTROLLER_NAMES = ("joint_state_broadcaster", "rizon_arm_controller", "gripper_controller")
+
+
+def _prepare(context, *args, **kwargs):
+    """展开 + 归一化描述，并组装所有动作。
+
+    放在 OpaqueFunction 里做，因为需要先算出 URDF 文本才能构造
+    robot_state_publisher 的参数 —— 而 launch 的 substitution 机制
+    没法在 Python 里"看到"中间结果。
+    """
+    from arm_sim_gazebo.robot_description import (
+        SimDescriptionError,
+        check_base_plugins,
+        default_controllers,
+        default_world,
+        default_xacro,
+        check_tactile_collisions,
+        model_link_pose,
+        prepare_urdf,
+        repo_root,
+        world_name,
+    )
+    from arm_sim_gazebo.sim_site import SimSiteError, resolve_world
+
+    world = LaunchConfiguration("world").perform(context).strip()
+    load_gripper = LaunchConfiguration("load_gripper").perform(context).strip().lower()
+    with_sensors = LaunchConfiguration("with_sensors").perform(context).strip().lower()
+    with_scene = LaunchConfiguration("with_scene").perform(context).strip().lower()
+    scene_file = LaunchConfiguration("scene").perform(context).strip()
+    headless = LaunchConfiguration("headless").perform(context).strip().lower()
+    verbose = LaunchConfiguration("verbose").perform(context).strip().lower()
+
+    # 相机清单在下面的传感器分支里才读，但场景分支（成像几何自检）也要用它。
+    # 先置空再赋值，避免"因为分支没进而在别处 NameError"这种低级但难查的错误。
+    specs = []
+
+    try:
+        root = repo_root()
+        # 与 gazebo_sim_launcher 用同一套优先级，直接 `ros2 launch` 的人也不会
+        # 遇到"改了 site.yaml 没反应"：--world > site.yaml 的 sim.world > 内置默认。
+        world_path, _world_origin = resolve_world(world, root, fallback=default_world(root))
+        world_path = str(world_path)
+
+        # 腕部 FT 的话题名先读出来，喂给 xacro —— 这个话题名有三处要对上
+        # （配置、URDF 里的 <topic>、应用层按 sn 推出的 /{sn}/ft_sensor_wrench），
+        # 让配置当唯一源头，xacro 不再自己写一份。
+        from arm_sim_gazebo.sim_wrist_ft import (
+            WristFtConfigError,
+            bridge_args as wrist_ft_bridge_args,
+            check_wrist_ft_sensor,
+        )
+        from arm_sim_gazebo.sim_wrist_ft import load_wrist_ft
+
+        try:
+            wft = load_wrist_ft(root=root)
+        except WristFtConfigError as exc:
+            raise RuntimeError(f"\n[gazebo_sim] 仿真腕部 FT 配置有问题：\n{exc}\n") from exc
+
+        # 抓取锁：仿真里唯一能保证"夹住的东西不会在机械臂连续运动时滑掉"的机制，
+        # 说明见 devices/sim/gazebo/config/grasp_lock.yaml 与 sim_grasp_lock.py。
+        from arm_sim_gazebo.sim_grasp_lock import (
+            DEFAULT_OBJECT_SDF_TMPL,
+            GraspLockError,
+            check_grasp_lock_sdf,
+        )
+        from arm_sim_gazebo.sim_grasp_lock import load_grasp_lock
+
+        try:
+            glock = load_grasp_lock(root=root)
+        except GraspLockError as exc:
+            raise RuntimeError(
+                f"\n[gazebo_sim] 仿真抓取锁配置有问题：\n{exc}\n"
+                f"  （只想先跑起来的话，把 grasp_lock.yaml 的 enabled 改成 false）\n"
+            ) from exc
+
+        # 静态核对：物体 SDF 里到底有没有那个插件、参数对不对得上。
+        # 差一处就**静默失效**（现象只是"规莫名掉了"），所以必须在这里说清楚。
+        glock_warnings: List[str] = []
+        if glock.enabled:
+            _obj_sdf = root / DEFAULT_OBJECT_SDF_TMPL.format(object=glock.object)
+            try:
+                _sdf_text = _obj_sdf.read_text(encoding="utf-8")
+            except OSError:
+                _sdf_text = ""
+            glock_warnings = check_grasp_lock_sdf(
+                _sdf_text, glock, sdf_label=str(_obj_sdf)
+            )
+
+        urdf, report = prepare_urdf(
+            xacro_path=default_xacro(root),
+            controllers_file=default_controllers(root),
+            load_gripper=load_gripper != "false",
+            with_sensors=with_sensors != "false",
+            wrist_ft_topic=wft.topic,
+        )
+        wname = world_name(world_path)
+        missing_base = check_base_plugins(Path(world_path))
+    except (SimDescriptionError, SimSiteError) as exc:
+        # 描述准备失败是最常见的上手障碍（环境没 source、路径不对）。
+        # 直接把可读原因打出来，而不是让它变成一个看不懂的 launch traceback。
+        raise RuntimeError(f"\n[gazebo_sim] 仿真描述准备失败：\n{exc}\n") from exc
+
+    actions = [
+        LogInfo(msg=f"[gazebo_sim] 仓库根: {root}"),
+        LogInfo(msg=f"[gazebo_sim] 世界: {world_path} (world 名: {wname})"),
+        LogInfo(msg=f"[gazebo_sim] URDF 归一化: {report.summary()}"),
+        LogInfo(msg=f"[gazebo_sim] {glock.describe()}"),
+    ]
+    for _w in glock_warnings:
+        actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 抓取锁检查: {_w}"))
+    if missing_base:
+        actions.append(
+            LogInfo(
+                msg=(
+                    "[gazebo_sim] ⚠ world 缺少基础系统: "
+                    + ", ".join(missing_base)
+                    + "。world 里只要声明了任何一个 <plugin>，Gazebo 就不再加载默认"
+                    "系统列表。缺 UserCommands 会让 /world/<名>/create 服务不存在，"
+                    "模型根本不会被创建，而 create 仍打印 'OK creation of entity' ——"
+                    "最终表现是控制器永远拉不起来。修法见 assembly.world 里的注释。"
+                )
+            )
+        )
+    if not report.changed:
+        actions.append(
+            LogInfo(
+                msg="[gazebo_sim] 注意：归一化没有改动任何内容。若 Gazebo 仍报 link 不存在，"
+                    "先确认 flexiv_description 的版本是否已自行修好这两个上游缺陷。"
+            )
+        )
+
+    # --- 1) 机器人描述 -------------------------------------------------------
+    # 显式传 URDF 字符串（而不是 Command(["xacro ..."])），因为要发的是
+    # **归一化后**的那一份。这是 RSP 与 create 必须一致的东西，见文件头 (c)。
+    actions.append(
+        Node(
+            package="robot_state_publisher",
+            executable="robot_state_publisher",
+            name="robot_state_publisher",
+            output="screen",
+            parameters=[
+                {"robot_description": urdf},
+                {"use_sim_time": True},
+            ],
+        )
+    )
+
+    # --- 2) Gazebo -----------------------------------------------------------
+    gz_args = ["-r"]  # 立刻开始推进仿真（不等 GUI 里的播放按钮）
+    if verbose == "true":
+        gz_args += ["-v", "4"]  # 打开 Gazebo 自身日志，排查插件加载失败时必需
+    if headless == "true":
+        # -s = 只跑服务端，不开 GUI。无显示器环境（本机就是）必须用这个。
+        gz_args.append("-s")
+    gz_args.append(world_path)
+
+    actions.append(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(
+                    FindPackageShare("ros_gz_sim").perform(context),
+                    "launch",
+                    "gz_sim.launch.py",
+                )
+            ),
+            launch_arguments={"gz_args": " ".join(gz_args)}.items(),
+        )
+    )
+
+    # --- 3) /clock 桥接（sim time 的来源，排在最前）-------------------------
+    actions.append(
+        Node(
+            package="ros_gz_bridge",
+            executable="parameter_bridge",
+            name="clock_bridge",
+            arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"],
+            output="screen",
+        )
+    )
+
+    # --- 3b) 世界模型位姿回读：物体被撞走时唯一能指名报错的数据源 ------------
+    # 为什么值得一条独立桥接：`perception.source=truth` 是**纯几何**的，
+    # 它按 scene.yaml 的**名义位姿**算孔位，从不问 Gazebo 物体现在在哪。
+    # 工件被碰走（实测：掉进台面通槽落到地面）之后，"真值"开始撒谎，
+    # 而现象全都指向别处（S05 报"两孔间距 169mm"、S07 一路下压不接触）。
+    # 这一条把 gz 的"每个模型实际位姿"搬进 ROS，让 sim_truth_perception
+    # 能把它和 scene.yaml 对上、指名报"工件不在它该在的地方"。
+    # 具体参数（话题名/类型/漂移容差）在 scene.yaml 的 pose_info 段。
+    from arm_sim_gazebo.sim_scene import load_scene as _load_scene
+
+    try:
+        _spec = _load_scene(None, root)
+    except Exception as exc:  # noqa: BLE001 —— 场景读不出来要说清是场景的问题
+        raise RuntimeError(f"\n[gazebo_sim] 场景配置有问题：\n{exc}\n") from exc
+    if _spec.pose_info is not None:
+        _pi = _spec.pose_info
+        actions.append(
+            Node(
+                package="ros_gz_bridge",
+                executable="parameter_bridge",
+                name="pose_info_bridge",
+                output="screen",
+                arguments=[f"{_pi.gz_topic}@{_pi.ros_type}[{_pi.gz_type}"],
+                # gz 的话题名固定是 /world/<world>/pose/info（命名空间由 gz 决定，
+                # 应用侧改不了），ros_topic 只是"到 ROS 这一侧叫什么"。
+                # ros_gz_bridge 没有改名能力，所以这里用 remap 把名字对到
+                # scene.yaml 声明的那一个 —— 两侧对不上的表现是
+                # "桥接器起来了但一个消息都没有"，那是个假故障。
+                remappings=[(_pi.gz_topic, _pi.ros_topic)],
+            )
+        )
+        actions.append(
+            LogInfo(
+                msg=f"[gazebo_sim] 世界模型位姿回读: {_pi.gz_topic} → {_pi.ros_topic}"
+            )
+        )
+
+    # --- 4) 相机：桥接 → 适配器 → （世界相机）补 TF --------------------------
+    # 只在挂了传感器时才做。--no-sensors 时跳过是必要的：适配器会去订阅
+    # 永远不存在的 gz 话题，它**不会报错**，只是静默没输出 —— 而"相机没图"
+    # 正是最需要快速定位的问题，不该被一个假故障混淆。
+    if with_sensors != "false":
+        from arm_sim_gazebo.sim_cameras import (
+            DEFAULT_CAMERAS_REL,
+            CameraConfigError,
+            bridge_args,
+            check_camera_consistency,
+            load_cameras,
+            rpy_to_quat,
+        )
+
+        try:
+            specs = load_cameras(root=root)
+        except CameraConfigError as exc:
+            raise RuntimeError(f"\n[gazebo_sim] 仿真相机清单有问题：\n{exc}\n") from exc
+
+        # 一致性：内参由 cameras.yaml 的 horizontal_fov 与 width/height 算，
+        # 而**渲染**用的是 SDF 里的值。两者不一致时图像与内参对不上，
+        # 三维点位会整体错 —— 而单目预览完全看不出来（gz 自己发的
+        # camera_info 本来就是错的，所以这份内参是"唯一真相"，更要保证它与渲染一致）。
+        # 图幅尤其要查：它决定 fx 的尺度，写错会让孔检测的像素门槛静默失效。
+        # 这里只告警不拦截：真有偏差时，仍应能起仿真去现场比对。
+        _cam_issues = check_camera_consistency(
+            specs,
+            world_file=Path(world_path),
+            sensors_xacro=Path(root)
+            / "devices/sim/gazebo/urdf/arm.sim.sensors.xacro",
+        )
+        # ⚠ 这里用 LogInfo 而不是 LogWarn：Humble 的 launch.actions 里没有
+        #   LogWarn（Iron 才加），照抄新版本会直接 ImportError 让整个 launch
+        #   加载失败 —— 一个"只是想说句话"的动作把仿真起不来，代价太大。
+        #   前缀里带 ⚠ 保证它在满屏日志里仍然显眼。
+        for _w in _cam_issues:
+            actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ {_w}"))
+
+        # 4a) 世界相机的 TF 兜底
+        #
+        # Gazebo **不会**为 world 里的 <model> 产生 ROS TF —— TF 只来自
+        # robot_state_publisher（也就是只覆盖 URDF 里的东西）。
+        # D435 正属于这一类：它的物理归属是"世界"（固定支架），不在 URDF 里
+        # （早期挂在 URDF 的 world link 上时，被 sdformat 连整棵子树静默丢掉）。
+        # 少了这条 TF，应用 tf2 查询直接报 "frame does not exist" ——
+        # 而图像本身完全正常，很容易误判成相机坏了。
+        #
+        # 位姿从 world 文件**解析**，不在这里重复配置：唯一真相就是 world 文件。
+        # 另配一份等于制造"改了 world 忘了改这里"，而症状是相机看到的东西
+        # 与它自称的位置对不上，极难查。
+        world_tf = []
+        for s in specs:
+            if not s.is_world_mounted:
+                continue
+            try:
+                (px, py, pz), (rr, rp, ry) = model_link_pose(
+                    Path(world_path), s.world_model, s.world_link
+                )
+            except SimDescriptionError as exc:
+                raise RuntimeError(
+                    f"\n[gazebo_sim] 世界相机 {s.ros_ns} 的 TF 补不出来：\n{exc}\n"
+                ) from exc
+            qx, qy, qz, qw = rpy_to_quat((rr, rp, ry))
+            world_tf.append(
+                Node(
+                    package="tf2_ros",
+                    executable="static_transform_publisher",
+                    name=f"tf_{s.ros_ns}",
+                    output="screen",
+                    arguments=[
+                        "--x", f"{px}", "--y", f"{py}", "--z", f"{pz}",
+                        "--qx", f"{qx}", "--qy", f"{qy}", "--qz", f"{qz}", "--qw", f"{qw}",
+                        "--frame-id", "world",
+                        "--child-frame-id", s.link_frame,
+                    ],
+                    parameters=[{"use_sim_time": True}],
+                )
+            )
+            actions.append(
+                LogInfo(
+                    msg=f"[gazebo_sim] 世界相机 {s.ros_ns}: world → {s.link_frame} "
+                        f"@ ({px:.3f}, {py:.3f}, {pz:.3f}) 由 world 文件解析得到"
+                )
+            )
+        if world_tf:
+            # 延后一点再发：/clock 还没起来时发布静态 TF 可能被 tf2 判成
+            # "未来时间戳"，而这一类问题只在很后面才以 extrapolation 报出来。
+            actions.append(TimerAction(period=2.0, actions=world_tf))
+
+        # 4b) 桥接：只做传输（gz 消息类型 → ROS 消息类型，话题名照抄）。
+        #     改名、坐标系、深度单位都交给 4c 的适配器 —— 桥接器改不了 frame_id，
+        #     而 frame_id 恰恰是最容易错又最不报错的那一项。
+        actions.append(
+            Node(
+                package="ros_gz_bridge",
+                executable="parameter_bridge",
+                name="camera_bridge",
+                output="screen",
+                arguments=bridge_args(specs),
+            )
+        )
+
+        # 4c) 适配器：盖光学系 frame_id + 深度 32FC1 米 → 16UC1 毫米 + 改成
+        #     应用期望的话题名。详见 sim_camera_adapter.py 顶部。
+        actions.append(
+            Node(
+                package="arm_sim_gazebo",
+                executable="sim_camera_adapter",
+                name="sim_camera_adapter",
+                output="screen",
+                parameters=[
+                    {"cameras_file": str(Path(root) / DEFAULT_CAMERAS_REL)},
+                    {"use_sim_time": True},
+                ],
+            )
+        )
+
+        # 4d) 触觉：桥接 + 适配器 -------------------------------------------
+        # 链路与相机同构：gz 只负责"有数据"，语义（轴向、基线、单位、接触超时）
+        # 全在适配器里做。见 sim_tactile.py / sim_tactile_adapter.py 顶部。
+        from arm_sim_gazebo.sim_tactile import (
+            DEFAULT_TACTILE_REL,
+            TactileConfigError,
+        )
+        from arm_sim_gazebo.sim_tactile import bridge_args as tactile_bridge_args
+        from arm_sim_gazebo.sim_tactile import load_tactile
+
+        try:
+            tac = load_tactile(root=root)
+        except TactileConfigError as exc:
+            raise RuntimeError(f"\n[gazebo_sim] 仿真触觉配置有问题：\n{exc}\n") from exc
+
+        # 描述级静态检查：接触传感器引用的 collision 名是 sdformat **自动生成**的，
+        # 换个 sdformat 版本就可能变，而变了之后传感器只是"永远没输出"、不报错。
+        # 这里把它和"传感器挂错关节""话题写在错误的层级""传感器被整个丢掉"
+        # 一起在启动时报出来。expected_topics 用配置里声明的话题做对照 ——
+        # 这是唯一能发现"传感器被静默丢弃"的办法。
+        for _w in check_tactile_collisions(
+            urdf,
+            expected_topics=[s.wrench_topic for s in tac.sides]
+            + [s.contact_topic for s in tac.sides],
+        ):
+            actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 触觉描述检查: {_w}"))
+
+        actions.append(
+            Node(
+                package="ros_gz_bridge",
+                executable="parameter_bridge",
+                name="tactile_bridge",
+                output="screen",
+                arguments=tactile_bridge_args(tac),
+            )
+        )
+        actions.append(
+            Node(
+                package="arm_sim_gazebo",
+                executable="sim_tactile_adapter",
+                name="sim_tactile_adapter",
+                output="screen",
+                parameters=[
+                    {"tactile_file": str(Path(root) / DEFAULT_TACTILE_REL)},
+                    {"use_sim_time": True},
+                ],
+            )
+        )
+
+    # 4e) 腕部 FT：只需桥接，**不需要适配器** ------------------------------
+    # 与触觉不同：触觉要轴向映射/基线/单位换算/contact 折算，所以有适配器；
+    # 腕部 FT 的 gz 输出本来就是牛顿、坐标系就是 child(link7) 系，而
+    # ros_gz_bridge **原生支持** gz.msgs.Wrench → WrenchStamped（实测确认）。
+    # 于是仿真产出的就是真机同名的 /<sn>/ft_sensor_wrench（同名同类型同单位），
+    # 应用层一行分支都不用写。少一层节点就少一个"配错了不报错"的地方。
+    if with_sensors != "false":
+        for _w in check_wrist_ft_sensor(urdf, wft):
+            actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 腕部 FT 检查: {_w}"))
+
+        actions.append(
+            Node(
+                package="ros_gz_bridge",
+                executable="parameter_bridge",
+                name="wrist_ft_bridge",
+                output="screen",
+                arguments=wrist_ft_bridge_args(wft),
+            )
+        )
+
+    # --- 5) spawn 机器人 -----------------------------------------------------
+    # -world 必须给对：world 名写错时 Gazebo **不报错**，只是实体没被创建，
+    # 表现成"launch 一切正常但 RViz 里什么都没有"。
+    actions.append(
+        TimerAction(
+            period=SPAWN_DELAY,
+            actions=[
+                Node(
+                    package="ros_gz_sim",
+                    executable="create",
+                    name="spawn_robot",
+                    output="screen",
+                    arguments=[
+                        "-world", wname,
+                        "-name", "rizon_sim",
+                        "-topic", "robot_description",
+                    ],
+                )
+            ],
+        )
+    )
+
+    # --- 5b) spawn 场景物体（工件 / 通止规）---------------------------------
+    # 场景物体**不进 world 文件**，而是在这里 spawn：
+    #   world 放"固定不动"的东西（地面、台子、支架相机），
+    #   物料（工件、规）会按站点/工序换位置，写进 world 就等于每次换料都改 world，
+    #   而 world 里还有物理参数与插件，动它的风险面太大（见 assembly.world 的注释）。
+    #
+    # ⚠️ 必须**等机器人 spawn 完**再 spawn 场景（这里再延后 2s）：
+    #    两者都走 /world/<名>/create 服务，同时打会争同一个服务，
+    #    重试窗口内失败的那个**不报错**（create 打印 OK 但实体没进），
+    #    表现成"工件时有时无"，非常难查。
+    if with_scene != "false":
+        from arm_sim_gazebo.sim_cameras import intrinsics as _intrinsics
+        from arm_sim_gazebo.sim_scene import (
+            SceneError,
+            check_pixel_gates,
+            load_scene,
+            operating_flange_z,
+            operating_pixel_report,
+            spawn_commands,
+            validate_scene,
+        )
+
+        try:
+            scene = load_scene(
+                Path(scene_file) if scene_file else None, root=root
+            )
+        except SceneError as exc:
+            raise RuntimeError(f"\n[gazebo_sim] 场景配置有问题：\n{exc}\n") from exc
+
+        actions.append(LogInfo(msg=f"[gazebo_sim] 场景: {scene.source}"))
+
+        # 摆放校验：悬空 / 穿模 / 出界 / 互相重叠 —— 全是"看起来正常、跑起来不对"
+        # 的那一类。这里只告警不拦截：真有问题时仍需能起仿真去看现场。
+        for _p in validate_scene(scene, root):
+            actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 场景摆放检查: {_p}"))
+
+        scene_nodes = []
+        for args in spawn_commands(scene, root):
+            name = args[args.index("-name") + 1]
+            scene_nodes.append(
+                Node(
+                    package="ros_gz_sim",
+                    executable="create",
+                    name=f"spawn_{name}",
+                    output="screen",
+                    arguments=["-world", wname] + args,
+                )
+            )
+        if scene_nodes:
+            actions.append(TimerAction(period=SPAWN_DELAY + 2.0, actions=scene_nodes))
+
+        # 成像几何自检：把"该被检测到的特征"在这台相机下换算成像素，
+        # 与门槛比一遍。这是**唯一**能在上真机之前发现"孔间距不够像素门槛"
+        # 的办法 —— 等真机上才发现要贵得多。
+        cam = next((s for s in specs if s.ros_ns == scene.operating.camera), None) if with_sensors != "false" else None
+        if cam is not None:
+            _fx = _intrinsics(cam, cam.width, cam.height)[0][0]
+            px = operating_pixel_report(scene, _fx, root)
+            if px:
+                actions.append(
+                    LogInfo(
+                        msg="[gazebo_sim] 孔检测几何自检（相机 {}，到工件 {:.2f}m，fx={:.1f}px）：\n".format(
+                            scene.operating.camera, scene.operating.camera_to_part_m, _fx
+                        )
+                        + "\n".join(f"    {k} = {v:.1f}px" for k, v in sorted(px.items()))
+                    )
+                )
+                for _p in check_pixel_gates(px, scene.gates_px):
+                    actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 孔检测几何: {_p}"))
+            _fz = operating_flange_z(scene, root)
+            if _fz is not None:
+                actions.append(
+                    LogInfo(
+                        msg=f"[gazebo_sim] 二阶段在仿真里应悬停到法兰 z≈{_fz:.3f}m"
+                            f"（真机 site.yaml 写的是 0.23 —— 两者不同是**已知**的，"
+                            f"原因是两边「基座与台面的相对高度」不同；"
+                            f"决定成像的是相机到工件的距离，见 scene.yaml 的 operating 段）"
+                    )
+                )
+        else:
+            actions.append(
+                LogInfo(
+                    msg="[gazebo_sim] 未做孔检测几何自检（缺相机清单，或 --no-sensors）"
+                )
+            )
+
+    # --- 6) 控制器 -----------------------------------------------------------
+    # 用 joint_trajectory_controller 而不是直接给关节发位置：应用层统一按
+    # rizon_arm_controller/follow_joint_trajectory 发轨迹，这个接口名与真机一致。
+    controllers = [
+        TimerAction(
+            period=CONTROLLER_DELAY + i * 2.0,
+            actions=[
+                Node(
+                    package="controller_manager",
+                    executable="spawner",
+                    name=f"spawn_{name}",
+                    output="screen",
+                    arguments=[
+                        name,
+                        "--controller-manager", "/controller_manager",
+                        # controller_manager 在 Gazebo 进程里，它是 sim time；
+                        # spawner 也要用 sim time，否则两者对时不同源。
+                        "--controller-manager-timeout", "120",
+                    ],
+                    parameters=[{"use_sim_time": True}],
+                )
+            ],
+        )
+        for i, name in enumerate(CONTROLLER_NAMES)
+    ]
+    actions.extend(controllers)
+
+    # --- 6b) 仿真夹爪适配器 --------------------------------------------------
+    # 为什么必须有它：sdformat 把 URDF 的 <mimic> 整个丢掉，gz-sim 也不实现，
+    # 所以 6 个从动关节不会跟随主关节。适配器按耦合公式算出 7 个关节的目标值
+    # 一起下发，并对外提供与真机**同名同类型**的 GripperExecute 服务
+    #（<ns>/open|close|move|rotate + status），上层代码一行都不用改。
+    #
+    # 耦合表来自 report.stripped_mimics —— 也就是 urdf_normalize 删掉 <mimic>
+    # 时记下来的那几张表。让 launch 传给适配器，而不是让适配器自己再展开一次
+    # xacro：两次展开在参数不同时会静默不一致，而症状是"手指角度不对"。
+    if load_gripper != "false":
+        from arm_sim_gazebo.sim_gripper import format_mimic_param
+
+        mimics = [format_mimic_param(*t) for t in report.stripped_mimics]
+        if not mimics:
+            # 这里必须出声：拿不到耦合表说明夹爪没被加载（或上游改了耦合方式），
+            # 而适配器会因此拒绝启动 —— 早知道比晚知道好。
+            actions.append(
+                LogInfo(
+                    msg="[gazebo_sim] ⚠ 归一化报告里没有 mimic 关系，"
+                    "仿真夹爪适配器将无法启动（它会明确报错）。"
+                    "检查 load_gripper 与 flexiv_description 的 grav 宏。"
+                )
+            )
+        actions.append(
+            TimerAction(
+                # 等 gripper_controller 起来再发命令：控制器还没 active 时
+                # 发出去的 /commands 会被丢弃（不报错），夹爪就一直停在初始位。
+                period=CONTROLLER_DELAY + len(CONTROLLER_NAMES) * 2.0 + 1.0,
+                actions=[
+                    Node(
+                        package="arm_sim_gazebo",
+                        executable="sim_gripper_adapter",
+                        name="sim_gripper_adapter",
+                        output="screen",
+                        parameters=[
+                            {
+                                "controllers_file": str(default_controllers(root)),
+                                "controller": "gripper_controller",
+                                "mimics": mimics,
+                                "robot_description": urdf,
+                                "namespace": "/gripper",
+                                # 抓取锁（仿真专用，见 sim_grasp_lock.py）：
+                                # 夹住后把物体固定到手指上，避免"位置控制夹持 +
+                                # 连续轨迹"导致的打滑掉件。
+                                "grasp_lock_enabled": bool(glock.enabled),
+                                "grasp_lock_object": str(glock.object),
+                                "grasp_lock_attach_topic": str(glock.attach_topic),
+                                "grasp_lock_detach_topic": str(glock.detach_topic),
+                                "grasp_lock_timeout_ms": int(glock.publish_timeout_ms),
+                            },
+                            {"use_sim_time": True},
+                        ],
+                    )
+                ],
+            )
+        )
+
+    # --- 6c) 机械臂的"应用接口"对齐真机 ---------------------------------------
+    # 仿真里唯一的控制通路是 ros2_control 的 JTC（control_msgs）。但应用层在
+    # 真机上用的是另一层接口 —— 这里把它们**按真机的名字与类型**补齐，
+    # 于是通止规装配（以及 stage2/stage3）在仿真里能不改一行代码就跑：
+    #
+    #   /follow_joint_trajectory   arm_msgs/FollowJointTrajectory → JTC
+    #       真机由 real_trajectory_adapter 提供（名字带 real，但实现只做 action
+    #       桥接、与硬件无关；这里**复用同一个节点**而不是再写一份 ——
+    #       两份实现迟早会不一致）。
+    #   /set_named_pose            arm_msgs/SetNamedPose（命名位姿）→ 上面那个
+    #       真机由 arm_bringup/stack.launch.py 起，仿真起同一份，只是把
+    #       joint_prefix 换成 sim_（关节名不同，点位数值是同一台机器人）。
+    #   /home                      std_srvs/Trigger
+    #       真机由 flexiv_arm_driver 提供（= 切工具系 + 转 /set_named_pose）。
+    #       仿真没有 Flexiv 驱动，用 sim_home_service 补同一份契约（不切工具系）。
+    actions.append(
+        Node(
+            package="arm_control",
+            executable="real_trajectory_adapter",
+            name="real_trajectory_adapter",
+            output="screen",
+            parameters=[{"use_sim_time": True}],
+        )
+    )
+    actions.append(
+        Node(
+            package="arm_control",
+            executable="named_pose_server",
+            name="named_pose_server",
+            output="screen",
+            parameters=[
+                # 命名位姿用**包内默认那份**（src/arm_control/config/named_poses.yaml）：
+                # 它只有 home/ready/inspect 这几个点，与"仿真里是哪台臂"无关；
+                # 真机走 site.yaml 的 robots[].paths.named_poses_file（那份带该臂的点位）。
+                str(root / "src" / "arm_control" / "config" / "named_poses.yaml"),
+                # 关节名：仿真里是 sim_joint1..7（前缀来自 urdf 的 arm_prefix，
+                # site.yaml 的 sim.joint_prefix 目前是"声明了但没接线"，见那一段注释）。
+                {"joint_prefix": "sim_"},
+                {"use_sim_time": True},
+            ],
+        )
+    )
+    actions.append(
+        Node(
+            package="arm_sim_gazebo",
+            executable="sim_home_service",
+            name="sim_home_service",
+            output="screen",
+            parameters=[{"use_sim_time": True}],
+        )
+    )
+
+    actions.append(
+        LogInfo(
+            msg=(
+                "[gazebo_sim] 启动中。验收命令：\n"
+                "    ros2 topic echo /clock --once                    # 仿真时间在走\n"
+                "    ros2 control list_controllers                    # 两个控制器 active\n"
+                "    ros2 topic echo /joint_states --once             # 7 个 sim_joint\n"
+                "    ros2 run tf2_ros tf2_echo world sim_tcp          # 末端位姿可查\n"
+                "    ros2 topic echo /tactile/left/status --once      # 触觉三力在发\n"
+                "    ign model -m workpiece -p                        # 工件在台面上（核对摆放）\n"
+                "    ign model -m gauge -p                            # 通止规立在台面上\n"
+                "    python3 scripts/check_sim_cameras.py             # ★ 相机的真正验收\n"
+                "    python3 scripts/check_sim_scene.py               # ★ 场景摆放 + 孔检测几何验收\n"
+                "  机械臂应在重力下**保持站住**。若它瘫下去或消失，说明描述有问题，"
+                "参考 arm_sim_gazebo/urdf_normalize.py 顶部列的两个上游缺陷。\n"
+                "  ★ 别用 'ros2 topic hz' 或肉眼预览来验收相机 —— 那些只能证明'有图像'。\n"
+                "    仿真相机的错误几乎全是静默的（深度单位差 1000 倍、frame_id 盖成\n"
+                "    本体系、gz 发的内参是错的、位姿 rpy 写错导致相机背对目标），\n"
+                "    它们全都能出图、能查到 TF。必须用 check_sim_cameras.py 反投影\n"
+                "    到 world、检查是否命中地面/台面，才算通过。\n"
+                "  ⚠ 深度话题是 16UC1 毫米（已从 gz 的 32FC1 米转换过）。\n"
+                "    内参也不是 gz 发的那份：gz 恒发 fx=277/cx=160/cy=120（它的默认\n"
+                "    60° 视场 + 半分辨率主点），适配器已按 cameras.yaml 的视场重算覆盖。\n"
+                "    所以改相机视场要同时改 cameras.yaml 和 SDF，启动时会自动比对告警。"
+            )
+        )
+    )
+
+    return actions
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "world",
+                default_value="",
+                description="世界文件绝对路径。留空=用仓库里默认的 assembly.world",
+            ),
+            DeclareLaunchArgument(
+                "load_gripper",
+                default_value="true",
+                description="是否挂 GN01 夹爪。装配任务需要 true",
+            ),
+            DeclareLaunchArgument(
+                "with_sensors",
+                default_value="true",
+                description="是否挂 D405 与触觉。关掉跑得更快，用于纯运动学验证",
+            ),
+            DeclareLaunchArgument(
+                "with_scene",
+                default_value="true",
+                description="是否 spawn 场景物料（工件/通止规）。false=只用空台面",
+            ),
+            DeclareLaunchArgument(
+                "scene",
+                default_value="",
+                description="场景配置文件路径。留空=用 devices/sim/gazebo/config/scene.yaml",
+            ),
+            DeclareLaunchArgument(
+                "headless",
+                default_value="true",
+                description="true=只开 Gazebo 服务端（无显示器环境用）；false=带 GUI",
+            ),
+            DeclareLaunchArgument(
+                "verbose",
+                default_value="false",
+                description="true=打开 Gazebo 详细日志（排查插件加载失败时开）",
+            ),
+            OpaqueFunction(function=_prepare),
+        ]
+    )

install_sim/arm_sim_gazebo/share/arm_sim_gazebo/package.bash

@@ -0,0 +1,31 @@
+# generated from colcon_bash/shell/template/package.bash.em
+
+# This script extends the environment for this package.
+
+# a bash script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+  # the prefix is two levels up from the package specific share directory
+  _colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)"
+else
+  _colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_bash_source_script() {
+  if [ -f "$1" ]; then
+    if [ -n "$COLCON_TRACE" ]; then
+      echo "# . \"$1\""
+    fi
+    . "$@"
+  else
+    echo "not found: \"$1\"" 1>&2
+  fi
+}
+
+# source sh script of this package
+_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/arm_sim_gazebo/package.sh"
+
+unset _colcon_package_bash_source_script
+unset _colcon_package_bash_COLCON_CURRENT_PREFIX

install_sim/arm_sim_gazebo/share/arm_sim_gazebo/package.dsv

@@ -0,0 +1,6 @@
+source;share/arm_sim_gazebo/hook/pythonpath.ps1
+source;share/arm_sim_gazebo/hook/pythonpath.dsv
+source;share/arm_sim_gazebo/hook/pythonpath.sh
+source;share/arm_sim_gazebo/hook/ament_prefix_path.ps1
+source;share/arm_sim_gazebo/hook/ament_prefix_path.dsv
+source;share/arm_sim_gazebo/hook/ament_prefix_path.sh

install_sim/arm_sim_gazebo/share/arm_sim_gazebo/package.ps1

@@ -0,0 +1,116 @@
+# generated from colcon_powershell/shell/template/package.ps1.em
+
+# function to append a value to a variable
+# which uses colons as separators
+# duplicates as well as leading separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+function colcon_append_unique_value {
+  param (
+    $_listname,
+    $_value
+  )
+
+  # get values from variable
+  if (Test-Path Env:$_listname) {
+    $_values=(Get-Item env:$_listname).Value
+  } else {
+    $_values=""
+  }
+  $_duplicate=""
+  # start with no values
+  $_all_values=""
+  # iterate over existing values in the variable
+  if ($_values) {
+    $_values.Split(":") | ForEach {
+      # not an empty string
+      if ($_) {
+        # not a duplicate of _value
+        if ($_ -eq $_value) {
+          $_duplicate="1"
+        }
+        if ($_all_values) {
+          $_all_values="${_all_values}:$_"
+        } else {
+          $_all_values="$_"
+        }
+      }
+    }
+  }
+  # append only non-duplicates
+  if (!$_duplicate) {
+    # avoid leading separator
+    if ($_all_values) {
+      $_all_values="${_all_values}:${_value}"
+    } else {
+      $_all_values="${_value}"
+    }
+  }
+
+  # export the updated variable
+  Set-Item env:\$_listname -Value "$_all_values"
+}
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+function colcon_prepend_unique_value {
+  param (
+    $_listname,
+    $_value
+  )
+
+  # get values from variable
+  if (Test-Path Env:$_listname) {
+    $_values=(Get-Item env:$_listname).Value
+  } else {
+    $_values=""
+  }
+  # start with the new value
+  $_all_values="$_value"
+  # iterate over existing values in the variable
+  if ($_values) {
+    $_values.Split(":") | ForEach {
+      # not an empty string
+      if ($_) {
+        # not a duplicate of _value
+        if ($_ -ne $_value) {
+          # keep non-duplicate values
+          $_all_values="${_all_values}:$_"
+        }
+      }
+    }
+  }
+  # export the updated variable
+  Set-Item env:\$_listname -Value "$_all_values"
+}
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+function colcon_package_source_powershell_script {
+  param (
+    $_colcon_package_source_powershell_script
+  )
+  # source script with conditional trace output
+  if (Test-Path $_colcon_package_source_powershell_script) {
+    if ($env:COLCON_TRACE) {
+      echo ". '$_colcon_package_source_powershell_script'"
+    }
+    . "$_colcon_package_source_powershell_script"
+  } else {
+    Write-Error "not found: '$_colcon_package_source_powershell_script'"
+  }
+}
+
+
+# a powershell script is able to determine its own path
+# the prefix is two levels up from the package specific share directory
+$env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.FullName
+
+colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/arm_sim_gazebo/hook/pythonpath.ps1"
+colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/arm_sim_gazebo/hook/ament_prefix_path.ps1"
+
+Remove-Item Env:\COLCON_CURRENT_PREFIX

install_sim/arm_sim_gazebo/share/arm_sim_gazebo/package.sh

@@ -0,0 +1,87 @@
+# generated from colcon_core/shell/template/package.sh.em
+
+# This script extends the environment for this package.
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+_colcon_prepend_unique_value() {
+  # arguments
+  _listname="$1"
+  _value="$2"
+
+  # get values from variable
+  eval _values=\"\$$_listname\"
+  # backup the field separator
+  _colcon_prepend_unique_value_IFS=$IFS
+  IFS=":"
+  # start with the new value
+  _all_values="$_value"
+  # workaround SH_WORD_SPLIT not being set in zsh
+  if [ "$(command -v colcon_zsh_convert_to_array)" ]; then
+    colcon_zsh_convert_to_array _values
+  fi
+  # iterate over existing values in the variable
+  for _item in $_values; do
+    # ignore empty strings
+    if [ -z "$_item" ]; then
+      continue
+    fi
+    # ignore duplicates of _value
+    if [ "$_item" = "$_value" ]; then
+      continue
+    fi
+    # keep non-duplicate values
+    _all_values="$_all_values:$_item"
+  done
+  unset _item
+  # restore the field separator
+  IFS=$_colcon_prepend_unique_value_IFS
+  unset _colcon_prepend_unique_value_IFS
+  # export the updated variable
+  eval export $_listname=\"$_all_values\"
+  unset _all_values
+  unset _values
+
+  unset _value
+  unset _listname
+}
+
+# since a plain shell script can't determine its own path when being sourced
+# either use the provided COLCON_CURRENT_PREFIX
+# or fall back to the build time prefix (if it exists)
+_colcon_package_sh_COLCON_CURRENT_PREFIX="/home/cc/ros2/install_sim/arm_sim_gazebo"
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+  if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then
+    echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
+    unset _colcon_package_sh_COLCON_CURRENT_PREFIX
+    return 1
+  fi
+  COLCON_CURRENT_PREFIX="$_colcon_package_sh_COLCON_CURRENT_PREFIX"
+fi
+unset _colcon_package_sh_COLCON_CURRENT_PREFIX
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_sh_source_script() {
+  if [ -f "$1" ]; then
+    if [ -n "$COLCON_TRACE" ]; then
+      echo "# . \"$1\""
+    fi
+    . "$@"
+  else
+    echo "not found: \"$1\"" 1>&2
+  fi
+}
+
+# source sh hooks
+_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/arm_sim_gazebo/hook/pythonpath.sh"
+_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/arm_sim_gazebo/hook/ament_prefix_path.sh"
+
+unset _colcon_package_sh_source_script
+unset COLCON_CURRENT_PREFIX
+
+# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks

install_sim/arm_sim_gazebo/share/arm_sim_gazebo/package.xml

@@ -0,0 +1,53 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>arm_sim_gazebo</name>
+  <version>0.1.0</version>
+  <description>
+    Gazebo 仿真后端。兑现 devices/sim/gazebo/manifest.yaml 里声明的
+    package: arm_sim_gazebo / executable: gazebo_sim_launcher。
+
+    职责：
+      1. 把 devices/sim/gazebo/urdf 下的仿真描述展开成 URDF，
+         并归一化成 Gazebo 能接受的形态（见 urdf_normalize.py 的长注释）
+      2. 起 Gazebo、spawn 机械臂、拉控制器、拉 ros_gz 桥
+      3. 把应用层需要的接口对齐成与真机/mock 完全一致
+         （控制器名 rizon_arm_controller、服务与话题名不变），
+         这样应用层代码不需要知道后端是谁
+
+    与真机环境严格隔离：本包只在 ros_gz_sim 环境构建（scripts/build_sim.sh），
+    真机环境不装它。反过来它也不依赖任何真机专属包。
+  </description>
+  <maintainer email="dev@example.com">arm_dev</maintainer>
+  <license>Apache-2.0</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>xacro</exec_depend>
+  <!-- Gazebo 侧（仅仿真环境有） -->
+  <exec_depend>ros_gz_sim</exec_depend>
+  <exec_depend>ros_gz_bridge</exec_depend>
+  <!-- 触觉适配器订阅 ros_gz_interfaces/msg/Contacts（接触传感器的原始消息），
+       发布 arm_msgs/msg/TactileStatus。两者都是运行期硬依赖。 -->
+  <exec_depend>ros_gz_interfaces</exec_depend>
+  <exec_depend>arm_msgs</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>gz_ros2_control</exec_depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>joint_trajectory_controller</exec_depend>
+  <!-- 描述与参数 -->
+  <exec_depend>flexiv_description</exec_depend>
+  <exec_depend>arm_control</exec_depend>
+  <exec_depend>arm_application</exec_depend>
+
+  <test_depend>python3-pytest</test_depend>
+  <test_depend>python3-yaml</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

install_sim/arm_sim_gazebo/share/arm_sim_gazebo/package.zsh

@@ -0,0 +1,42 @@
+# generated from colcon_zsh/shell/template/package.zsh.em
+
+# This script extends the environment for this package.
+
+# a zsh script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+  # the prefix is two levels up from the package specific share directory
+  _colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)"
+else
+  _colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_zsh_source_script() {
+  if [ -f "$1" ]; then
+    if [ -n "$COLCON_TRACE" ]; then
+      echo "# . \"$1\""
+    fi
+    . "$@"
+  else
+    echo "not found: \"$1\"" 1>&2
+  fi
+}
+
+# function to convert array-like strings into arrays
+# to workaround SH_WORD_SPLIT not being set
+colcon_zsh_convert_to_array() {
+  local _listname=$1
+  local _dollar="$"
+  local _split="{="
+  local _to_array="(\"$_dollar$_split$_listname}\")"
+  eval $_listname=$_to_array
+}
+
+# source sh script of this package
+_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/arm_sim_gazebo/package.sh"
+unset convert_zsh_to_array
+
+unset _colcon_package_zsh_source_script
+unset _colcon_package_zsh_COLCON_CURRENT_PREFIX

install_sim/arm_sim_gazebo/share/colcon-core/packages/arm_sim_gazebo

@@ -0,0 +1 @@
+ament_index_python:arm_application:arm_control:arm_msgs:controller_manager:flexiv_description:gz_ros2_control:joint_state_broadcaster:joint_trajectory_controller:launch:launch_ros:rclpy:robot_state_publisher:ros_gz_bridge:ros_gz_interfaces:ros_gz_sim:tf2_ros:xacro