ros_contorl/scripts/check_sim_gripper.py

#!/usr/bin/env python3
"""仿真夹爪的验收：**几何**行程对不对，不是"服务有返回"。

为什么需要它
------------
夹爪出问题的方式几乎全是静默的：

* 耦合链只套了一层 multiplier  —— 夹爪会动、角度也是那个量级，就是夹不准；
* 命令数组顺序与控制器不一致    —— 角度发给了别的关节，夹爪"动得很奇怪"；
* 毫米与米混用（差 1000 倍）    —— 夹爪开到别的地方；
* 夹爪适配器根本没起来          —— 服务不存在，但**话题照旧**、控制器照旧 active。

前三条都能通过"调服务有 success=true、status 里有数"这种验收。
所以这里分三层，只有第二、三层算数：

    第 1 层  /gripper/* 服务在、/gripper/status 有数据          （不算验收）
    第 2 层  请求开口 mm == 关节反馈换回来的开口 mm             （证明下发链路）
    第 3 层  两个指尖在 world 系里的**实际间距**随请求变化，
             且"合拢→张开"的行程 == 请求的开口                   （证明耦合与几何）

第 3 层是关键：第 2 层只说明"关节值到位了"。如果 mimic 的 multiplier 写错，
手指根本不会按预期张开，而第 2 层完全看不出来。也正因为这一层是**独立**的，
它才逼出了下面两个真实故障 —— 都是"验收脚本自己错"，比被测对象错更常见：

⚠️ 三个踩过的坑（都让本脚本先给出过错误结论）
--------------------------------------------
1. **米 / 毫米**。TF 查出来的坐标是米，而本脚本的阈值与期望值全是毫米。
   漏掉 ×1000 时，127 mm 会显示成 0.13 mm —— 恰好"看起来像夹爪没张开"，
   于是跑去查耦合公式，而真正的问题就在本文件里。（判据全部走 _dist_mm。）

2. **残留进程**。旧会话没停干净时，会有两个 sim_gripper_adapter 同时订阅
   /joint_states、同时发布 /gripper/status，服务调用还会被随机分给其中一个；
   残留的 static_transform_publisher 更阴 —— 它发 latched 的 /tf_static，
   会把**旧相机外参**按在新仿真的 TF 树上。症状一样是"读数全乱"。
   → 所以本脚本第一步查重名节点（`:meth:`GripperCheck._duplicate_nodes`），
     启动侧也加了闸门（scripts/run_sim_gz.sh 的预检 + setsid 进程组）。

3. **判据要与机构无关**。两侧"对称"不能用"两指中点漂移"来判：tip frame 是
   刚性固连在**会转动**的指垫上的（指垫既平移又转，指面才保持平行），所以
   frame 走的是弧线，中点必然位移 16 mm 左右。真正与方向无关的判据是
   "两侧各自走了多远"（实测 52.56 / 52.56 mm）。

⚠️ 指尖坐标系原点的固有间距
    实测两个 `*_finger_tip` frame 在完全合拢时仍然相距 27.09 mm（tip frame 不在
    夹持面上，而且会随手指转动）。所以**不要**拿"指尖间距"直接当开口看；
    本脚本用它的**变化量**（行程）判据 —— 那个变化量与开口是 1:1 的
    （实测：请求 8 / 20 / 50 mm → 间距增加 8.00 / 20.00 / 50.00 mm）。

用法
----
    source scripts/sim_env.sh
    python3 scripts/check_sim_gripper.py            # 需要 Gazebo 仿真已在跑
    python3 scripts/check_sim_gripper.py --verbose  # 另打印每档的指尖原始坐标

退出码：0 = 通过；1 = 未通过；2 = 环境不可用（服务不在 / 有重名节点）。
"""
from __future__ import annotations

import argparse
import math
import sys
import time
from typing import Dict, List, Optional, Tuple

import rclpy
from rclpy.node import Node
from tf2_ros import Buffer, TransformListener

try:
    from arm_msgs.msg import GripperStatus
    from arm_msgs.srv import GripperExecute
except ModuleNotFoundError as exc:  # pragma: no cover - 只在环境没 source 时触发
    # 直接让 Python 抛"找不到模块"很难定位：真正的原因通常是**忘了 source 环境**，
    # 而不是 arm_msgs 没编译。所以这里把原因和修法一次说清。
    sys.exit(
        f"找不到 {exc.name}：请先 source 仿真环境（它把 install_sim/ 加进 PYTHONPATH）：\n"
        f"    source scripts/sim_env.sh\n"
        f"再执行本脚本。若仍失败，说明 install_sim/ 还没构建：\n"
        f"    bash scripts/build_sim.sh"
    )

#: 指尖坐标系的**固有**间距不影响行程判据，但打印出来省得有人误读。
TIP_LEFT = "sim_left_finger_tip"
TIP_RIGHT = "sim_right_finger_tip"
WORLD = "world"

#: 每个开口请求之后等多久再量（位置控制器 + 物理稳定）。
SETTLE_S = 2.5

#: 第 2 层容差（毫米）：请求开口 vs 关节反馈换回的开口。
POSITION_TOL_MM = 0.5
#: 第 3 层容差（毫米）：指尖行程 vs 期望行程。
TRAVEL_TOL_MM = 1.5
#: 左右对称性容差（毫米）：两侧位移量之差。
SYMMETRY_TOL_MM = 1.5


def _spin(node: Node, seconds: float) -> None:
    deadline = time.monotonic() + seconds
    while time.monotonic() < deadline:
        rclpy.spin_once(node, timeout_sec=0.05)


def _tip_positions(buf: Buffer, node: Node, attempts: int = 40):
    """取两个指尖在 world 系里的位置；查不到返回 None。"""
    for _ in range(attempts):
        try:
            left = buf.lookup_transform(WORLD, TIP_LEFT, rclpy.time.Time())
            right = buf.lookup_transform(WORLD, TIP_RIGHT, rclpy.time.Time())
            return (
                (left.transform.translation.x, left.transform.translation.y, left.transform.translation.z),
                (right.transform.translation.x, right.transform.translation.y, right.transform.translation.z),
            )
        except Exception:
            _spin(node, 0.2)
    return None


def _dist_pts(a, b) -> float:
    """两点距离，**米**（原始量纲，调用方自己决定怎么用）。"""
    return math.sqrt(sum((x - y) ** 2 for x, y in zip(a, b)))


def _dist_mm(a, b) -> float:
    """两个位置之间的距离，**毫米**。

    ⚠️ 这个换算漏过一次，症状很典型：TF 查出来的坐标是**米**，而本脚本里所有
    阈值/期望值都是**毫米**（POSITION_TOL_MM、TRAVEL_TOL_MM、请求开口…）。
    漏掉 *1000 的话，127 mm 会显示成 0.13 —— 恰好"看起来像夹爪没张开"，
    于是跑去看耦合公式，而真正的问题只在本文件这一行。
    （实测：同一份数据下，指尖坐标是对的，只是判据全被缩小了 1000 倍。）
    """
    return 1000.0 * _dist_pts(a, b)


# ⚠️ 属性名都要避开 rclpy.Node 自己的内部属性：`self._clients` 是 Node 的私有
#    成员（一个 list），覆盖它会直接在 create_client 里崩（实测踩过）。
#    所以这里统一用 `_grip_clients` / `_last_status` / `_tf_buf`。
class GripperCheck(Node):
    def __init__(self) -> None:
        super().__init__("sim_gripper_check")
        self._tf_buf = Buffer()
        TransformListener(self._tf_buf, self)
        self._last_status: Optional[GripperStatus] = None
        self.create_subscription(GripperStatus, "/gripper/status", self._on_status, 10)
        self._grip_clients: Dict[str, object] = {}
        for verb in ("open", "close", "move"):
            self._grip_clients[verb] = self.create_client(GripperExecute, f"/gripper/{verb}")

    def _on_status(self, msg: GripperStatus) -> None:
        self._last_status = msg

    # --- 基础设施 ---------------------------------------------------------
    def wait_ready(self, timeout: float = 30.0) -> List[str]:
        """等服务和状态都就绪。返回缺失项（空列表=就绪）。"""
        missing = []
        for verb, cli in self._grip_clients.items():
            if not cli.wait_for_service(timeout_sec=timeout):  # type: ignore[attr-defined]
                missing.append(f"/gripper/{verb}")
        if self._last_status is None:
            _spin(self, 3.0)
        if self._last_status is None:
            missing.append("/gripper/status（话题无数据）")
        return missing

    def call(self, verb: str, position: float = 0.0) -> Tuple[bool, str]:
        cli = self._grip_clients[verb]
        req = GripperExecute.Request()
        req.command.command = verb
        req.command.position = float(position)
        fut = cli.call_async(req)  # type: ignore[attr-defined]
        while not fut.done():
            _spin(self, 0.05)
        res = fut.result()
        if res is None:
            return False, "服务没有返回"
        return bool(res.success), str(res.message)

    def movement(self, verb: str, position: float = 0.0):
        """下发一条命令，等稳定，返回 (请求说明, 状态开口, 两指尖, 是否成功)。"""
        ok, msg = self.call(verb, position)
        _spin(self, SETTLE_S)
        tips = _tip_positions(self._tf_buf, self)
        aperture = float(self._last_status.position) if self._last_status else None
        return ok, msg, aperture, tips

    # --- 三层验收 ---------------------------------------------------------
    def _duplicate_nodes(self) -> List[str]:
        """查出"同名节点有两个"的情况。

        ⚠️ 这是踩出来的：残留的旧会话与新会话同时活着时，两个
        ``sim_gripper_adapter`` 会同时订阅 /joint_states、同时发布
        /gripper/status，服务调用还会被随机分给其中一个 ——
        验收脚本读到的开口恒为 0 mm、指尖间距 0.03 mm（真实值 127 mm），
        看起来完全像夹爪换算写错了。

        所以验收第一步就是确认"图里只有一个我"。启动侧的那道闸门在
        scripts/run_sim_gz.sh 的预检里（拒绝起第二个仿真），这里是第二道。
        """
        names = self.get_node_names()
        dupes = []
        for known in (
            "sim_gripper_adapter",
            "sim_camera_adapter",
            "sim_tactile_adapter",
            "robot_state_publisher",
            "joint_state_broadcaster",
            "gripper_controller",
        ):
            n = sum(1 for name in names if name.strip("/") == known)
            if n > 1:
                dupes.append(f"{known} × {n}")
        return dupes

    def check(self, verbose: bool) -> int:
        dupes = self._duplicate_nodes()
        if dupes:
            print("环境不干净：仿真节点有重名（说明有残留的旧会话在跑）：", file=sys.stderr)
            for d in dupes:
                print(f"  - {d}", file=sys.stderr)
            print(
                "\n两个同名节点会抢话题/服务/TF，读数全是乱的。先停干净：\n"
                "    bash scripts/stop_sim_gz.sh\n"
                "再重启（run_sim_gz.sh 自己也会拒绝在残留状态下启动）。",
                file=sys.stderr,
            )
            return 2

        missing = self.wait_ready()
        if missing:
            print("环境不可用，缺：", file=sys.stderr)
            for m in missing:
                print(f"  - {m}", file=sys.stderr)
            print(
                "\n先确认仿真在跑（bash scripts/run_sim_gz.sh），"
                "且夹爪适配器起来了（ros2 node list | grep sim_gripper_adapter）。",
                file=sys.stderr,
            )
            return 2

        print("第 1 层：服务与状态")
        print(f"  /gripper/{{open,close,move}} 可用；/gripper/status 有数据（开口 {self._last_status.position:.2f} mm）")

        # --- 第 2、3 层：走一遍开口序列 -----------------------------------
        requests: List[Tuple[str, float]] = [
            ("close", 0.0),
            ("move", 8.0),
            ("move", 20.0),
            ("move", 50.0),
            ("open", 0.0),
        ]
        rows = []
        for verb, pos in requests:
            ok, msg, aperture, tips = self.movement(verb, pos)
            label = "全开" if verb == "open" else ("全闭合" if verb == "close" else f"{pos:.0f} mm")
            rows.append((label, float(pos) if verb == "move" else None, ok, msg, aperture, tips))

        print("\n第 2 层：请求开口 vs 关节反馈换回的开口（下发链路）")
        print(f"  {'命令':<10}{'期望开口':>10}{'状态开口':>10}{'差(mm)':>10}  ")
        l2_fail = []
        # "期望开口"对 open 来说是仿真行程，从状态里推出来（open 时的读数）。
        open_row = rows[-1]
        stroke = open_row[4]
        for label, want, ok, msg, aperture, tips in rows:
            expect = stroke if label == "全开" else (0.0 if label == "全闭合" else want)
            if aperture is None:
                l2_fail.append(f"{label}: 拿不到状态开口")
                print(f"  {label:<10}{expect:>10.2f}{'--':>10}{'--':>10}  状态缺失")
                continue
            diff = aperture - (expect or 0.0)
            bad = abs(diff) > POSITION_TOL_MM
            if bad:
                l2_fail.append(f"{label}: 状态开口 {aperture:.2f} != 期望 {expect:.2f}")
            if not ok:
                l2_fail.append(f"{label}: 服务返回失败（{msg}）")
            print(f"  {label:<10}{expect:>10.2f}{aperture:>10.2f}{diff:>10.2f}  {'✗' if bad else '✓'} {msg}")

        print("\n第 3 层：指尖的**实际**位移（真几何，这一层才证明耦合是对的）")
        tips_ok = all(r[5] is not None for r in rows)
        if not tips_ok:
            print("  查不到指尖 TF，无法验证几何 —— 这不算通过。", file=sys.stderr)
            return 1

        # ⚠️ 这一层**故意不用 status 的开口读数**当基准：开口反馈取自从动关节，
        #    而第 2 层已经在验那条链路。这里只认"我请求了多少毫米"，
        #    几何摆在那里，跟任何软件读数无关 —— 这才叫独立验证。
        closed_tips = rows[0][5]
        open_tips = rows[-1][5]
        span_closed = _dist_mm(*closed_tips)
        span_open = _dist_mm(*open_tips)
        spans = [_dist_mm(*r[5]) for r in rows]

        print(f"  合拢：指尖间距 {span_closed:7.2f} mm（tip frame 固有间距，不是开口）")
        print(f"  全开：指尖间距 {span_open:7.2f} mm")

        # 逐档对比"请求开口"与"实测间距增量"：仿真里 1 mm 开口 ≙ 1 mm 间距，
        # 所以两者必须相等（探针实测 27.02→127.02 mm，正好 1.000）。
        travel_bad = []
        for label, want, ok, msg, aperture, tips in rows:
            if want is None:
                continue  # close / open 的 request 不是毫米数，单独判
            actual = _dist_mm(*tips) - span_closed
            err = actual - want
            bad = abs(err) > TRAVEL_TOL_MM
            if bad:
                travel_bad.append(
                    f"请求 {want:.0f} mm，指尖实际张开 {actual:.2f} mm（差 {err:+.2f}）"
                )
            print(
                f"  请求 {want:5.1f} mm：指尖张开 {actual:7.2f} mm  "
                f"差 {err:+6.2f} mm  {'✗' if bad else '✓'}"
            )

        # 全开那一档用**几何自洽**来判：必须比 50 mm 那档还大，且与第 2 层的
        # 开口读数一致（两者都错成一样是不可能的）。
        if stroke:
            err = (span_open - span_closed) - stroke
            if abs(err) > TRAVEL_TOL_MM:
                travel_bad.append(
                    f"全开行程 {span_open - span_closed:.2f} mm != 开口读数 {stroke:.2f} mm"
                )
            print(
                f"  全开行程 {span_open - span_closed:7.2f} mm  期望 {stroke:.2f} mm  "
                f"差 {err:+.2f} mm  {'✓' if abs(err) <= TRAVEL_TOL_MM else '✗'}"
            )

        # 单调性：请求越大，间距必须越大（方向错了会在这里露出来）。
        monotonic = all(b > a for a, b in zip(spans, spans[1:]))
        print(f"  单调递增：{'✓' if monotonic else '✗ ' + ' → '.join(f'{s:.2f}' for s in spans)}")

        # 对称性：两侧**行程相等**才算这条耦合链是对的。
        #
        # ⚠️ 这里换过两次判据，两次都是"判据本身错了"，值得记下来：
        #    1) 最初把两侧位移投影到"左指的张开方向"再比大小 → 一个完全
        #       对称的夹爪被判成不对称（左 +52.6 / 右 -42.5）。因为左指的
        #       位移方向自己带着 x 分量，拿它当基准去投影右侧必然偏。
        #    2) 改成"两指中点漂移" → 又误判（漂移 16.29 mm，而两侧各走
        #       52.56 / 52.55 mm，好得不能再好）。原因：tip frame 是刚性固连
        #       在**会旋转**的指垫上的（指垫既平移又转动，指面才保持平行），
        #       所以 frame 走的是弧线 —— 两侧弧线的"共同分量"让中点发生位移，
        #       这跟"左右不对称"是两回事。
        #    真正与方向无关、又能反映耦合好坏的量只有一个：**两侧各自走了多远**。
        left_move = 1000.0 * _dist_pts(closed_tips[0], open_tips[0])
        right_move = 1000.0 * _dist_pts(closed_tips[1], open_tips[1])
        asym = abs(left_move - right_move)
        asym_bad = asym > SYMMETRY_TOL_MM
        mid_closed = tuple((a + b) / 2.0 for a, b in zip(*closed_tips))
        mid_open = tuple((a + b) / 2.0 for a, b in zip(*open_tips))
        mid_drift = 1000.0 * _dist_pts(mid_closed, mid_open)
        print(
            f"  对称性：左右各走 {left_move:.2f} / {right_move:.2f} mm，"
            f"差 {asym:.2f} mm  {'✗' if asym_bad else '✓'}"
        )
        print(
            f"    （参考：两指中点整体位移 {mid_drift:.2f} mm。tip frame 固连在"
            f"会转动的指垫上，会画弧，所以这个数不为 0 是正常的，不参与判据。）"
        )

        if verbose:
            print("\n明细：")
            for label, _want, ok, msg, aperture, tips in rows:
                d = "--" if tips is None else f"{_dist_mm(*tips):.2f}"
                print(f"  {label:<10} ok={ok} 开口={aperture} 指尖间距={d} {msg}")
            # 原始坐标也打出来：上面那一列数字是"间距"，看着对/错时都需要
            # 回头核对它到底取到了哪两个点（历史上这里出过取错帧的假象）。
            print("\n  指尖坐标（world 系，米）：")
            for label, _want, _ok, _msg, _ap, tips in rows:
                if tips is None:
                    print(f"    {label:<10} --")
                    continue
                lx, rx = tips
                print(
                    f"    {label:<10} 左=({lx[0]:+.4f},{lx[1]:+.4f},{lx[2]:+.4f}) "
                    f"右=({rx[0]:+.4f},{rx[1]:+.4f},{rx[2]:+.4f})"
                )

        problems = list(l2_fail)
        problems.extend(travel_bad)
        if not monotonic:
            problems.append("开口不是单调递增 —— 方向或顺序反了")
        if asym_bad:
            problems.append(
                f"左右不对称（左右行程差 {asym:.2f} mm）—— 某一侧耦合没生效"
            )

        print()
        if problems:
            print("结论：不通过", file=sys.stderr)
            for p in problems:
                print(f"  - {p}", file=sys.stderr)
            return 1
        print("结论：通过（下发链路 + 耦合几何 + 方向 + 对称性）")
        return 0


def main() -> int:
    ap = argparse.ArgumentParser(description=__doc__.splitlines()[0])
    ap.add_argument("--verbose", action="store_true", help="打印每一档的明细")
    args = ap.parse_args()

    rclpy.init()
    node = GripperCheck()
    try:
        return node.check(args.verbose)
    finally:
        node.destroy_node()
        rclpy.shutdown()


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