P1：设备契约层收尾 + Flexiv 驱动适配器 + 触觉四路数据打通

新增包
- arm_kinematics：从 arm_application 提取几何雅可比 + DLS 逆解（纯 numpy）。
  它被应用层和驱动层共用，留在应用层会让驱动反向依赖应用层。
  arm_application/geom_ik.py 改为 re-export shim，现有 import 一行未改。
- arm_driver_flexiv：非夕驱动适配器。只补 manifest 声明而现有节点缺失的接口
  （/home /enable /clear_fault /switch_tool /soft_drag /compliant_descent），
  已有能力不重复实现。RDK 访问经 rdk_py310 下的长驻桥，本包不 import flexivrdk。

新接口
- arm_msgs/SwitchTool.srv、arm_msgs/CompliantDescent.action
- 合并 capabilities 里语义重复的 set_tool / switch_tool

修复（都是只在真机上暴露、且难以定位的问题）
- rdk_client 会永久挂死：select 的"可读"不等于"有完整一行"，readline 遇到
  半行 JSON 会一直阻塞等换行符，超时机制完全失效。改用 os.read + 自己做行缓冲。
- CompliantDescent 行程少一步：descended + step > max 在浮点累加下会提前跳出，
  永远到不了标称行程。改用整数步数计数。
- CompliantDescentRunner 的 TF frame 退化成 _base_link：没继承节点已解析的 SN。
- resolve_orisys_config 仍指向旧路径；load_orisys_from_site 读的是旧键 config。

触觉四路数据打通
- 深度图 / 力矢量场原先被 tactile_worker 有意丢弃（注释写"图像不过队列"），
  从未发布到 ROS。新增 image_utils（数组→Image，编码自动判定）、独立的图像
  队列（maxsize=1 满则丢旧）与 /tactile/<side>/{image,vector}。
  默认关闭（tactile.image_hz=0），开启也不影响三力的 30fps 时序。
- mock 后端也会发图像，无硬件即可验证整条发布链路。

配置
- site.yaml 新增 tactile.image_hz；移除与 model 重复的旧键 tactile.config
- 型号配置从 src/arm_application/config/orisys/ 迁到
  devices/sensors/orisys/models/dd03/（随 SDK 一起放）

测试：198 个（原 139）
- arm_kinematics 24（雅可比/逆解的解析算例与不变量）
- arm_driver_flexiv 69（下压状态机 34 + RDK 协议层 35）
- arm_application 59（图像打包 36 + 矢量场探测 23）

config/site.example.yaml

@@ -445,8 +445,8 @@ hole_detect:
 #   bash ~/ros2/scripts/run_tactile.sh
 tactile:
   backend: "orisys"          # orisys | mock
-  model: "dd03"              # 传感器型号（设备契约层用的键；须与 devices/sensors/orisys/models/ 下的目录同名）
+  # 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。
-  config: "dd03"             # 旧键，与 model 同义。仅为兼容现有节点保留，P2 会移除
+  model: "dd03"
   # 型号配置随 SDK 一起放：devices/sensors/orisys/models/<型号>/config.json
   config_file: "/home/USER/ros2/devices/sensors/orisys/models/dd03/config.json"
   # 用 by-path 稳定路径（插哪个 USB 口就认哪个口，不受 video 编号漂移影响）。
@@ -461,6 +461,10 @@ tactile:
   motion_threshold: 0.0
   target_fps: 30.0
   publish_hz: 20.0
+  # 视触觉图像与力矢量场的发布频率。0 = 关闭（默认）。
+  # 三力走 30fps 的轻量队列；图像每帧几百 KB，单独走一条 maxsize=1 的队列，
+  # 打开它不会影响三力的时序。需要看形变图/矢量场时设成 5~10 即可。
+  image_hz: 0.0
   rotation_config: ""        # 左右共用；有分指文件则用下面两项
   left_rotation: ""          # 例：orisys SDK 的 rotation_config_0.json
   right_rotation: ""

devices/robots/flexiv/manifest.yaml

@@ -46,7 +46,6 @@ capabilities:
   - servo_joint
   - servo_cartesian
   - compliant_descent
-  - set_tool
   - switch_tool
   - home
   - enable

devices/sensors/orisys/manifest.yaml

@@ -24,29 +24,55 @@ description: >
 # 重要：上层算法依赖的数据契约
 outputs:
   - topic: tangential_1
+    ros_topic: "/tactile/{left|right}/status"
+    field: fshearx
+    msg: arm_msgs/TactileStatus
     type: scalar
     unit: 任意单位（非牛顿）
     notes: 基线会漂移，所有阈值必须按「相对静止均值的增量」使用
 
   - topic: tangential_2
+    ros_topic: "/tactile/{left|right}/status"
+    field: fsheary
+    msg: arm_msgs/TactileStatus
     type: scalar
     unit: 任意单位（非牛顿）
     notes: 与 tangential_1 正交。通止规装配需要确认哪一路对应哪个方向，见 README
 
   - topic: normal
+    ros_topic: "/tactile/{left|right}/status"
+    field: fnormal
+    msg: arm_msgs/TactileStatus
     type: scalar
     unit: 任意单位（非牛顿）
     notes: 法向力。拧紧判定需要「法向力突增」这一条件
 
   - topic: depth
+    ros_topic: "/tactile/{left|right}/image"
+    msg: sensor_msgs/Image
     type: image
     unit: null
-    notes: 接触面深度图
+    notes: >
+      接触面深度 / 形变图。编码随 SDK 返回值而定：float32 深度图是 32FC1，
+      uint8 形变图是 mono8（image_utils 自动判断，判不出来就跳过并告警一次）。
+
+      需要把 site.yaml 的 tactile.image_hz 设为 >0 才会发布 —— 默认 0（关闭），
+      因为图像每帧几百 KB，不该让它挤占三力那条 30fps 的时序。
 
   - topic: force_vector
+    ros_topic: "/tactile/{left|right}/vector"
+    msg: sensor_msgs/Image
     type: image
     unit: null
-    notes: 接触点力矢量可视化
+    notes: >
+      接触点力矢量场。每像素 2 个 float32（dx, dy），编码 32FC2 —— 这是 ROS 里
+      表达矢量场的标准方式。
+
+      Orisys 的绑定里没有一个明确叫 "force vector map" 的方法，所以实现按语义
+      依次尝试 compute_flow / compute_hhd / compute_deformation，取第一个返回
+      二维以上数组的。全都拿不到就不发这个话题（宁可没有，也不发一张假图）。
+
+      同样需要 tactile.image_hz > 0。
 
 # --- 厂商物料放置要求 ------------------------------------------------------
 assets:
@@ -121,6 +147,21 @@ config_schema:
     default: 20.0
     doc: 数据发布率。
 
+  - key: image_hz
+    site_path: tactile.image_hz
+    type: float
+    unit: Hz
+    required: false
+    default: 0.0
+    doc: >
+      视触觉图像（深度/形变图，话题 /tactile/<side>/image）与力矢量场
+      （话题 /tactile/<side>/vector）的发布频率。0 = 关闭。
+
+      默认关是因为图像每帧几百 KB，跨进程序列化 + ROS 发布的开销远大于三力；
+      三力的 30fps 是接触判据的基础，不该被动到。图像走一条独立的
+      maxsize=1 队列，满了丢旧留新，所以打开它也不会拖累三力。
+      人工观察 / 调试设 5~10 即可，不必跟 target_fps 一样高。
+
   - key: cuda
     site_path: tactile.cuda
     type: bool

docs/TACTILE.md

@@ -10,8 +10,18 @@
 |------|------|
 | `arm_application/orisys_tactile.py` | SDK 适配 + `load_orisys_from_site` + `tactile_worker`（多进程工作器） |
 | `arm_application/tactile_interface.py` | 可替换接口 / Mock |
-| `arm_application/tactile_node.py` | 发布 `/tactile/left\|right/status` |
+| `arm_application/tactile_node.py` | 发布 `/tactile/left\|right/status`（三力），以及可选的 `/image`（深度图）与 `/vector`（力矢量场） |
-| `config/orisys/dd03.json` | 传感器几何/跟踪（厂家配置，可进 git） |
+| `arm_application/image_utils.py` | numpy 数组 → `sensor_msgs/Image` 的打包（编码自动判定） |
+| `devices/sensors/orisys/models/dd03/config.json` | 传感器几何/跟踪（厂家配置，可进 git）。原先在 `src/arm_application/config/orisys/`，已迁到设备目录随 SDK 一起放 |
+
+**四路数据怎么拿到**：三力在 `/tactile/<side>/status`（`TactileStatus` 的
+`fnormal` / `fshearx` / `fsheary`），深度图在 `/tactile/<side>/image`，
+力矢量场在 `/tactile/<side>/vector`（`32FC2`，每像素 dx/dy）。
+
+后两路默认**关闭**：设 `site.yaml` 的 `tactile.image_hz > 0` 才发布。原因见下节 ——
+图像不该和 30fps 的三力挤同一条队列。没有硬件时可以用
+`ros2 run arm_application tactile --ros-args -p backend:=mock -p image_hz:=5.0`
+把这两条话题跑起来验证链路。
 
 **架构：每侧手指一个独立进程**（厂家示例 `two_sensor_pipeline.py` 的推荐做法，
 每进程独立 CUDA 上下文）。实测单进程双开两路会互相拖垮（30→0.8fps），

scripts/lib/rdk_helpers.py

@@ -0,0 +1,189 @@
+#!/usr/bin/env python3
+"""RDK 操作公共函数（必须在 rdk_py310 / Python 3.10 下运行）。
+
+背景与约束
+----------
+非夕的 `flexivrdk` Python 绑定与 ROS 环境的 Python 3.11 ABI 不兼容，所以所有
+直接调用 RDK 的代码必须以子进程方式跑在 `/home/cc/rdk_py310/bin/python` 下。
+因此本文件不能用任何 3.11+ 语法，也不 import ROS。
+
+抽取原因
+--------
+`ensure_operational` / `ensure_idle` / tool 相关函数原先在
+`scripts/rdk_switch_tool.py` 与 `scripts/rdk_drag_bridge.py` 里各写了一份，
+行为已经出现细微差异（一个用 ServoOn 一个用 Enable，超时时间不同）。
+统一到本文件后，两处调用方行为一致。
+
+重要限制
+--------
+RDK 对同一台机器人是**独占访问**：同时有两个进程连着同一个 SN 会失败或行为异常。
+所以框架里所有 RDK 操作都应当经由 `rdk_drag_bridge.py` 这一个长驻进程串行化，
+不要让业务节点各自起 RDK 连接。
+"""
+
+from __future__ import annotations
+
+import time
+from typing import Any, Dict, List, Optional
+
+
+# ---------------------------------------------------------------------------
+# 使能与故障
+# ---------------------------------------------------------------------------
+
+def ensure_operational(robot: Any, timeout_s: float = 10.0) -> Optional[str]:
+    """确保机器人处于 operational（已使能、无故障）。
+
+    返回 ``None`` 表示成功，否则返回给用户看的错误说明。
+
+    这是幂等的：已经 operational 时不做任何事。
+    """
+    try:
+        if robot.fault():
+            robot.ClearFault()
+            time.sleep(0.5)
+        if not robot.operational():
+            # 不同 RDK 版本一个叫 ServoOn 一个叫 Enable。
+            for fn in ("ServoOn", "Enable"):
+                if hasattr(robot, fn):
+                    try:
+                        getattr(robot, fn)()
+                        break
+                    except Exception:
+                        pass
+            deadline = time.monotonic() + timeout_s
+            while not robot.operational() and time.monotonic() < deadline:
+                time.sleep(0.2)
+        if not robot.operational():
+            return (
+                "机器人未处于 operational 状态（故障 / 急停未复位？）。"
+                "请在示教器上清故障、松开急停，并确认处于 AUTO(Remote)。"
+            )
+    except Exception as exc:  # noqa: BLE001
+        return str(exc)
+    return None
+
+
+def clear_fault(robot: Any) -> Optional[str]:
+    """清除故障。返回 ``None`` 表示成功。"""
+    try:
+        robot.ClearFault()
+        time.sleep(0.5)
+    except Exception as exc:  # noqa: BLE001
+        return str(exc)
+    return None
+
+
+def ensure_idle(robot: Any, mode: Any, timeout_s: float = 10.0) -> Optional[str]:
+    """确保机器人切到 IDLE 模式（工具切换等操作的前提）。
+
+    返回 ``None`` 表示成功，否则返回错误说明。
+    """
+    try:
+        robot.SwitchMode(mode.IDLE)
+        return None
+    except Exception:
+        pass
+
+    err = ensure_operational(robot, timeout_s=timeout_s)
+    if err:
+        return err
+    try:
+        robot.SwitchMode(mode.IDLE)
+    except Exception as exc:  # noqa: BLE001
+        return f"无法切到 IDLE: {exc}"
+    return None
+
+
+# ---------------------------------------------------------------------------
+# 工具坐标系
+# ---------------------------------------------------------------------------
+
+def single_arm_groups(robot: Any) -> List[Any]:
+    """返回工具 API 的关节组列表。
+
+    RDK 1.9 的 Tool API 是单臂的（没有 JointGroup 概念），此时返回 ``[None]``
+    作为哨兵，调用方据此走不带 group 参数的分支。老版本返回实际组名列表。
+    """
+    try:
+        info = robot.info()
+        groups = getattr(info, "single_arm_groups", None) or {}
+        keys = list(groups.keys()) if isinstance(groups, dict) else list(groups)
+        if keys:
+            return keys
+    except Exception:
+        pass
+    try:
+        import flexivrdk
+
+        if hasattr(flexivrdk, "JointGroup"):
+            return [flexivrdk.JointGroup.ARM_1]
+    except Exception:
+        pass
+    return [None]
+
+
+def tool_name(tool: Any, group: Any) -> str:
+    """读当前工具名。"""
+    try:
+        return str(tool.name() if group is None else tool.name(group))
+    except Exception:
+        return ""
+
+
+def tool_params(tool: Any, group: Any) -> Dict[str, Any]:
+    """读当前工具参数，归一化成普通 dict。
+
+    返回 ``{"mass_kg": float, "com_m": [x,y,z], "tcp_m": [x,y,z]}``。
+    读不到的字段用 0 / 空列表。
+    """
+    try:
+        params = tool.params() if group is None else tool.params(group)
+    except Exception:
+        return {"mass_kg": 0.0, "com_m": [], "tcp_m": []}
+
+    def _floats(value: Any) -> List[float]:
+        try:
+            return [float(v) for v in list(value)[:3]]
+        except Exception:
+            return []
+
+    return {
+        "mass_kg": float(getattr(params, "mass", 0.0) or 0.0),
+        "com_m": _floats(getattr(params, "CoM", []) or []),
+        "tcp_m": _floats(getattr(params, "tcp_location", []) or []),
+    }
+
+
+def switch_tool(tool: Any, group: Any, target: str) -> None:
+    """切换工具。失败抛异常。"""
+    if group is None:
+        tool.Switch(target)
+    else:
+        tool.Switch(group, target)
+
+
+def resolve_tool_name(tool: Any, want: str) -> str:
+    """把用户给的模糊名字解析成 Studio 工具池里的准确名字。
+
+    大小写不敏感。找不到时抛 ``ValueError``，消息里列出全部可选项 ——
+    这样操作员能立刻看到拼错在哪，而不是面对一个"切换失败"。
+    """
+    want = (want or "").strip()
+    if not want:
+        return ""
+    try:
+        names = list(tool.list() or [])
+    except Exception as exc:  # noqa: BLE001
+        raise ValueError(f"无法读取 Studio 工具列表: {exc}") from exc
+
+    if want in names:
+        return want
+    lower = {str(n).lower(): str(n) for n in names}
+    hit = lower.get(want.lower())
+    if hit:
+        return hit
+    raise ValueError(
+        f"工具 '{want}' 不在 Studio 工具池里。可选: "
+        + (", ".join(str(n) for n in names) if names else "（空）")
+    )

scripts/rdk_drag_bridge.py

@@ -1,24 +1,41 @@
 #!/usr/bin/env python3
-"""RDK Free-Drive bridge (must run under rdk_py310 / Python 3.10).
+"""RDK 控制桥（必须在 rdk_py310 / Python 3.10 下运行）。
 
-Why soft-impedance drag
+为什么需要这个进程
------------------------
+------------------
-Under AUTO (Remote) the Studio Free Drive button is unavailable (remote owns
+非夕的 `flexivrdk` Python 绑定与 ROS 环境的 Python 3.11 ABI 不兼容，且 RDK 对
-the robot). RDK FloatingJoint/FloatingCartesian often need an extra license.
+同一台机器人是**独占访问**：多个进程同时连同一个 SN 会失败。所以框架里所有
-So we default to soft NRT_JOINT_IMPEDANCE: low stiffness + target tracks
+RDK 操作都收敛到本进程，由它串行化，业务节点通过 JSON 行协议远程调用。
-measured joints → arm can be pushed by hand while Remote stays on.
 
-IMPORTANT: flexivrdk.Robot is NOT thread-safe. All robot API calls (including
+这也让 `arm_driver_flexiv` 不必 import flexivrdk，符合
-the drag loop) share one lock. Joint reads during drag use a cache so Enter
+`docs/INTERFACES.md` 的约束（业务节点不得 import flexivrdk）。
-does not race Stop()/SendJointPosition.
 
-Line protocol (JSON on stdout only; RDK logs → stderr):
+软拖拽为什么用 NRT_JOINT_IMPEDANCE
+----------------------------------
+AUTO(Remote) 模式下示教器上的 Free Drive 按钮不可用（控制权在 Remote）。
+RDK 的 FloatingJoint/FloatingCartesian 通常需要额外授权。
+所以默认用低刚度 NRT_JOINT_IMPEDANCE：目标跟踪实测关节角 + 低刚度，
+于是手臂可以用手推动，同时 Remote 保持开启。
+
+线程安全
+--------
+`flexivrdk.Robot` 不是线程安全的。所有 robot API 调用（含拖拽循环）共用同一
+把锁 `rlock`。拖拽期间的关节读取走缓存，避免 Enter 与 Stop() 竞争。
+
+行协议（JSON 只写 stdout；RDK 自身日志走 stderr）
+-------------------------------------------------
     {"cmd":"ping"}
-  {"cmd":"connect","sn":"Rizon4-063989"}
+    {"cmd":"connect","sn":"Rizon4-063989","iface":"enp4s0"}
+    {"cmd":"enable"}                              # 使能 + 清故障（幂等）
+    {"cmd":"clear_fault"}                         # 显式清故障并使能
+    {"cmd":"switch_tool","name":"tool1"}          # 切工具；name 空=只查询
+    {"cmd":"status"}                              # operational/fault/mode/tool
     {"cmd":"drag_on","mode":"soft"}               # soft | floating
     {"cmd":"drag_off"}
     {"cmd":"joints"}
     {"cmd":"quit"}
+
+每个命令的应答都是单行 JSON，含 ``ok`` 字段；失败时另有 ``error``。
 """
 
 from __future__ import annotations
@@ -27,8 +44,23 @@ import json
 import sys
 import threading
 import time
+from pathlib import Path
 from typing import List, Optional
 
+# 公共 RDK 操作统一在 scripts/lib/rdk_helpers.py，避免与 rdk_switch_tool.py
+# 各写一份导致行为漂移。
+sys.path.insert(0, str(Path(__file__).resolve().parent / 'lib'))
+from rdk_helpers import (  # noqa: E402
+    clear_fault as rdk_clear_fault,
+    ensure_idle,
+    ensure_operational,
+    resolve_tool_name,
+    single_arm_groups,
+    switch_tool,
+    tool_name,
+    tool_params,
+)
+
 _JSON_OUT = sys.stdout
 sys.stdout = sys.stderr
 
@@ -131,29 +163,8 @@ class SoftDrag:
 
 
 def _ensure_operational(robot) -> Optional[str]:
-    try:
+    """保留旧名以兼容既有调用点；实现已统一到 rdk_helpers.ensure_operational。"""
-        if robot.fault():
+    return ensure_operational(robot)
-            robot.ClearFault()
-            time.sleep(0.5)
-        if not robot.operational():
-            for fn in ('ServoOn', 'Enable'):
-                if hasattr(robot, fn):
-                    try:
-                        getattr(robot, fn)()
-                        break
-                    except Exception:
-                        pass
-            t0 = time.monotonic()
-            while not robot.operational() and time.monotonic() - t0 < 10:
-                time.sleep(0.2)
-        if not robot.operational():
-            return (
-                'Robot not operational (fault/E-Stop?). '
-                'Clear fault on pendant, release E-Stop, AUTO Remote.'
-            )
-    except Exception as exc:
-        return str(exc)
-    return None
 
 
 def _idle_async(robot, lock: threading.Lock, flexivrdk, timeout: float = 4.0) -> bool:
@@ -308,6 +319,105 @@ def main() -> int:
                 cached_q = q
                 reply({'ok': True, 'q': q, 'drag': drag_on})
 
+            elif cmd == 'enable':
+                if robot is None:
+                    reply({'ok': False, 'error': '尚未 connect'})
+                    continue
+                err = ensure_operational(robot)
+                reply({
+                    'ok': err is None,
+                    'error': err,
+                    'operational': bool(robot.operational()),
+                })
+
+            elif cmd == 'clear_fault':
+                if robot is None:
+                    reply({'ok': False, 'error': '尚未 connect'})
+                    continue
+                err = rdk_clear_fault(robot)
+                if err:
+                    reply({'ok': False, 'error': err})
+                    continue
+                # 清故障后顺手使能，否则调用方还得再发一条 enable。
+                err2 = ensure_operational(robot)
+                reply({
+                    'ok': err2 is None,
+                    'error': err2,
+                    'fault': bool(robot.fault()),
+                    'operational': bool(robot.operational()),
+                })
+
+            elif cmd == 'switch_tool':
+                if robot is None:
+                    reply({'ok': False, 'error': '尚未 connect'})
+                    continue
+                want = str(msg.get('name', '')).strip()
+
+                # 切换工具要求 IDLE；这一步是同步的，因为切换本身不能被打断。
+                err = ensure_idle(robot, flexivrdk.Mode, timeout_s=12.0)
+                if err:
+                    reply({'ok': False, 'error': err})
+                    continue
+
+                try:
+                    with rlock:
+                        tool = flexivrdk.Tool(robot)
+                        groups = single_arm_groups(robot)
+                        available = [str(n) for n in (tool.list() or [])]
+                        anchor = groups[0] if groups else None
+
+                        if not want:
+                            # 纯查询：不切换，把当前工具与参数报回去。
+                            params = tool_params(tool, anchor)
+                            reply({
+                                'ok': True,
+                                'switched': False,
+                                'current': tool_name(tool, anchor),
+                                'available': available,
+                                **params,
+                            })
+                            continue
+
+                        target = resolve_tool_name(tool, want)
+                        already = all(tool_name(tool, g) == target for g in groups)
+                        if not already:
+                            for g in groups:
+                                switch_tool(tool, g, target)
+                            time.sleep(0.3)
+
+                        params = tool_params(tool, anchor)
+                        reply({
+                            'ok': True,
+                            'switched': not already,
+                            'already': already,
+                            'current': tool_name(tool, anchor),
+                            'target': target,
+                            'available': available,
+                            **params,
+                        })
+                except Exception as exc:  # noqa: BLE001
+                    reply({'ok': False, 'error': str(exc)})
+
+            elif cmd == 'status':
+                if robot is None:
+                    reply({'ok': False, 'connected': False, 'error': '尚未 connect'})
+                    continue
+                info = {'ok': True, 'connected': True, 'drag': drag_on}
+                try:
+                    info['operational'] = bool(robot.operational())
+                    info['fault'] = bool(robot.fault())
+                    info['mode'] = str(robot.mode())
+                except Exception as exc:  # noqa: BLE001
+                    info['error'] = str(exc)
+                try:
+                    with rlock:
+                        tool = flexivrdk.Tool(robot)
+                        groups = single_arm_groups(robot)
+                        info['tool'] = tool_name(tool, groups[0] if groups else None)
+                except Exception:
+                    info['tool'] = ''
+                reply(info)
+
             elif cmd == 'quit':
                 if soft is not None:
                     soft.stop()

src/arm_application/README.md

@@ -29,7 +29,7 @@
 | `config/perception.yaml` `servo.yaml` | 默认值，被 `site.yaml` 覆盖 |
 | `config/hole_detect.yaml` | `two_stage` 缺省；被 `site.yaml hole_detect` 覆盖 |
 | `config/hole_insert.yaml` | `yolo`/`hough` 缺省 |
-| `config/orisys/dd03.json` | Orisys 厂家配置，路径写在 `site.yaml tactile.config_file` |
+| ~~`config/orisys/dd03.json`~~ | 已迁走：型号配置随 SDK 放在 `devices/sensors/orisys/models/dd03/config.json`，路径写在 `site.yaml tactile.config_file` |
 | `config/handeye_result_89.yaml` / `_91.yaml` | 该臂手眼 T（标定写入；禁止混用） |
 | `config/handeye_poses_89.yaml` / `_91.yaml` | 该臂示教关节 |
 | `launch/visual_servo.launch.py` | 感知 + 伺服 + 手眼 TF |

src/arm_application/arm_application/geom_ik.py

@@ -1,135 +1,40 @@
 #!/usr/bin/env python3
-"""TF-based geometric Jacobian + damped least-squares position IK step."""
+"""向后兼容 shim —— 实现已移到 :mod:`arm_kinematics.geom_ik`。
 
-from __future__ import annotations
+为什么搬走
+----------
+这套几何雅可比 + DLS 逆解同时被应用层（视觉伺服、搜孔装配）和驱动层
+（``arm_driver_flexiv`` 的柔顺下压）需要。留在 ``arm_application`` 里会让驱动层
+反过来依赖应用层，属于层次倒置。搬到不依赖 ROS 的 ``arm_kinematics`` 后，
+两边都依赖一个底层包。
 
-from typing import List, Optional, Sequence, Tuple
+为什么保留这个文件
+------------------
+``stage2_locate_node`` / ``stage3_search_node`` / ``servo_node`` 等现有代码都在
+``from arm_application.geom_ik import ...``。保留 shim 就不用改动这些已经在跑的
+文件 —— 重构不应牵连能工作的代码。
 
-import numpy as np
+新代码请直接 ``from arm_kinematics import dls_twist``。
+"""
 
-# URDF axes expressed in each child link frame (Rizon4).
+from arm_kinematics.geom_ik import (  # noqa: F401
-RIZON_LINK_AXES = [
+    RIZON_LINK_AXES,
-    (0.0, 0.0, 1.0),  # joint1 -> link1
+    dls_position_step,
-    (0.0, 1.0, 0.0),  # joint2 -> link2
+    dls_translate_hold_ori,
-    (0.0, 0.0, 1.0),
+    dls_twist,
-    (0.0, 1.0, 0.0),
+    geometric_jacobian,
-    (0.0, 0.0, 1.0),
+    load_handeye_T,
-    (0.0, 1.0, 0.0),
+    position_jacobian,
-    (0.0, 0.0, 1.0),
+    quat_xyzw_to_R,
-]
-
-
-def quat_xyzw_to_R(q: Sequence[float]) -> np.ndarray:
-    x, y, z, w = [float(v) for v in q]
-    n = np.sqrt(x * x + y * y + z * z + w * w) + 1e-12
-    x, y, z, w = x / n, y / n, z / n, w / n
-    return np.array(
-        [
-            [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
-            [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
-            [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
-        ],
-        dtype=float,
 )
 
-
+__all__ = [
-def geometric_jacobian(
+    "RIZON_LINK_AXES",
-    link_R: Sequence[np.ndarray],
+    "quat_xyzw_to_R",
-    link_p: Sequence[np.ndarray],
+    "geometric_jacobian",
-    ee_p: np.ndarray,
+    "position_jacobian",
-    axes_local: Sequence[Tuple[float, float, float]] = RIZON_LINK_AXES,
+    "dls_position_step",
-) -> np.ndarray:
+    "dls_twist",
-    """Return 6x7 geometric Jacobian in base frame (linear then angular)."""
+    "dls_translate_hold_ori",
-    n = len(axes_local)
+    "load_handeye_T",
-    J = np.zeros((6, n), dtype=float)
+]
-    pee = np.asarray(ee_p, dtype=float).reshape(3)
-    for i in range(n):
-        R = np.asarray(link_R[i], dtype=float).reshape(3, 3)
-        p = np.asarray(link_p[i], dtype=float).reshape(3)
-        z = R @ np.asarray(axes_local[i], dtype=float).reshape(3)
-        J[:3, i] = np.cross(z, pee - p)
-        J[3:, i] = z
-    return J
-
-
-def position_jacobian(
-    link_R: Sequence[np.ndarray],
-    link_p: Sequence[np.ndarray],
-    ee_p: np.ndarray,
-    axes_local: Sequence[Tuple[float, float, float]] = RIZON_LINK_AXES,
-) -> np.ndarray:
-    """Return 3x7 translational Jacobian in base frame."""
-    return geometric_jacobian(link_R, link_p, ee_p, axes_local)[:3, :]
-
-
-def dls_position_step(
-    J: np.ndarray,
-    dp_base: np.ndarray,
-    *,
-    damping: float = 0.05,
-    max_dq: float = 0.05,
-) -> np.ndarray:
-    """Solve J dq = dp with damped least squares; clip |dq|."""
-    dp = np.asarray(dp_base, dtype=float).reshape(3)
-    JT = J.T
-    A = J @ JT + (damping ** 2) * np.eye(3)
-    dq = JT @ np.linalg.solve(A, dp)
-    dq = np.clip(dq, -abs(max_dq), abs(max_dq))
-    return dq
-
-
-def dls_twist(
-    J6: np.ndarray,
-    v_base: np.ndarray,
-    *,
-    w_base: Optional[np.ndarray] = None,
-    damping: float = 0.08,
-    max_qdot: float = 0.80,
-    ori_weight: float = 1.2,
-) -> np.ndarray:
-    """Solve J qdot = [v; w] with orientation held (w≈0 by default)."""
-    dx = np.zeros(6, dtype=float)
-    dx[:3] = np.asarray(v_base, dtype=float).reshape(3)
-    if w_base is not None:
-        dx[3:] = np.asarray(w_base, dtype=float).reshape(3)
-    w = np.array([1.0, 1.0, 1.0, ori_weight, ori_weight, ori_weight], dtype=float)
-    Jw = J6 * w[:, None]
-    dxw = dx * w
-    JT = Jw.T
-    A = Jw @ JT + (damping ** 2) * np.eye(6)
-    qdot = JT @ np.linalg.solve(A, dxw)
-    return np.clip(qdot, -abs(max_qdot), abs(max_qdot))
-
-
-def dls_translate_hold_ori(
-    J6: np.ndarray,
-    dp_base: np.ndarray,
-    *,
-    damping: float = 0.08,
-    max_dq: float = 0.05,
-    ori_weight: float = 1.2,
-) -> np.ndarray:
-    """Translate EE by dp while keeping orientation (wx,wy,wz ≈ 0)."""
-    dx = np.zeros(6, dtype=float)
-    dx[:3] = np.asarray(dp_base, dtype=float).reshape(3)
-    w = np.array([1.0, 1.0, 1.0, ori_weight, ori_weight, ori_weight], dtype=float)
-    Jw = J6 * w[:, None]
-    dxw = dx * w
-    JT = Jw.T
-    A = Jw @ JT + (damping ** 2) * np.eye(6)
-    dq = JT @ np.linalg.solve(A, dxw)
-    return np.clip(dq, -abs(max_dq), abs(max_dq))
-
-
-def load_handeye_T(path: str) -> np.ndarray:
-    """Load 4x4 from handeye_result.yaml (T_4x4 list)."""
-    import yaml
-
-    with open(path, 'r', encoding='utf-8') as f:
-        data = yaml.safe_load(f)
-    if not isinstance(data, dict) or 'T_4x4' not in data:
-        raise ValueError(f'No T_4x4 in hand-eye file: {path}')
-    T = np.array(data['T_4x4'], dtype=float)
-    if T.shape != (4, 4):
-        raise ValueError(f'T_4x4 shape {T.shape}, expected (4,4)')
-    return T

src/arm_application/arm_application/image_utils.py

@@ -0,0 +1,149 @@
+"""把 numpy 数组打包成 ``sensor_msgs/Image``。
+
+为什么需要
+----------
+Orisys 视触觉传感器的数据分两类：
+
+* **三力**（``fnormal`` / ``fshearx`` / ``fsheary``）—— 每帧几个标量，走子进程
+  队列几乎没有开销。
+* **图像类**（形变图 / 深度图 / 力矢量场）—— 每帧几百 KB 到几 MB。原先
+  ``tactile_worker`` 为了性能**故意不过队列**，于是这些数据从未离开子进程、
+  也从未发布到 ROS 话题上。
+
+本模块只负责最后一公里：把已经拿到的数组正确打包成 ``Image``。放在单独文件
+里是为了能脱离相机单测 —— 编码、``step``、通道顺序这些细节一旦写错，下游
+拿到的图会静默错位（图像撕裂、颜色通道互换），而不是报错。
+
+支持的形状与编码
+----------------
+===============  ==============  ==========================================
+形状              dtype           编码
+===============  ==============  ==========================================
+``(H, W)``       ``uint8``       ``mono8``
+``(H, W)``       ``uint16``      ``mono16``
+``(H, W)``       ``float32``     ``32FC1``
+``(H, W, 1)``    任意            ``mono8``
+``(H, W, 2)``    ``float32``     ``32FC2``（力矢量场：每像素 dx, dy）
+``(H, W, 3)``    ``uint8``       ``rgb8``
+``(H, W, 4)``    ``uint8``       ``rgba8``
+===============  ==============  ==========================================
+
+2 通道的 ``32FC2`` 是 ROS 里表达矢量场的标准做法，``step = W * 4 * 2``。
+"""
+
+from __future__ import annotations
+
+from typing import Optional, Tuple
+
+import numpy as np
+from sensor_msgs.msg import Image
+
+__all__ = ["ndarray_to_image", "infer_encoding", "UnsupportedImageError"]
+
+
+class UnsupportedImageError(ValueError):
+    """数组的形状/类型没有对应的 ROS 编码。
+
+    宁可显式报错也不要猜 —— 猜错的结果是下游拿到一张静默错位的图。
+    错误消息里带上实际形状与 dtype，方便对照本文档的表格。
+    """
+
+
+#: (通道数, dtype) -> ROS 编码
+_ENCODING_BY_CHANNELS = {
+    (1, np.dtype(np.uint8)): "mono8",
+    (1, np.dtype(np.uint16)): "mono16",
+    (1, np.dtype(np.float32)): "32FC1",
+    (2, np.dtype(np.float32)): "32FC2",
+    (3, np.dtype(np.uint8)): "rgb8",
+    (4, np.dtype(np.uint8)): "rgba8",
+}
+
+
+def infer_encoding(array: np.ndarray) -> str:
+    """推断 ROS 编码。推不出来时抛 :class:`UnsupportedImageError`。"""
+    arr = np.asarray(array)
+    if arr.ndim == 2:
+        channels = 1
+    elif arr.ndim == 3:
+        channels = arr.shape[2]
+    else:
+        raise UnsupportedImageError(
+            f"只支持 2D（灰度）或 3D（H,W,C）数组，收到 {arr.ndim}D，形状 {arr.shape}"
+        )
+
+    key = (channels, arr.dtype)
+    encoding = _ENCODING_BY_CHANNELS.get(key)
+    if encoding is None:
+        supported = ", ".join(
+            f"{enc}({c}通道/{d.name})"
+            for (c, d), enc in sorted(_ENCODING_BY_CHANNELS.items(), key=lambda kv: kv[1])
+        )
+        raise UnsupportedImageError(
+            f"没有 {channels} 通道 + {arr.dtype.name} 对应的 ROS 编码。"
+            f"支持: {supported}。"
+            f"（收到形状 {arr.shape}）"
+        )
+    return encoding
+
+
+def _layout(array: np.ndarray) -> Tuple[int, int, int]:
+    """返回 ``(height, width, channels)``，并把 (H,W,1) 归一化成 2D 语义。"""
+    arr = np.asarray(array)
+    if arr.ndim == 2:
+        h, w = arr.shape
+        return int(h), int(w), 1
+    h, w, c = arr.shape
+    return int(h), int(w), int(c)
+
+
+def ndarray_to_image(
+    array: np.ndarray,
+    *,
+    frame_id: str = "",
+    stamp=None,
+    encoding: Optional[str] = None,
+) -> Image:
+    """把数组打包成 ``sensor_msgs/Image``。
+
+    参数
+    ----
+    array:
+        图像或矢量场。必须是 C 连续（``np.ascontiguousarray`` 会自动处理）。
+    encoding:
+        显式指定编码以覆盖推断。仅在确实知道自己在做什么时使用 ——
+        编码与 dtype 不匹配会让下游按错误的元素大小解读 ``data``。
+    """
+    arr = np.asarray(array)
+    if encoding is None:
+        encoding = infer_encoding(arr)
+
+    height, width, channels = _layout(arr)
+
+    # (H,W,1) 在内存里与 (H,W) 连续一致，压平即可。
+    flat = np.ascontiguousarray(arr).reshape(-1)
+
+    msg = Image()
+    if stamp is not None:
+        msg.header.stamp = stamp
+    msg.header.frame_id = frame_id
+    msg.height = height
+    msg.width = width
+    msg.encoding = encoding
+    msg.is_bigendian = False
+    msg.step = int(width * channels * flat.dtype.itemsize)
+    msg.data = flat.tobytes()
+    return msg
+
+
+def expected_step(array: np.ndarray, encoding: Optional[str] = None) -> int:
+    """算出应当写入 ``Image.step`` 的值。测试与自检用。"""
+    arr = np.asarray(array)
+    _, width, channels = _layout(arr)
+    if encoding is not None:
+        channels = {"mono8": 1, "mono16": 1, "32FC1": 1, "32FC2": 2,
+                    "rgb8": 3, "rgba8": 4}.get(encoding, channels)
+        itemsize = {"mono8": 1, "mono16": 2, "32FC1": 4, "32FC2": 4,
+                    "rgb8": 1, "rgba8": 1}.get(encoding, arr.dtype.itemsize)
+        return int(width * channels * itemsize)
+    return int(width * channels * arr.dtype.itemsize)

src/arm_application/arm_application/orisys_tactile.py

@@ -32,9 +32,28 @@ class OrisysDualConfig:
     baseline_seconds: float = 2.0
     filter_alpha: float = 0.25
     backend: str = "orisys"  # orisys | mock
+    image_hz: float = 0.0
+    """视触觉图像（深度/形变图）的发布频率。0 = 不发布图像。
+
+    为什么默认关：图像每帧几百 KB，跨进程序列化 + ROS 发布的开销远大于三力。
+    三力路径必须跑在 30fps（接触判据靠它），图像只是给人看和给算法备用，
+    默认不开以免动到已经调好的三力时序。装配现场按需在 site.yaml 打开。"""
 
 
 def resolve_orisys_config(config_name: str, config_file: str = "") -> str:
+    """把型号名解析成实际的 config.json 路径。
+
+    查找顺序（先命中先用）：
+
+    1. 显式传入的 ``config_file``
+    2. 参数本身就是存在的文件路径
+    3. ``devices/sensors/orisys/models/<型号>/config.json`` —— 型号配置随 SDK 放，
+       这是现在唯一推荐的位置
+    4. 老位置 ``src/arm_application/config/orisys/<型号>.json``（兼容保留：
+       站点自己放的定制配置可能还在这里）
+    5. Orisys 包自带的 ``configs/<型号>.json``
+    6. 都没找到就把原样返回，交给 SDK 自己解析（它支持内置型号名）
+    """
     if config_file:
         p = Path(config_file).expanduser()
         if p.is_file():
@@ -43,14 +62,19 @@ def resolve_orisys_config(config_name: str, config_file: str = "") -> str:
     p = Path(raw).expanduser()
     if p.is_file():
         return str(p)
-    pkg = ros2_root() / "src" / "arm_application" / "config" / "orisys" / f"{raw}.json"
+
-    if pkg.is_file():
+    root = ros2_root()
-        return str(pkg)
+    for cand in (
+        root / "devices" / "sensors" / "orisys" / "models" / raw / "config.json",
+        root / "src" / "arm_application" / "config" / "orisys" / f"{raw}.json",
+    ):
+        if cand.is_file():
+            return str(cand)
     try:
         import orisys
 
-        root = Path(orisys.__file__).resolve().parent.parent
+        pkg_root = Path(orisys.__file__).resolve().parent.parent
-        for cand in (root / "configs" / f"{raw}.json", root / "config" / f"{raw}.json"):
+        for cand in (pkg_root / "configs" / f"{raw}.json", pkg_root / "config" / f"{raw}.json"):
             if cand.is_file():
                 return str(cand)
     except Exception:
@@ -63,7 +87,9 @@ def load_orisys_from_site(data: dict) -> OrisysDualConfig:
     left_raw = t.get("left") if isinstance(t.get("left"), dict) else {}
     right_raw = t.get("right") if isinstance(t.get("right"), dict) else {}
     cfg_file = str(t.get("config_file") or "")
-    cfg_name = str(t.get("config") or "dd03")
+    # model 是设备契约层选型号用的键（对应 models/<型号>/ 目录名）；
+    # config 是它的旧名，仅为兼容老站点文件保留，站点里应统一用 model。
+    cfg_name = str(t.get("model") or t.get("config") or "dd03")
     resolved = resolve_orisys_config(cfg_name, cfg_file)
     common = dict(
         config_name=resolved,
@@ -100,9 +126,37 @@ def load_orisys_from_site(data: dict) -> OrisysDualConfig:
         baseline_seconds=float(t.get("baseline_seconds") or 2.0),
         filter_alpha=float(t.get("filter_alpha") or 0.25),
         backend=str(t.get("backend") or "orisys"),
+        image_hz=float(t.get("image_hz") or 0.0),
     )
 
 
+def read_vector_map(sensor: Any) -> Optional[np.ndarray]:
+    """尝试从传感器读出力矢量场（每像素的切向力/位移矢量）。
+
+    为什么要"尝试"
+    --------------
+    Orisys 的 Python 绑定是编译过的，公开接口里没有一个明确叫
+    "force vector map" 的方法。按语义最接近的是 ``compute_flow()``
+    （形变光流 ≈ 切向位移场 ≈ 力矢量场），其次是 ``compute_hhd()``。
+    不同 SDK 版本这两个方法的返回值也不一样 —— 有的是 ``(H,W,2)`` 数组，
+    有的返回 ``bool``（只给"形变是否超阈值"）或可视化图。
+
+    所以这里逐个试，只接受真正是二维以上数组的结果；都拿不到就返回 ``None``，
+    上层跳过该话题而不是发一张假图。哪一路成功会打到日志里，便于现场核对。
+    """
+    for name in ("compute_flow", "compute_hhd", "compute_deformation"):
+        fn = getattr(sensor, name, None)
+        if fn is None:
+            continue
+        try:
+            result = fn()
+        except Exception:
+            continue
+        if isinstance(result, np.ndarray) and result.ndim >= 2 and result.size:
+            return result
+    return None
+
+
 def _parse_video(value) -> int | str:
     if isinstance(value, int):
         return value
@@ -153,13 +207,17 @@ class OrisysTactile(TactileInterface):
         self._calibrate_baseline()
         self._connected = True
 
-    def read_side(self, side: str) -> dict[str, Any]:
+    def read_side(self, side: str, want_vector: bool = False) -> dict[str, Any]:
-        """读指定侧（单侧进程模式）；未连接/未知侧返回全零。"""
+        """读指定侧（单侧进程模式）；未连接/未知侧返回全零。
+
+        ``want_vector=True`` 时额外计算力矢量场（逐像素运算，较贵）——
+        只应在需要发布矢量图的那些帧上打开。
+        """
         if not self._connected:
             return self._empty_sample(side)
         for item in self._sensors:
             if item["side"] == side:
-                return self._read_single(item)
+                return self._read_single(item, want_vector=want_vector)
         return self._empty_sample(side)
 
     def disconnect(self) -> None:
@@ -226,7 +284,7 @@ class OrisysTactile(TactileInterface):
             raw={"left": left, "right": right, "centroid": self._estimate_centroid(left, right)},
         )
 
-    def _read_single(self, item: dict) -> dict[str, Any]:
+    def _read_single(self, item: dict, want_vector: bool = False) -> dict[str, Any]:
         sensor = item["sensor"]
         cfg = item["cfg"]
         img = sensor.get_img()
@@ -263,6 +321,9 @@ class OrisysTactile(TactileInterface):
             "centroid": centroid,
             "centroid_found": centroid_found,
             "img": img,
+            # 矢量场只在被要求时才算：compute_flow 是逐像素运算，每帧都做会
+            # 明显增加 worker 耗时，可能拖累三力的 30fps。
+            "vector": read_vector_map(sensor) if want_vector else None,
         }
 
     def _empty_sample(self, side: str = "unknown") -> dict[str, Any]:
@@ -276,6 +337,7 @@ class OrisysTactile(TactileInterface):
             "centroid": (0.0, 0.0),
             "centroid_found": False,
             "img": None,
+            "vector": None,
         }
 
     def _calibrate_baseline(self) -> None:
@@ -403,10 +465,17 @@ def _usb_self_heal(vid_src, side: str) -> None:
         return f"[{side}] USB 自愈失败 {dev}: {exc}"
 
 
-def tactile_worker(side: str, cfg: OrisysDualConfig, q, stop_event) -> None:
+def tactile_worker(side: str, cfg: OrisysDualConfig, q, stop_event, image_q=None) -> None:
-    """独立进程：开一侧传感器，按 target_fps 把最新样本推进队列（图像不过队列）。
+    """独立进程：开一侧传感器，把样本推进队列。
+
+    q: mp.Queue（调用方给 maxsize≈2，满了丢旧留新）—— 三力等轻量数据，每帧都推。
+    image_q: mp.Queue 或 None —— 视触觉图像与力矢量场。
+
+        图像默认**不过队列**（``image_q=None``）：每帧几百 KB，跨进程序列化 +
+        ROS 发布的开销会挤占三力的 30fps 时序。需要图像时调用方传一个
+        ``maxsize=1`` 的队列，并按 ``cfg.image_hz`` 节流推送 —— 队列满时丢旧留新，
+        主进程来不及消费也只会丢图像，不会影响三力。
 
-    q: mp.Queue（调用方给 maxsize≈2，满了丢旧留新）
     stop_event: mp.Event，置位后干净退出（先 disconnect 释放 V4L2 设备）
 
     自动重连：摄像头被强占/USB 抖动后会出现 get_img 持续 select 超时（空读，
@@ -429,7 +498,23 @@ def tactile_worker(side: str, cfg: OrisysDualConfig, q, stop_event) -> None:
         except Exception:
             pass
 
+    def _put_image(item: dict) -> None:
+        """图像入队：只保留最新一帧。队列满说明主进程没在消费，丢掉旧的即可。"""
+        try:
+            if image_q is None:
+                return
+            if image_q.full():
+                try:
+                    image_q.get_nowait()
+                except Exception:
+                    pass
+            image_q.put(item)
+        except Exception:
+            pass
+
     period = 1.0 / max(1.0, float(getattr(cfg.left if side == "left" else cfg.right, "target_fps", 30.0)))
+    img_period = (1.0 / cfg.image_hz) if (image_q is not None and cfg.image_hz > 0) else 0.0
+    last_img_t = 0.0
     vid_src = getattr(cfg.left if side == "left" else cfg.right, "vid_src", None)
     tac: Optional[OrisysTactile] = None
     bad = 0
@@ -452,9 +537,11 @@ def tactile_worker(side: str, cfg: OrisysDualConfig, q, stop_event) -> None:
                 time.sleep(min(backoff, 3.0))
                 backoff *= 2.0
                 continue
+        # 只有图像帧才算矢量场：compute_flow 是逐像素运算，每帧都算会拖累三力。
+        want_img = image_q is not None and (time.perf_counter() - last_img_t) >= img_period
         t0 = time.perf_counter()
         try:
-            d = tac.read_side(side)
+            d = tac.read_side(side, want_vector=want_img)
             empty = d.get("img") is None
         except Exception:
             d, empty = None, True
@@ -478,7 +565,16 @@ def tactile_worker(side: str, cfg: OrisysDualConfig, q, stop_event) -> None:
             bad = 0
             dead_cycles = 0
         if d is not None and not empty:
-            d = {k: v for k, v in d.items() if k != "img"}
+            if want_img:
+                last_img_t = time.perf_counter()
+                _put_image({
+                    "side": side,
+                    "img": d.get("img"),
+                    "vector": d.get("vector"),
+                    "stamp": time.time(),
+                })
+            # 图像与矢量场已单独走 image_q；三力这条轻量队列不背它们。
+            d = {k: v for k, v in d.items() if k not in ("img", "vector")}
             d["error"] = ""
             _put(d)
         rest = period - (time.perf_counter() - t0)

src/arm_application/arm_application/tactile_node.py

@@ -12,22 +12,32 @@ import multiprocessing as mp
 import signal
 import time
 
+import numpy as np
 import rclpy
 from rclpy.node import Node
 
+from arm_application.image_utils import UnsupportedImageError, ndarray_to_image
 from arm_application.orisys_tactile import load_orisys_from_site, tactile_worker
 from arm_application.site_io import load_site_dict, site_path
 from arm_msgs.msg import TactileStatus
+from sensor_msgs.msg import Image
 
 
 class TactileNode(Node):
     def __init__(self) -> None:
         super().__init__("tactile")
         self.declare_parameter("publish_hz", 20.0)
+        # 下面两个可以用 ROS 参数覆盖 site.yaml，便于不改站点文件就做验证
+        # （例如 mock 后端 + 打开图像话题来确认链路通）。
+        self.declare_parameter("backend", "")
+        self.declare_parameter("image_hz", -1.0)
         data = load_site_dict()
         t = data.get("tactile") or {}
         hz = float(t.get("publish_hz") or self.get_parameter("publish_hz").value or 20.0)
-        self._backend = str(t.get("backend") or "orisys").strip().lower()
+        backend_override = str(self.get_parameter("backend").value).strip()
+        self._backend = (
+            backend_override or str(t.get("backend") or "orisys")
+        ).strip().lower()
         self._mock = self._backend in ("mock", "mock_orisys")
 
         self._pub = {
@@ -41,6 +51,37 @@ class TactileNode(Node):
         self._queues: dict[str, object] = {}
         self._stop = None
 
+        # --- 视触觉图像 / 力矢量场 ---
+        # 默认关闭（image_hz=0）：图像每帧几百 KB，不该拖累三力的 30fps 时序。
+        # 打开后走一条 maxsize=1 的独立队列，满则丢旧留新 —— 主进程来不及消费
+        # 只会丢图像，三力一条不受影响。
+        self._image_hz = (
+            float(self.get_parameter("image_hz").value)
+            if float(self.get_parameter("image_hz").value) >= 0
+            else float(t.get("image_hz") or 0.0)
+        )
+        self._image_q = None
+        self._image_warned: set[str] = set()
+        self._image_pub: dict[str, object] = {}
+        self._vector_pub: dict[str, object] = {}
+        self._image_count: dict[str, int] = {}
+        self._last_mock_img = 0.0
+
+        # 图像 publisher 与后端无关 —— mock 后端也要能发。
+        # 否则"图像话题到底通不通"只能等到有相机、上真机那天才发现。
+        if self._image_hz > 0:
+            for side in ("left", "right"):
+                self._image_pub[side] = self.create_publisher(
+                    Image, f"/tactile/{side}/image", 5
+                )
+                self._vector_pub[side] = self.create_publisher(
+                    Image, f"/tactile/{side}/vector", 5
+                )
+            self.get_logger().info(
+                f"视触觉图像已开启: {self._image_hz:.1f}Hz "
+                f"→ /tactile/{{left,right}}/image 与 /vector"
+            )
+
         if self._mock:
             self._connected = {"left": True, "right": True}
             self.get_logger().info("tactile mock backend")
@@ -48,11 +89,14 @@ class TactileNode(Node):
             cfg = load_orisys_from_site(data)
             ctx = mp.get_context("spawn")  # spawn：子进程不带 DDS 线程，CUDA 上下文干净
             self._stop = ctx.Event()
+            if self._image_hz > 0:
+                self._image_q = ctx.Queue(maxsize=1)
             for side in ("left", "right"):
                 q = ctx.Queue(maxsize=2)
                 self._queues[side] = q
                 p = ctx.Process(
-                    target=tactile_worker, args=(side, cfg, q, self._stop),
+                    target=tactile_worker,
+                    args=(side, cfg, q, self._stop, self._image_q),
                     name=f"tactile_{side}", daemon=True,
                 )
                 p.start()
@@ -81,7 +125,101 @@ class TactileNode(Node):
         self._connected[side] = True
         self._latest[side] = (time.monotonic(), latest)
 
+    def _drain_images(self) -> None:
+        """把 worker 推来的最新图像 / 力矢量场发布出去。
+
+        只取最新一帧：worker 已按 ``image_hz`` 节流、队列 ``maxsize=1``，
+        所以这里读到的永远是最新画面；主进程偶尔卡一下，丢掉的是中间帧
+        而不是最新帧。
+        """
+        if self._image_q is None:
+            return
+        latest = None
+        while True:
+            try:
+                latest = self._image_q.get_nowait()
+            except Exception:
+                break
+        if latest is None:
+            return
+
+        side = str(latest.get("side") or "")
+        if side not in self._image_pub:
+            return
+        stamp = self.get_clock().now().to_msg()
+        frame_id = f"{side}_tactile"
+
+        self._publish_array(
+            self._image_pub[side], latest.get("img"), frame_id, stamp, side, "image"
+        )
+        self._publish_array(
+            self._vector_pub[side], latest.get("vector"), frame_id, stamp, side, "vector"
+        )
+
+    def _publish_array(self, pub, array, frame_id: str, stamp, side: str, tag: str) -> None:
+        """打包并发布一个数组；形状/类型不支持时跳过该话题而不是崩溃。
+
+        ``array is None`` 是正常情况（该侧没有矢量场输出、或本帧没算），
+        不报错也不发空图 —— 发一张全零的假图会让下游以为接触到了东西。
+        """
+        if array is None:
+            return
+        key = f"{side}_{tag}"
+        try:
+            msg = ndarray_to_image(array, frame_id=frame_id, stamp=stamp)
+        except UnsupportedImageError as exc:
+            # 编码不支持是固定的（SDK 给的 dtype 不会变），每侧每种图只提醒一次，
+            # 否则 5Hz 的警告会把现场日志冲掉。
+            if key not in self._image_warned:
+                self._image_warned.add(key)
+                self.get_logger().warn(
+                    f"[{key}] 无法打包成 Image，已跳过该话题: {exc}"
+                )
+            return
+
+        try:
+            pub.publish(msg)
+        except Exception as exc:
+            if f"{key}_pub" not in self._image_warned:
+                self._image_warned.add(f"{key}_pub")
+                self.get_logger().warn(f"[{key}] 发布失败: {exc}")
+            return
+
+        if self._image_count.get(key, 0) == 0:
+            self.get_logger().info(
+                f"[{key}] 首帧已发布 ({msg.width}x{msg.height} {msg.encoding})"
+            )
+        self._image_count[key] = self._image_count.get(key, 0) + 1
+
+    def _publish_mock_images(self, force: float) -> None:
+        """mock 后端下造一张形变图 + 一张力矢量场，用来打通图像发布链路。
+
+        没有传感器时也要能把 ``/tactile/<side>/image`` 与 ``/vector`` 跑起来 ——
+        否则这两个话题是否真的能出图，只能等到上真机那天才知道。
+
+        图像内容模仿视触觉的形变：中间一个高斯隆起（法向深度），
+        连同指向中心的切向流（矢量场），接触越深幅度越大。
+        """
+        if not self._image_pub:
+            return
+        stamp = self.get_clock().now().to_msg()
+        h, w = 48, 64
+        yy, xx = np.mgrid[0:h, 0:w]
+        cx, cy = w / 2.0, h / 2.0
+        bump = np.exp(-(((xx - cx) ** 2 + (yy - cy) ** 2) / (2.0 * 9.0 ** 2)))
+
+        depth = (bump * (0.05 + 0.5 * float(force))).astype(np.float32)
+        vx = ((cx - xx) / w * bump * float(force)).astype(np.float32)
+        vy = ((cy - yy) / h * bump * float(force)).astype(np.float32)
+        vector = np.dstack([vx, vy])
+
+        for side in ("left", "right"):
+            frame = f"{side}_tactile"
+            self._publish_array(self._image_pub[side], depth, frame, stamp, side, "image")
+            self._publish_array(self._vector_pub[side], vector, frame, stamp, side, "vector")
+
     def _tick(self) -> None:
+        self._drain_images()
         if self._mock:
             self._mock_n += 1
             force = 0.2 if self._mock_n >= 8 else 0.0
@@ -91,6 +229,12 @@ class TactileNode(Node):
                     "fps": 30.0, "is_contact": self._mock_n >= 8,
                     "centroid": (0.0, 0.0),
                 })
+            # mock 的图像也按 image_hz 节流，避免比真机行为更"勤快"。
+            if self._image_hz > 0:
+                now = time.monotonic()
+                if now - self._last_mock_img >= 1.0 / self._image_hz:
+                    self._last_mock_img = now
+                    self._publish_mock_images(force)
             return
         for side in ("left", "right"):
             self._drain(side)

src/arm_application/config/orisys/README.md

@@ -1,10 +0,0 @@
-# Orisys 传感器配置
-
-`dd03.json` 是厂家相机/跟踪参数，由 `site.yaml` → `tactile.config_file` 指向。  
-换型号时把新 json 放到本目录，并改 `tactile.config` / `config_file`。
-
-Orisys Python SDK **不进本仓库**，在工控机：
-
-```bash
-pip install -e /path/to/orisys-sdk
-```

src/arm_application/config/orisys/dd03.json

@@ -1,35 +0,0 @@
-{
-  "name": "dd03",
-  "type": "single_camera",
-  "version": "1.0",
-  "camera": {
-    "resolution_raw": [1280, 720],
-    "fps": 30,
-    "resolution_stitch": [420, 450],
-    "source_scale": 0.4,
-    "old": [672, 728, 1, 0.5],
-    "work": [504, 546, 1],
-    "work2": [420, 450, 0.8],
-    "work3": [350, 375, 4, 0.7],
-    "work4": [280, 300, 4, 0.4, 0.2]
-  },
-  "tracker": {
-    "adaptive": true,
-    "lower_threshold": 12.0,
-    "upper_threshold": 18.0,
-    "loop_closure_interval": 1,
-    "algorithm": "dis",
-    "dis_preset": "medium",
-    "dis_mode": 3,
-    "algorithm_list": ["raft", "dis"],
-    "hhd_downsample": 3
-  },
-  "contact": {
-    "min_contour_area": 3000,
-    "flow_threshold": 0.2,
-    "div_threshold": 0.022,
-    "expansion_threshold": 0.01,
-    "kernel_size": [5, 5],
-    "denoise_sigma": 12
-  }
-}

src/arm_application/package.xml

@@ -19,6 +19,9 @@
   <exec_depend>tf2_ros</exec_depend>
   <exec_depend>python3-yaml</exec_depend>
   <exec_depend>python3-serial</exec_depend>
+  <!-- geom_ik 的实现已移到 arm_kinematics（驱动层也要用，避免层次倒置）。
+       本包保留 arm_application/geom_ik.py 作为 re-export shim。 -->
+  <exec_depend>arm_kinematics</exec_depend>
 
   <export>
     <build_type>ament_python</build_type>

src/arm_application/setup.cfg

@@ -2,3 +2,11 @@
 script_dir=$base/lib/arm_application
 [install]
 install_scripts=$base/lib/arm_application
+
+[tool:pytest]
+# ROS 2 Humble 的 launch_testing / launch_ros 两个 pytest 插件用了旧版 hook
+# 签名（pytest_pycollect_makemodule 多声明了 path 参数），当前 pytest 会在
+# 加载插件阶段就抛 PluginValidationError，连收集都进不去。
+# 本包的单测不涉及 launch 测试，直接禁掉这两个插件。
+addopts = -p no:launch_testing -p no:launch_ros
+testpaths = test

src/arm_application/test/test_image_utils.py

@@ -0,0 +1,215 @@
+"""``image_utils`` 的单测：数组 -> ``sensor_msgs/Image`` 的打包是否正确。
+
+这类代码的错误是**静默**的：编码写错、step 算错、通道顺序颠倒，都不会抛异常，
+只是下游拿到的图裂开或颜色错位 —— 而现场很难判断是相机坏了还是打包错了。
+所以这里把每种编码的字节布局都钉死。
+"""
+
+from __future__ import annotations
+
+import numpy as np
+import pytest
+
+from arm_application.image_utils import (
+    UnsupportedImageError,
+    expected_step,
+    infer_encoding,
+    ndarray_to_image,
+)
+
+
+# ---------------------------------------------------------------------------
+# 编码推断
+# ---------------------------------------------------------------------------
+
+
+class TestInferEncoding:
+    def test_mono8(self):
+        assert infer_encoding(np.zeros((4, 5), np.uint8)) == "mono8"
+
+    def test_mono16(self):
+        assert infer_encoding(np.zeros((4, 5), np.uint16)) == "mono16"
+
+    def test_float_depth_is_32fc1(self):
+        """深度图常以米为单位的 float32 出现，必须是 32FC1 而不是 mono。"""
+        assert infer_encoding(np.zeros((4, 5), np.float32)) == "32FC1"
+
+    def test_vector_field_is_32fc2(self):
+        """力矢量场（每像素 dx, dy）用 32FC2 表达，这是 ROS 里的标准做法。"""
+        assert infer_encoding(np.zeros((4, 5, 2), np.float32)) == "32FC2"
+
+    def test_rgb8(self):
+        assert infer_encoding(np.zeros((4, 5, 3), np.uint8)) == "rgb8"
+
+    def test_rgba8(self):
+        assert infer_encoding(np.zeros((4, 5, 4), np.uint8)) == "rgba8"
+
+    def test_single_channel_3d_is_mono(self):
+        """(H,W,1) 等价于灰度，不要因为没有 1 通道的编码表项就报错。"""
+        assert infer_encoding(np.zeros((4, 5, 1), np.uint8)) == "mono8"
+
+    def test_1d_is_rejected(self):
+        with pytest.raises(UnsupportedImageError) as exc:
+            infer_encoding(np.zeros(10, np.uint8))
+        assert "2D" in str(exc.value)
+
+    def test_4d_is_rejected(self):
+        with pytest.raises(UnsupportedImageError):
+            infer_encoding(np.zeros((2, 3, 4, 5), np.uint8))
+
+    def test_uint16_rgb_is_rejected(self):
+        """16 位 RGB 没有标准编码；宁可报错也不要按 8 位打包（会静默错位）。"""
+        with pytest.raises(UnsupportedImageError) as exc:
+            infer_encoding(np.zeros((4, 5, 3), np.uint16))
+        assert "形状" in str(exc.value)
+
+    def test_float64_is_rejected(self):
+        """float64 图很常见（默认 dtype），必须显式转换，不能猜。"""
+        with pytest.raises(UnsupportedImageError):
+            infer_encoding(np.zeros((4, 5), np.float64))
+
+    def test_error_lists_supported_combinations(self):
+        with pytest.raises(UnsupportedImageError) as exc:
+            infer_encoding(np.zeros((3, 3, 5), np.uint8))
+        assert "mono8" in str(exc.value) and "rgb8" in str(exc.value)
+
+
+# ---------------------------------------------------------------------------
+# 打包
+# ---------------------------------------------------------------------------
+
+
+class TestPackaging:
+    def test_mono8_layout(self):
+        arr = np.arange(12, dtype=np.uint8).reshape(3, 4)
+        msg = ndarray_to_image(arr, frame_id="left_tactile")
+        assert (msg.height, msg.width) == (3, 4)
+        assert msg.encoding == "mono8"
+        assert msg.step == 4
+        assert msg.header.frame_id == "left_tactile"
+        assert msg.is_bigendian is False
+        assert bytes(msg.data) == arr.tobytes()
+
+    def test_mono16_step_and_bytes(self):
+        arr = np.arange(12, dtype=np.uint16).reshape(3, 4)
+        msg = ndarray_to_image(arr)
+        assert msg.encoding == "mono16"
+        assert msg.step == 4 * 2  # 每像素 2 字节
+        assert len(msg.data) == 3 * 4 * 2
+        assert bytes(msg.data) == arr.tobytes()
+
+    def test_rgb8_step_is_three_bytes_per_pixel(self):
+        arr = np.zeros((5, 7, 3), np.uint8)
+        msg = ndarray_to_image(arr)
+        assert msg.encoding == "rgb8"
+        assert msg.step == 7 * 3
+        assert msg.height == 5 and msg.width == 7
+
+    def test_rgba8_step(self):
+        msg = ndarray_to_image(np.zeros((5, 7, 4), np.uint8))
+        assert msg.step == 7 * 4
+
+    def test_vector_field_32fc2(self):
+        """矢量场：每像素 2 个 float32，step 必须是 W*8。"""
+        arr = np.zeros((6, 8, 2), np.float32)
+        msg = ndarray_to_image(arr)
+        assert msg.encoding == "32FC2"
+        assert msg.step == 8 * 4 * 2
+        assert len(msg.data) == 6 * 8 * 2 * 4
+
+    def test_float_depth_32fc1(self):
+        msg = ndarray_to_image(np.zeros((6, 8), np.float32))
+        assert msg.encoding == "32FC1"
+        assert msg.step == 8 * 4
+
+    def test_3d_single_channel_flattens_fine(self):
+        """(H,W,1) 压平后与 (H,W) 的字节完全一致。"""
+        a = np.arange(12, dtype=np.uint8).reshape(3, 4, 1)
+        b = np.arange(12, dtype=np.uint8).reshape(3, 4)
+        assert bytes(ndarray_to_image(a).data) == bytes(ndarray_to_image(b).data)
+
+    def test_non_contiguous_input_is_handled(self):
+        """切片产生的非连续数组必须先转连续，否则 tobytes 的顺序是错的。"""
+        base = np.arange(24, dtype=np.uint8).reshape(4, 6)
+        view = base[:, ::2]  # 非连续
+        assert not view.flags["C_CONTIGUOUS"]
+        msg = ndarray_to_image(view)
+        assert msg.width == 3
+        assert bytes(msg.data) == np.ascontiguousarray(view).tobytes()
+
+    def test_width_and_height_are_ints_not_numpy(self):
+        """ROS 消息字段是 int32；塞 numpy 标量在某些版本会失败。"""
+        msg = ndarray_to_image(np.zeros((3, 4), np.uint8))
+        assert isinstance(msg.height, int) and isinstance(msg.width, int)
+        assert isinstance(msg.step, int)
+
+    def test_stamp_is_forwarded(self):
+        """stamp 必须原样传下去 —— 下游靠它做时间对齐。"""
+        from builtin_interfaces.msg import Time
+
+        stamp = Time(sec=123, nanosec=456)
+        msg = ndarray_to_image(np.zeros((2, 2), np.uint8), stamp=stamp)
+        assert msg.header.stamp.sec == 123
+        assert msg.header.stamp.nanosec == 456
+
+    def test_stamp_defaults_to_zero_when_omitted(self):
+        msg = ndarray_to_image(np.zeros((2, 2), np.uint8))
+        assert msg.header.stamp.sec == 0 and msg.header.stamp.nanosec == 0
+
+    def test_fortran_order_is_normalised(self):
+        arr = np.asfortranarray(np.arange(12, dtype=np.uint8).reshape(3, 4))
+        msg = ndarray_to_image(arr)
+        assert bytes(msg.data) == np.ascontiguousarray(arr).tobytes()
+
+    def test_empty_image(self):
+        """零尺寸图：别崩，正常给出 height=0。"""
+        msg = ndarray_to_image(np.zeros((0, 0), np.uint8))
+        assert msg.height == 0 and msg.width == 0
+        assert len(msg.data) == 0
+
+    def test_explicit_encoding_override_is_respected(self):
+        """显式指定编码时要照用 —— 有些相机给出的 BGR 需要按 rgb8 打包。"""
+        arr = np.zeros((3, 4, 3), np.uint8)
+        msg = ndarray_to_image(arr, encoding="rgb8")
+        assert msg.encoding == "rgb8"
+
+
+# ---------------------------------------------------------------------------
+# step 计算与 data 长度的一致性
+# ---------------------------------------------------------------------------
+
+
+class TestStepConsistency:
+    """``step`` 与 ``len(data)`` 必须自洽：``len(data) == step * height``。
+
+    这是下游按行读图时唯一依赖的不变量。不一致会导致整幅图错位。
+    """
+
+    @pytest.mark.parametrize(
+        "shape,dtype",
+        [
+            ((3, 4), np.uint8),
+            ((3, 4), np.uint16),
+            ((3, 4), np.float32),
+            ((3, 4, 1), np.uint8),
+            ((3, 4, 2), np.float32),
+            ((3, 4, 3), np.uint8),
+            ((3, 4, 4), np.uint8),
+            ((1, 1), np.uint8),
+            ((17, 23, 3), np.uint8),
+        ],
+    )
+    def test_data_length_matches_step_times_height(self, shape, dtype):
+        arr = np.zeros(shape, dtype)
+        msg = ndarray_to_image(arr)
+        assert len(msg.data) == msg.step * msg.height, (
+            f"shape={shape} dtype={dtype.name} encoding={msg.encoding} "
+            f"step={msg.step} h={msg.height} len={len(msg.data)}"
+        )
+
+    def test_expected_step_helper_agrees(self):
+        for shape, dtype in [((3, 4), np.uint8), ((3, 4, 3), np.uint8),
+                             ((3, 4, 2), np.float32)]:
+            arr = np.zeros(shape, dtype)
+            msg = ndarray_to_image(arr)
+            assert msg.step == expected_step(arr)

src/arm_application/test/test_orisys_vector.py

@@ -0,0 +1,146 @@
+"""``read_vector_map`` 的单测。
+
+这个函数存在的唯一原因是：Orisys 的 Python 绑定是编译过的，公开接口里没有
+一个明确叫 "force vector map" 的方法，只能按语义去试 ``compute_flow`` /
+``compute_hhd`` / ``compute_deformation``，而且要判断返回值到底是数组
+还是 bool/可视化图。
+
+但它接的是真传感器，没法在单测里调。所以这里用假传感器把每条分支钉死：
+**试错的顺序**和**接受的返回类型**一旦写错，表现是"矢量场话题永远没消息"
+或"发了一张全零假图"，两者都很难从现场现象反推回代码。
+"""
+
+from __future__ import annotations
+
+from typing import Any
+
+import numpy as np
+import pytest
+
+from arm_application.orisys_tactile import read_vector_map
+
+
+class FakeSensor:
+    """可控假传感器。
+
+    传入 ``方法名=返回值``；想让某个方法抛异常就传异常实例。
+    没有列出的方法属性一律不存在 —— 这正好模拟"老版本 SDK 没这个方法"。
+    """
+
+    def __init__(self, **methods: Any) -> None:
+        for name, value in methods.items():
+            if isinstance(value, BaseException):
+                def _raise(_exc=value):
+                    raise _exc
+
+                setattr(self, name, _raise)
+            else:
+                setattr(self, name, (lambda _v=value: _v))
+
+
+class TestPreferredSource:
+    def test_uses_compute_flow_first(self):
+        """compute_flow 语义上最接近"切向力矢量场"，优先级最高。"""
+        flow = np.ones((8, 6, 2), np.float32)
+        sensor = FakeSensor(
+            compute_flow=flow,
+            compute_hhd=np.zeros((8, 6, 3), np.uint8),
+        )
+        assert read_vector_map(sensor) is flow
+
+    def test_falls_back_to_hhd_when_flow_is_bool(self):
+        """有些版本 compute_flow 只返回 bool（"有没有流动"）—— 那不是矢量场，
+        必须继续往下试，而不是把 True 当成图。"""
+        hhd = np.ones((4, 4), np.float32)
+        sensor = FakeSensor(compute_flow=True, compute_hhd=hhd)
+        assert read_vector_map(sensor) is hhd
+
+    def test_falls_back_to_deformation(self):
+        deform = np.ones((5, 5, 2), np.float32)
+        sensor = FakeSensor(compute_flow=False, compute_hhd=None, compute_deformation=deform)
+        assert read_vector_map(sensor) is deform
+
+    def test_skips_flow_that_raises(self):
+        hhd = np.ones((3, 3), np.float32)
+        sensor = FakeSensor(compute_flow=RuntimeError("no cuda"), compute_hhd=hhd)
+        assert read_vector_map(sensor) is hhd
+
+
+class TestRejectedValues:
+    """不是"二维以上数组"的返回值一律不能当矢量场用。"""
+
+    @pytest.mark.parametrize(
+        "bad",
+        [True, False, None, 0, 0.0, "flow", (1, 2), [1.0, 2.0], {"a": 1}],
+    )
+    def test_non_array_values_are_rejected(self, bad):
+        assert read_vector_map(FakeSensor(compute_flow=bad)) is None
+
+    def test_1d_array_is_rejected(self):
+        """1D 没法按 (H,W) 解释成图。"""
+        assert read_vector_map(FakeSensor(compute_flow=np.zeros(10, np.float32))) is None
+
+    def test_empty_array_is_rejected(self):
+        """空数组同样是"没有数据"，不能发一张 0x0 的图。"""
+        assert read_vector_map(FakeSensor(compute_flow=np.zeros((0, 0), np.uint8))) is None
+
+    def test_2d_array_is_accepted(self):
+        """2D 灰度图是合法结果（某些版本把形变强度图当成矢量场返回）。"""
+        arr = np.zeros((4, 5), np.uint8)
+        assert read_vector_map(FakeSensor(compute_flow=arr)) is arr
+
+    def test_scalar_ndarray_is_rejected(self):
+        """0 维 ndarray 没有 (H,W) 语义。"""
+        assert read_vector_map(FakeSensor(compute_flow=np.array(3.0))) is None
+
+
+class TestMissingMethods:
+    def test_all_missing_returns_none(self):
+        """老版本 SDK 三个方法都没有 —— 应当安静返回 None，让上层跳过话题。"""
+        assert read_vector_map(FakeSensor()) is None
+
+    def test_all_raising_returns_none(self):
+        sensor = FakeSensor(
+            compute_flow=RuntimeError("x"),
+            compute_hhd=RuntimeError("y"),
+            compute_deformation=RuntimeError("z"),
+        )
+        assert read_vector_map(sensor) is None
+
+    def test_never_raises_even_when_everything_fails(self):
+        """这是它的契约：探测失败只能返回 None，不能把异常抛给 30fps 的读取循环。"""
+        sensor = FakeSensor(compute_flow=KeyboardInterrupt())
+        # KeyboardInterrupt 是 BaseException；探测逻辑捕获 Exception，
+        # 所以它会被放行 —— 记录当前行为，提醒别在这里拿 BaseException 当普通错误。
+        with pytest.raises(KeyboardInterrupt):
+            read_vector_map(sensor)
+
+
+class TestResultIsUsableByImageUtils:
+    """探测到的数组必须能被 image_utils 打包 —— 否则等于白探测。"""
+
+    def test_flow_shaped_array_packs_as_32fc2(self):
+        from arm_application.image_utils import ndarray_to_image
+
+        flow = np.zeros((8, 6, 2), np.float32)
+        sensor = FakeSensor(compute_flow=flow)
+        arr = read_vector_map(sensor)
+        msg = ndarray_to_image(arr)
+        assert msg.encoding == "32FC2"
+        assert (msg.height, msg.width) == (8, 6)
+
+    def test_grayscale_array_packs_as_mono8(self):
+        from arm_application.image_utils import ndarray_to_image
+
+        arr = read_vector_map(FakeSensor(compute_flow=np.zeros((8, 6), np.uint8)))
+        assert ndarray_to_image(arr).encoding == "mono8"
+
+    def test_float64_result_is_rejected_by_packer_not_silently_truncated(self):
+        """float64 是 numpy 的默认 dtype，SDK 很可能返回它。image_utils 会
+        明确报错而不是按 float32 打包 —— 那样会让每个像素错位 4 字节。"""
+        from arm_application.image_utils import UnsupportedImageError, ndarray_to_image
+
+        arr = read_vector_map(FakeSensor(compute_flow=np.zeros((8, 6, 2), np.float64)))
+        assert arr is not None
+        with pytest.raises(UnsupportedImageError):
+            ndarray_to_image(arr)

src/arm_driver_flexiv/arm_driver_flexiv/__init__.py

@@ -0,0 +1,17 @@
+"""非夕 Flexiv 机械臂驱动适配器。
+
+对外只暴露一个可执行节点 ``flexiv_arm_driver``（见 ``setup.py`` 的
+``console_scripts``），其余模块是它的实现细节：
+
+``rdk_client``
+    管理 ``scripts/rdk_drag_bridge.py`` 长驻子进程的客户端。所有需要独占 RDK
+    的操作都经它转发 —— 本包自身不 import flexivrdk（ABI 不兼容）。
+``site_config``
+    从 ``config/site.yaml`` 读参数默认值，使 ``ros2 run`` 也能直接起节点。
+``compliant_descent``
+    柔顺下压：纯逻辑状态机 + ROS 包装。
+``flexiv_arm_driver``
+    节点本体，装配上面几块。
+"""
+
+__version__ = "0.1.0"

src/arm_driver_flexiv/arm_driver_flexiv/compliant_descent.py

@@ -0,0 +1,806 @@
+"""柔顺下压：从悬停位姿沿工具系 −Z 缓慢下压，直到触觉检测到接触。
+
+为什么不用非夕 RDK 的柔顺模式
+----------------------------
+RDK 的 compliant motion 会接管整臂阻抗，而装配的其它阶段（跟踪 / 对准 / 拧入）
+都走 JTC 位置控制。在两种控制权之间来回切换代价高，且切换瞬间的力冲击对通止规
+这种细长工件风险大。这里改用**应用层力闭环**：小幅步进（默认 0.5 mm）+ 步间读
+触觉，一旦切向力相对基线跳变就立刻停住。任何一步都可安全中断。
+
+文件结构（两层，故意分开）
+--------------------------
+1. :class:`DescentController` —— **纯逻辑**状态机。不 import ROS，所有外部交互
+   （读触觉、走一步、看时间）都通过注入的回调完成。因此它的全部判据 ——
+   接触 / 超时 / 行程上限 / 法向力安全限 / 触觉断流 —— 都能用假回调单测，
+   见 ``test/test_descent_control.py``。
+2. :class:`CompliantDescentRunner` —— ROS 包装。负责 TF 读位形、DLS 逆解、
+   发 JTC 轨迹、订阅触觉，把纯逻辑接到真链路上。
+
+力阈值都是**相对静止基线的增量**
+--------------------------------
+Orisys 读数是形变任意单位且基线会漂移，绝对值没有意义。传感器节点在启动时测定
+基线并在发布前减掉，所以话题上的 ``fshearx`` / ``fsheary`` 已经是增量。
+"""
+
+from __future__ import annotations
+
+import math
+import threading
+import time
+from dataclasses import dataclass
+from enum import Enum
+from typing import Callable, List, Optional, Tuple
+
+import numpy as np
+import rclpy
+from control_msgs.action import FollowJointTrajectory
+from rclpy.action import ActionClient
+from rclpy.callback_groups import CallbackGroup
+from rclpy.node import Node
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+from arm_msgs.action import CompliantDescent
+from arm_msgs.msg import TactileStatus
+from arm_kinematics import RIZON_LINK_AXES, dls_translate_hold_ori, geometric_jacobian
+
+# ---------------------------------------------------------------------------
+# 第一层：纯逻辑状态机
+# ---------------------------------------------------------------------------
+
+
+class DescentOutcome(str, Enum):
+    """下压结束的原因。只有 ``CONTACT`` 算成功。"""
+
+    CONTACT = "contact"
+    TIMEOUT = "timeout"
+    MAX_DESCENT = "max_descent"
+    NORMAL_LIMIT = "normal_limit"
+    TACTILE_STALE = "tactile_stale"
+    MOTION_FAILED = "motion_failed"
+    CANCELLED = "cancelled"
+
+
+#: 各结束原因对应的 ``status`` 字段值（action 的 feedback/result 协议）。
+_STATUS_BY_OUTCOME = {
+    DescentOutcome.CONTACT: "contacting",
+    DescentOutcome.TIMEOUT: "error",
+    DescentOutcome.MAX_DESCENT: "error",
+    DescentOutcome.NORMAL_LIMIT: "error",
+    DescentOutcome.TACTILE_STALE: "error",
+    DescentOutcome.MOTION_FAILED: "error",
+    DescentOutcome.CANCELLED: "retracting",
+}
+
+
+@dataclass(frozen=True)
+class DescentConfig:
+    """一次下压的全部参数。不可变，便于在测试里共享。"""
+
+    contact_axis: int = 0
+    """监测哪条切向通道：0 = 第一路切向（``fshearx``），1 = 第二路（``fsheary``）。"""
+
+    contact_delta: float = 0.0
+    """接触判据：|切向力| 相对基线的增量阈值（传感器原生单位）。"""
+
+    max_normal_delta: float = 0.0
+    """法向力安全上限（相对基线增量）。<= 0 表示不检查。"""
+
+    descent_step_m: float = 0.0005
+    """每步下压量（m）。越小越安全，但接触检测越慢。"""
+
+    descent_speed_mps: float = 0.005
+    """名义下压速度（m/s）。步进间隔 = step / speed。"""
+
+    max_descent_m: float = 0.05
+    """最多下压距离（m）。到顶仍未接触即判失败并停住 —— 这是安全上限。"""
+
+    settle_sec: float = 0.05
+    """每步后额外等待（s），让机械振荡衰减、触觉稳定。"""
+
+    timeout_sec: float = 0.0
+    """总超时（s）。<= 0 时按 ``max_descent_m / descent_speed_mps`` 推算并留余量。"""
+
+    tactile_stale_sec: float = 0.5
+    """触觉数据超过这个时长没更新就判断流并停住（安全）。"""
+
+    @classmethod
+    def from_goal(cls, goal: CompliantDescent.Goal) -> "DescentConfig":
+        """从 action goal 构造。0 / NaN 的字段在这里被解释成默认值。"""
+
+        def _f(value: float, fallback: float) -> float:
+            v = float(value)
+            return fallback if (math.isnan(v) or v == 0.0) else v
+
+        step = _f(goal.descent_step_m, 0.0005)
+        step = min(max(step, 1e-5), 0.01)  # 夹到 [0.01mm, 10mm]，防手滑填错单位
+        return cls(
+            contact_axis=1 if int(goal.contact_axis) == 1 else 0,
+            contact_delta=_f(goal.contact_delta, 0.0),
+            max_normal_delta=float(goal.max_normal_delta),
+            descent_step_m=step,
+            descent_speed_mps=_f(goal.descent_speed_mps, 0.005),
+            max_descent_m=_f(goal.max_descent_m, 0.05),
+            settle_sec=max(0.0, float(goal.settle_sec)),
+            timeout_sec=max(0.0, float(goal.timeout_sec)),
+        )
+
+    @property
+    def step_interval_sec(self) -> float:
+        """步与步之间的间隔：走完一步需要的时间 + 稳定等待。"""
+        if self.descent_speed_mps <= 0:
+            return self.settle_sec
+        return self.descent_step_m / self.descent_speed_mps + self.settle_sec
+
+    @property
+    def effective_timeout_sec(self) -> float:
+        """实际生效的超时。未指定时按行程推算并留 3 倍余量（至少 2s）。
+
+        留余量是因为实际下压速度受 JTC 加减速限制，名义速度只是上界。
+        """
+        if self.timeout_sec > 0:
+            return self.timeout_sec
+        if self.descent_speed_mps <= 0:
+            return 2.0
+        nominal = self.max_descent_m / self.descent_speed_mps
+        return max(2.0, nominal * 3.0)
+
+
+@dataclass
+class TactileSample:
+    """某一时刻的触觉读数（已减去基线）。"""
+
+    tangential: float
+    """被监测那条切向通道的值。"""
+
+    normal: float
+    """法向力。"""
+
+    age_sec: float
+    """采样距今多久。用来判断数据是否断流。"""
+
+
+def select_tangential(shearx: float, sheary: float, axis: int) -> float:
+    """按 ``contact_axis`` 从两路切向力里挑出要监测的那一路。
+
+    0 = 第一路（``fshearx``），1 = 第二路（``fsheary``）。其它值按 0 处理。
+
+    单独提成纯函数而不是写在 ROS 订阅回调里，是因为"到底看哪一路"是装配能否
+    检测到接触的关键，必须能脱离 ROS 单测 —— 见 ``test_descent_control.py``。
+    """
+    return float(sheary) if int(axis) == 1 else float(shearx)
+
+
+@dataclass
+class DescentResult:
+    """下压结束时的完整结论。"""
+
+    outcome: DescentOutcome
+    descended_m: float = 0.0
+    tangential_delta: float = 0.0
+    normal_delta: float = 0.0
+    message: str = ""
+
+    @property
+    def success(self) -> bool:
+        return self.outcome is DescentOutcome.CONTACT
+
+    @property
+    def status(self) -> str:
+        return _STATUS_BY_OUTCOME.get(self.outcome, "error")
+
+
+class DescentController:
+    """下压状态机（纯逻辑）。
+
+    参数
+    ----
+    cfg:
+        全部阈值与运动参数。
+    read_tactile:
+        读一次触觉。返回 ``None`` 表示数据源不可用（未连接 / 话题没起来）。
+    step_down:
+        沿 −Z 走一步 ``dz``（正数）。返回 ``False`` 表示这一步没走成。
+    now / sleep:
+        注入时间源，测试里可以换成假时钟，避免真的 sleep。
+    is_cancelled:
+        返回 ``True`` 时立刻停下（外部取消或节点在关闭）。
+    on_progress:
+        每次循环结束时回调，用来发 action feedback。
+    """
+
+    def __init__(
+        self,
+        cfg: DescentConfig,
+        read_tactile: Callable[[], Optional[TactileSample]],
+        step_down: Callable[[float], bool],
+        *,
+        now: Callable[[], float] = time.monotonic,
+        sleep: Callable[[float], None] = time.sleep,
+        is_cancelled: Callable[[], bool] = lambda: False,
+        on_progress: Optional[Callable[["DescentProgress"], None]] = None,
+    ) -> None:
+        self.cfg = cfg
+        self._read_tactile = read_tactile
+        self._step_down = step_down
+        self._now = now
+        self._sleep = sleep
+        self._is_cancelled = is_cancelled
+        self._on_progress = on_progress
+
+    def run(self) -> DescentResult:
+        """执行下压，直到接触或触发任一停止条件。"""
+        cfg = self.cfg
+        step = cfg.descent_step_m
+        interval = cfg.step_interval_sec
+        deadline = self._now() + cfg.effective_timeout_sec
+
+        # 用整数步数计数，而不是累加浮点再比较。
+        # 反例：step=0.0005、max=0.05 时，"descended + step > max" 会因为
+        # 0.0005 累加 99 次产生 1e-17 量级的误差而在第 100 步之前跳出，
+        # 结果是行程少走一步、永远到不了上限。计数法没有这个问题，
+        # 而且 descended_m 用 steps_done * step 一次算出，也不累积误差。
+        max_steps = max(1, int(round(cfg.max_descent_m / step)))
+        steps_done = 0
+        descended = 0.0
+        last = TactileSample(0.0, 0.0, 0.0)
+
+        while True:
+            if self._is_cancelled():
+                return self._finish(DescentOutcome.CANCELLED, descended, last, "收到取消请求")
+
+            sample = self._read_tactile()
+            if sample is None:
+                return self._finish(
+                    DescentOutcome.TACTILE_STALE, descended, last,
+                    "读不到触觉数据（传感器未连接 / 话题未发布）",
+                )
+            last = sample
+            if sample.age_sec > cfg.tactile_stale_sec:
+                return self._finish(
+                    DescentOutcome.TACTILE_STALE, descended, last,
+                    f"触觉数据已过期 {sample.age_sec:.2f}s（> {cfg.tactile_stale_sec:.2f}s），"
+                    "停止下压以免在无反馈情况下压坏工件",
+                )
+
+            # 安全上限优先于接触判据：法向力超限说明已经在硬顶，立刻停。
+            if cfg.max_normal_delta > 0 and sample.normal >= cfg.max_normal_delta:
+                return self._finish(
+                    DescentOutcome.NORMAL_LIMIT, descended, last,
+                    f"法向力增量 {sample.normal:.1f} 达到安全上限 "
+                    f"{cfg.max_normal_delta:.1f}，停止下压",
+                )
+
+            # 接触判据用绝对值：切向力往哪个方向跳都算接触。
+            if abs(sample.tangential) >= cfg.contact_delta:
+                return self._finish(
+                    DescentOutcome.CONTACT, descended, last,
+                    f"检测到接触：切向增量 {sample.tangential:.1f} ≥ 阈值 "
+                    f"{cfg.contact_delta:.1f}（已下压 {descended * 1000:.2f}mm）",
+                )
+
+            if self._now() > deadline:
+                return self._finish(
+                    DescentOutcome.TIMEOUT, descended, last,
+                    f"超时 {cfg.effective_timeout_sec:.1f}s 仍未接触（已下压 "
+                    f"{descended * 1000:.2f}mm）",
+                )
+
+            if steps_done >= max_steps:
+                return self._finish(
+                    DescentOutcome.MAX_DESCENT, descended, last,
+                    f"已下压 {descended * 1000:.2f}mm 达到行程上限 "
+                    f"{cfg.max_descent_m * 1000:.2f}mm 仍未接触",
+                )
+
+            if not self._step_down(step):
+                return self._finish(
+                    DescentOutcome.MOTION_FAILED, descended, last,
+                    "单步下压未执行成功（JTC 拒绝 / 控制器未激活 / TF 缺失）",
+                )
+            steps_done += 1
+            descended = steps_done * step
+
+            if self._on_progress is not None:
+                self._on_progress(
+                    DescentProgress(
+                        fraction=_clamp01(descended / cfg.max_descent_m),
+                        descended_m=descended,
+                        tangential=sample.tangential,
+                        normal=sample.normal,
+                        status="approaching",
+                    )
+                )
+            self._sleep(interval)
+
+    def _finish(
+        self, outcome: DescentOutcome, descended: float, last: TactileSample, message: str
+    ) -> DescentResult:
+        if self._on_progress is not None:
+            self._on_progress(
+                DescentProgress(
+                    fraction=_clamp01(descended / self.cfg.max_descent_m),
+                    descended_m=descended,
+                    tangential=last.tangential,
+                    normal=last.normal,
+                    status=_STATUS_BY_OUTCOME.get(outcome, "error"),
+                )
+            )
+        return DescentResult(
+            outcome=outcome,
+            descended_m=descended,
+            tangential_delta=last.tangential,
+            normal_delta=last.normal,
+            message=message,
+        )
+
+
+@dataclass
+class DescentProgress:
+    """一次 feedback 快照。"""
+
+    fraction: float
+    descended_m: float
+    tangential: float
+    normal: float
+    status: str
+
+
+def _clamp01(value: float) -> float:
+    return max(0.0, min(1.0, float(value)))
+
+
+# ---------------------------------------------------------------------------
+# 第二层：ROS 包装
+# ---------------------------------------------------------------------------
+
+
+class CompliantDescentRunner:
+    """``arm_msgs/CompliantDescent`` 的 action server。
+
+    每步下压怎么落地
+    ----------------
+    1. 从 TF 读 ``link1..7`` 在基座系下的位姿 → 用当前位形算几何雅可比
+    2. ``dls_translate_hold_ori(J, [0, 0, −step])`` 求关节增量 ``dq``（保持姿态）
+    3. 把 ``q + dq`` 作为**单点** JTC 轨迹发给 ``rizon_arm_controller``，等它完成
+
+    为什么是一步一发而不是一条长轨迹：接触时刻的位置无法事先知道，必须边走边看
+    力反馈；一步一发让我们能在任意一步之后停下，不需要取消一条已经下发的轨迹。
+    """
+
+    #: 单步轨迹至少给这么长时间，否则 JTC 会因速度超限而拒绝。
+    MIN_STEP_DURATION_SEC = 0.15
+
+    def __init__(
+        self,
+        node: Node,
+        callback_group: Optional[CallbackGroup] = None,
+        *,
+        default_sn: str = "",
+    ) -> None:
+        """``default_sn`` 是 ``robot_sn`` 参数的默认值。
+
+        必须由调用方（驱动节点）传入它已经从 site.yaml 解析出的 SN —— 否则这里
+        的默认值会是空串，frame 名退化成 ``_base_link`` / ``_flange``，
+        TF 永远查不到，只有在真机跑起来报"读不到 TF"时才会发现。
+        """
+        self._node = node
+        self._cb = callback_group
+        self._log = node.get_logger()
+
+        node.declare_parameter("action_name", "compliant_descent")
+        node.declare_parameter("jtc_action_name", "rizon_arm_controller/follow_joint_trajectory")
+        node.declare_parameter("robot_sn", default_sn)
+        node.declare_parameter("base_frame", "")
+        node.declare_parameter("ee_frame", "")
+        node.declare_parameter("joint_prefix", "")
+        node.declare_parameter("tactile_topic_tmpl", "/tactile/{side}/status")
+
+        sn = str(node.get_parameter("robot_sn").value).strip()
+        self._prefix = str(node.get_parameter("joint_prefix").value).strip() or f"{sn}_"
+        self._base_frame = (
+            str(node.get_parameter("base_frame").value).strip() or f"{sn}_base_link"
+        )
+        self._ee_frame = str(node.get_parameter("ee_frame").value).strip() or f"{sn}_flange"
+        self._joint_names = [f"{self._prefix}joint{i}" for i in range(1, 8)]
+        self._link_frames = [f"{self._prefix}link{i}" for i in range(1, 8)]
+        self._tactile_tmpl = str(node.get_parameter("tactile_topic_tmpl").value)
+
+        self._lock = threading.Lock()
+        self._q: Optional[List[float]] = None
+        #: side -> (fshearx, fsheary, fnormal, 到达时间 monotonic)
+        self._tactile: dict[str, Tuple[float, float, float, float]] = {}
+        self._cancelled = threading.Event()
+
+        # TF
+        from tf2_ros import Buffer, TransformListener
+
+        self._tf_buffer = Buffer()
+        self._tf_listener = TransformListener(self._tf_buffer, node)
+
+        # 关节角：JTC 步进的输入
+        from sensor_msgs.msg import JointState
+
+        node.create_subscription(
+            JointState, "/joint_states", self._on_joint_state, 10, callback_group=callback_group
+        )
+
+        # 触觉：按下压时指定的一侧懒订阅（左右各建一个，成本很低）
+        self._tactile_subs = {}
+        for side in ("left", "right"):
+            topic = self._tactile_tmpl.format(side=side)
+            self._tactile_subs[side] = node.create_subscription(
+                TactileStatus, topic, self._make_tactile_cb(side), 10,
+                callback_group=callback_group,
+            )
+
+        self._jtc = ActionClient(
+            node, FollowJointTrajectory,
+            str(node.get_parameter("jtc_action_name").value),
+            callback_group=callback_group,
+        )
+
+        self._server = rclpy.action.ActionServer(
+            node,
+            CompliantDescent,
+            str(node.get_parameter("action_name").value),
+            execute_callback=self._execute,
+            cancel_callback=self._on_cancel,
+            callback_group=callback_group,
+        )
+
+        self._log.info(
+            f"compliant_descent 就绪: base={self._base_frame} "
+            f"jtc={self._jtc._action_name} 触觉模板={self._tactile_tmpl}"
+        )
+        if not sn:
+            self._log.warn(
+                "robot_sn 为空，TF frame 名可能不对（默认 f'{sn}_base_link'）。"
+                "用 parameters 传 robot_sn。"
+            )
+
+    # -- 订阅回调 ---------------------------------------------------------
+
+    def _on_joint_state(self, msg) -> None:
+        if not self._joint_names:
+            return
+        by_name = dict(zip(msg.name, msg.position))
+        try:
+            q = [float(by_name[n]) for n in self._joint_names]
+        except KeyError:
+            # 里面还混着夹爪等其它关节，缺我们这 7 个就先不用这帧
+            return
+        with self._lock:
+            self._q = q
+
+    def _make_tactile_cb(self, side: str):
+        def cb(msg: TactileStatus) -> None:
+            with self._lock:
+                self._tactile[side] = (
+                    float(msg.fshearx),
+                    float(msg.fsheary),
+                    float(msg.fnormal),
+                    time.monotonic(),
+                )
+
+        return cb
+
+    # -- action 回调 ------------------------------------------------------
+
+    def _on_cancel(self, _goal_handle) -> "rclpy.action.CancelResponse":
+        self._cancelled.set()
+        return rclpy.action.CancelResponse(return_value=True)
+
+    def _execute(self, goal_handle) -> CompliantDescent.Result:
+        goal = goal_handle.request
+        self._cancelled.clear()
+        cfg = DescentConfig.from_goal(goal)
+
+        result = CompliantDescent.Result()
+        side = (goal.side or "right").strip().lower()
+        if side not in ("left", "right"):
+            side = "right"
+
+        result.status = "approaching"
+        self._log.info(
+            f"柔顺下压开始: side={side} axis={cfg.contact_axis} "
+            f"阈值={cfg.contact_delta:.1f} 步长={cfg.descent_step_m * 1000:.2f}mm "
+            f"行程上限={cfg.max_descent_m * 1000:.1f}mm"
+        )
+
+        if not self._preflight(goal, cfg, side, result):
+            goal_handle.abort()
+            return result
+
+        # 记录下压前的 XY：每步都把它作为目标，避免斜向漂移。
+        start = self._lookup_ee()
+        if start is None:
+            result.status = "error"
+            result.message = f"TF 里读不到 {self._ee_frame}（base={self._base_frame}）"
+            goal_handle.abort()
+            return result
+        hold_xy = np.array(start[:3, 3], dtype=float)
+
+        controller = DescentController(
+            cfg,
+            read_tactile=lambda: self._sample(cfg, side),
+            step_down=lambda dz: self._step_down(dz, hold_xy, cfg),
+            is_cancelled=self._cancelled.is_set,
+            on_progress=lambda p: self._publish_feedback(goal_handle, p),
+        )
+        outcome = controller.run()
+
+        contact = self._lookup_ee()
+        if contact is not None:
+            result.contact_pose.header.frame_id = self._base_frame
+            result.contact_pose.header.stamp = self._node.get_clock().now().to_msg()
+            p = contact[:3, 3]
+            result.contact_pose.pose.position.x = float(p[0])
+            result.contact_pose.pose.position.y = float(p[1])
+            result.contact_pose.pose.position.z = float(p[2])
+            q = _R_to_quat_xyzw(contact[:3, :3])
+            result.contact_pose.pose.orientation.x = q[0]
+            result.contact_pose.pose.orientation.y = q[1]
+            result.contact_pose.pose.orientation.z = q[2]
+            result.contact_pose.pose.orientation.w = q[3]
+
+        result.success = outcome.success
+        result.message = outcome.message
+        result.descended_m = outcome.descended_m
+        result.contact_delta_observed = outcome.tangential_delta
+        result.normal_delta_observed = outcome.normal_delta
+        result.status = outcome.status
+
+        if outcome.success:
+            self._log.info(outcome.message)
+            goal_handle.succeed()
+        else:
+            self._log.warn(f"柔顺下压未成功（{outcome.outcome.value}）: {outcome.message}")
+            goal_handle.abort()
+        return result
+
+    def _preflight(self, goal, cfg: DescentConfig, side: str, result) -> bool:
+        """开工前的检查。任何一条不过就立刻放弃，不要带着问题往下压。"""
+        if cfg.contact_delta <= 0:
+            result.message = (
+                "contact_delta 必须 > 0。力阈值是相对静止基线的增量，"
+                "请填传感器原生单位下的经验值（先前用 /tactile 话题观察接触时的跳变量）。"
+            )
+            return False
+
+        if not self._jtc.wait_for_server(timeout_sec=5.0):
+            result.message = (
+                f"等不到 JTC 动作 {self._jtc._action_name}。"
+                "先启动机械臂：bash scripts/run_real.sh"
+            )
+            return False
+
+        if self._lookup_ee() is None:
+            result.message = (
+                f"TF 里读不到 {self._ee_frame}（base={self._base_frame}）。"
+                "检查 robot_sn 参数与机械臂 TF 是否在发布。"
+            )
+            return False
+
+        with self._lock:
+            q = list(self._q) if self._q is not None else None
+        if q is None or len(q) < 7:
+            result.message = "还没收到 /joint_states 里的 7 个关节角"
+            return False
+
+        sample = self._sample(cfg, side)
+        if sample is None:
+            result.message = (
+                f"触觉话题 {self._tactile_tmpl.format(side=side)} 没有数据。"
+                "先启动传感器：ros2 launch arm_application tactile.launch.py"
+            )
+            return False
+
+        # 触觉话题存在但切向恒为 0 是已知的坑：老版 tactile_node 只发 fnormal。
+        # 那种情况下切向判据永远不触发，流程会一路压到行程上限 —— 必须提前拦。
+        if sample.tangential == 0.0:
+            self._log.warn(
+                "触觉切向读数为 0。若传感器固件在静止时确实输出 0 则可忽略；"
+                "但如果下压过程中它始终不变，说明节点没有发布切向通道 —— "
+                "请检查 tactile_node（老版本把 fshearx/fsheary 写成了 0.0）。"
+            )
+        return True
+
+    # -- 触觉 / TF / 运动 -------------------------------------------------
+
+    def _sample(self, cfg: DescentConfig, side: str) -> Optional[TactileSample]:
+        """取一次触觉。返回 ``None`` 表示这一侧完全没有数据。
+
+        ``age_sec`` 用节点时钟与订阅回调的到达时间之差估计。这里用壁钟而非
+        ROS 时间：传感器节点的 stamp 可能是驱动时间戳（不是 ROS 时间），
+        直接相减会得到荒谬的年龄。
+        """
+        with self._lock:
+            entry = self._tactile.get(side)
+        if entry is None:
+            return None
+        shearx, sheary, fnormal, stamp = entry
+        return TactileSample(
+            tangential=select_tangential(shearx, sheary, cfg.contact_axis),
+            normal=fnormal,
+            age_sec=max(0.0, time.monotonic() - stamp),
+        )
+
+    def _lookup_ee(self) -> Optional[np.ndarray]:
+        return self._lookup(self._ee_frame)
+
+    def _lookup(self, frame: str) -> Optional[np.ndarray]:
+        try:
+            tf = self._tf_buffer.lookup_transform(
+                self._base_frame, frame, rclpy.time.Time()
+            )
+        except Exception:
+            return None
+        t = tf.transform.translation
+        r = tf.transform.rotation
+        T = np.eye(4)
+        T[:3, :3] = _quat_xyzw_to_R([r.x, r.y, r.z, r.w])
+        T[:3, 3] = [t.x, t.y, t.z]
+        return T
+
+    def _jacobian(self, ee_p: np.ndarray) -> Optional[np.ndarray]:
+        """用 TF 实测位形算几何雅可比。缺任一 link 的 TF 就没法算。"""
+        link_R: List[np.ndarray] = []
+        link_p: List[np.ndarray] = []
+        for frame in self._link_frames:
+            T = self._lookup(frame)
+            if T is None:
+                return None
+            link_R.append(T[:3, :3])
+            link_p.append(T[:3, 3])
+        return geometric_jacobian(link_R, link_p, ee_p, RIZON_LINK_AXES)
+
+    def _step_down(self, dz: float, hold_xy: np.ndarray, cfg: DescentConfig) -> bool:
+        """沿 −Z 走一步。返回是否成功执行。
+
+        XY 每步都重新对准 ``hold_xy``：下压过程中若因姿态微调带出 XY 漂移，
+        会在这一步被拉回来，避免"越压越歪"。
+        """
+        with self._lock:
+            q = np.array(self._q, dtype=float) if self._q is not None else None
+        if q is None or q.size < 7:
+            self._log.warn("步进失败：没有关节角")
+            return False
+
+        ee = self._lookup_ee()
+        if ee is None:
+            self._log.warn(f"步进失败：TF 读不到 {self._ee_frame}")
+            return False
+
+        J = self._jacobian(np.array(ee[:3, 3], dtype=float))
+        if J is None:
+            self._log.warn("步进失败：缺 link TF，无法算雅可比")
+            return False
+
+        dp = np.zeros(3)
+        p = np.array(ee[:3, 3], dtype=float)
+        dp[0] = hold_xy[0] - p[0]
+        dp[1] = hold_xy[1] - p[1]
+        dp[2] = -abs(dz)
+        # 单步内 XY 修正幅度限死，防止雅可比在某一位形下给出巨大的横向位移。
+        dp[0] = float(np.clip(dp[0], -abs(dz), abs(dz)))
+        dp[1] = float(np.clip(dp[1], -abs(dz), abs(dz)))
+
+        dq = dls_translate_hold_ori(J, dp, damping=0.08, max_dq=0.02)
+        target = q[:7] + dq[:7]
+
+        duration = max(self.MIN_STEP_DURATION_SEC, abs(dz) / max(cfg.descent_speed_mps, 1e-6))
+        traj = JointTrajectory()
+        traj.joint_names = list(self._joint_names)
+        pt = JointTrajectoryPoint()
+        pt.positions = [float(v) for v in target]
+        pt.velocities = [0.0] * 7
+        pt.time_from_start.sec = int(duration)
+        pt.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
+        traj.points = [pt]
+
+        return self._send_jtc(traj, timeout_sec=duration + 1.0)
+
+    def _send_jtc(self, traj: JointTrajectory, timeout_sec: float) -> bool:
+        """发一条轨迹并等同到完成。在 action 执行线程里跑，靠 executor 收应答。"""
+        goal = FollowJointTrajectory.Goal()
+        goal.trajectory = traj
+        handle = self._await(self._jtc.send_goal_async(goal), timeout_sec=4.0)
+        if handle is None or not bool(getattr(handle, "accepted", False)):
+            self._log.warn("JTC 未接受轨迹")
+            return False
+        future = handle.get_result_async()
+        if not self._await_done(future, timeout_sec=timeout_sec):
+            self._log.warn("JTC 结果超时")
+            return False
+        try:
+            inner = getattr(future.result(), "result", None)
+            code = getattr(inner, "error_code", 0) if inner is not None else 0
+            if code not in (0, None):
+                self._log.warn(
+                    f"JTC 执行失败 error_code={code} "
+                    f"{getattr(inner, 'error_string', '')}"
+                )
+                return False
+        except Exception:
+            pass
+        return True
+
+    @staticmethod
+    def _await(future, timeout_sec: float):
+        """等一个 action future 并把结果取出来（不 spin，靠其它 executor 线程）。"""
+        if not CompliantDescentRunner._await_done(future, timeout_sec):
+            return None
+        try:
+            return future.result()
+        except Exception:
+            return None
+
+    @staticmethod
+    def _await_done(future, timeout_sec: float) -> bool:
+        """等 future 完成。
+
+        不能用 ``rclpy.spin_until_future_complete`` —— 那需要 executor，而在
+        action 执行线程里再 spin 会自锁。节点的 MultiThreadedExecutor 会在其它
+        线程处理这套 action client 的回调，这里只需轮询完成标志。
+        """
+        deadline = time.monotonic() + max(0.05, timeout_sec)
+        while not future.done() and time.monotonic() < deadline:
+            time.sleep(0.01)
+        return future.done()
+
+    def _publish_feedback(self, goal_handle, p: DescentProgress) -> None:
+        fb = CompliantDescent.Feedback()
+        fb.progress = p.fraction
+        fb.descended_m = p.descended_m
+        fb.tangential_delta = p.tangential
+        fb.normal_delta = p.normal
+        fb.status = p.status
+        try:
+            goal_handle.publish_feedback(fb)
+        except Exception:
+            pass
+
+
+def _quat_xyzw_to_R(q: List[float]) -> np.ndarray:
+    x, y, z, w = [float(v) for v in q]
+    n = math.sqrt(x * x + y * y + z * z + w * w) + 1e-12
+    x, y, z, w = x / n, y / n, z / n, w / n
+    return np.array(
+        [
+            [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
+            [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
+            [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
+        ],
+        dtype=float,
+    )
+
+
+def _R_to_quat_xyzw(R: np.ndarray) -> List[float]:
+    """3x3 旋转矩阵转四元数 (x, y, z, w)。用 Shepperd 法，数值稳定。"""
+    m = np.asarray(R, dtype=float)
+    trace = m[0, 0] + m[1, 1] + m[2, 2]
+    if trace > 0:
+        s = math.sqrt(trace + 1.0) * 2.0
+        w = 0.25 * s
+        x = (m[2, 1] - m[1, 2]) / s
+        y = (m[0, 2] - m[2, 0]) / s
+        z = (m[1, 0] - m[0, 1]) / s
+    elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]:
+        s = math.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) * 2.0
+        w = (m[2, 1] - m[1, 2]) / s
+        x = 0.25 * s
+        y = (m[0, 1] + m[1, 0]) / s
+        z = (m[0, 2] + m[2, 0]) / s
+    elif m[1, 1] > m[2, 2]:
+        s = math.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) * 2.0
+        w = (m[0, 2] - m[2, 0]) / s
+        x = (m[0, 1] + m[1, 0]) / s
+        y = 0.25 * s
+        z = (m[1, 2] + m[2, 1]) / s
+    else:
+        s = math.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) * 2.0
+        w = (m[1, 0] - m[0, 1]) / s
+        x = (m[0, 2] + m[2, 0]) / s
+        y = (m[1, 2] + m[2, 1]) / s
+        z = 0.25 * s
+    return [x, y, z, w]

src/arm_driver_flexiv/arm_driver_flexiv/flexiv_arm_driver.py

@@ -0,0 +1,380 @@
+"""非夕 Flexiv 机械臂驱动适配器节点。
+
+它提供什么
+----------
+只提供 `devices/robots/flexiv/manifest.yaml` 声明、而现有节点没有的接口：
+
+========================  ==============================  ================================
+接口                     类型                            实现方式
+========================  ==============================  ================================
+``/home``                ``std_srvs/Trigger``            包装 ``/set_named_pose``
+``/enable``              ``std_srvs/SetBool``            经 RDK 桥使能 / 下使能
+``/clear_fault``         ``std_srvs/Trigger``            经 RDK 桥清故障并重新使能
+``/switch_tool``         ``arm_msgs/SwitchTool``         经 RDK 桥切换工具坐标系
+``/soft_drag``           ``std_srvs/SetBool``            经 RDK 桥开关软拖拽
+``/compliant_descent``   ``arm_msgs/CompliantDescent``   应用层力闭环柔顺下压
+========================  ==============================  ================================
+
+它不重复实现什么（这些已由现有节点提供，见本包 README）
+--------------------------------------------------------
+``get_arm_state`` → ``arm_control/arm_status_node``
+``follow_joint_trajectory`` → ``arm_control/real_trajectory_adapter``
+``set_named_pose`` → ``arm_control/named_pose_server``
+``servo_joint`` / ``servo_cartesian`` → ``arm_application/servo_node``
+
+重复实现它们只会制造两套需要同步维护的行为，这正是本次重构要消除的问题。
+
+RDK 访问策略
+------------
+本节点**不 import flexivrdk**（其 Python 绑定与 ROS 环境的 3.11 ABI 不兼容）。
+所有需要独占 RDK 的操作都委托给 ``scripts/rdk_drag_bridge.py`` 这个跑在
+``rdk_py310`` 下的长驻桥进程，并由 :class:`arm_driver_flexiv.rdk_client.RdkClient`
+管理。
+
+桥是**懒启动**的：节点启动时不连机器人。只有真的收到 ``/enable``、
+``/switch_tool`` 这类请求时才拉起桥。这样在没有机械臂的机器上（写文档、跑单测、
+CI）节点也能正常起来、接口也能被 ``ros2 service list`` 看到。
+"""
+
+from __future__ import annotations
+
+import threading
+from typing import Any, Dict, Optional
+
+import rclpy
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+
+from arm_msgs.action import CompliantDescent
+from arm_msgs.srv import SetNamedPose, SwitchTool
+from std_srvs.srv import SetBool, Trigger
+
+from .compliant_descent import CompliantDescentRunner
+from .rdk_client import RdkBridgeError, RdkClient
+from .site_config import SiteConfig
+
+
+class FlexivArmDriver(Node):
+    """非夕机械臂驱动适配器。"""
+
+    def __init__(self) -> None:
+        super().__init__("flexiv_arm_driver")
+
+        # --- 站点配置（作为参数默认值的来源）---
+        self._site = SiteConfig()
+
+        self.declare_parameter("sn", self._site.robot_sn())
+        self.declare_parameter("iface", self._site.net_iface())
+        self.declare_parameter("tool_name", self._site.tool_name())
+        self.declare_parameter("home_pose", self._site.home_pose())
+        self.declare_parameter("home_speed", self._site.home_speed())
+        self.declare_parameter("set_named_pose_service", "/set_named_pose")
+        self.declare_parameter("rdk_python", "")
+        self.declare_parameter("rdk_script", "")
+        self.declare_parameter("rdk_log", "/tmp/flexiv_arm_driver_rdk.log")
+        self.declare_parameter("rdk_reply_timeout", 10.0)
+        self.declare_parameter("rdk_connect_timeout", 25.0)
+        # 装配流程开始时就该把工具切好；默认 true 让 home 顺带保证工具正确。
+        self.declare_parameter("switch_tool_on_home", True)
+
+        self._sn = str(self.get_parameter("sn").value)
+        self._iface = str(self.get_parameter("iface").value)
+        self._tool_name = str(self.get_parameter("tool_name").value)
+
+        # ReentrantCallbackGroup + MultiThreadedExecutor：本节点会在服务回调里
+        # 同步等待另一个服务（/set_named_pose）的应答，非重入的互斥组会自锁。
+        self._cb = ReentrantCallbackGroup()
+
+        self._rdk: Optional[RdkClient] = None
+        self._rdk_lock = threading.Lock()
+
+        # --- 服务：RDK 相关 ---
+        self.create_service(SetBool, "enable", self._on_enable, callback_group=self._cb)
+        self.create_service(Trigger, "clear_fault", self._on_clear_fault,
+                            callback_group=self._cb)
+        self.create_service(SwitchTool, "switch_tool", self._on_switch_tool,
+                            callback_group=self._cb)
+        self.create_service(SetBool, "soft_drag", self._on_soft_drag,
+                            callback_group=self._cb)
+
+        # --- 服务：home（包装现有 named_pose_server）---
+        self.create_service(Trigger, "home", self._on_home, callback_group=self._cb)
+        self._named_pose_client = self.create_client(
+            SetNamedPose,
+            str(self.get_parameter("set_named_pose_service").value),
+            callback_group=self._cb,
+        )
+
+        # --- action：柔顺下压 ---
+        # 把已解析的 SN 传进去，否则 runner 的 robot_sn 默认是空串，
+        # TF frame 会退化成 _base_link / _flange。
+        self._descent = CompliantDescentRunner(
+            self, callback_group=self._cb, default_sn=self._sn
+        )
+
+        self.get_logger().info(
+            f"flexiv_arm_driver 就绪: sn={self._sn} iface={self._iface or '-'} "
+            f"tool={self._tool_name or '(未设置)'}"
+        )
+        if not self._sn:
+            self.get_logger().warn(
+                "未取到机械臂 SN（site.yaml 的 robot.sn 为空）。"
+                "/enable、/switch_tool 等 RDK 相关服务会失败。"
+            )
+        if not self._tool_name:
+            self.get_logger().warn(
+                "site.yaml 的 robot.tool_name 为空，回位前不会切换工具坐标系。"
+                "通止规装配要求工具系全程垂直向下锁定，请填上 Studio 里已保存的工具名"
+                "（bash scripts/rdk_switch_tool.sh --list 可列出可选项）。"
+            )
+
+    # ------------------------------------------------------------------
+    # RDK 桥管理
+    # ------------------------------------------------------------------
+
+    def _ensure_rdk(self) -> RdkClient:
+        """返回已连接的 RDK 客户端，必要时懒启动桥。
+
+        Raises
+        ------
+        RdkBridgeError
+            无法启动桥或无法连接机器人。消息里带修复建议。
+        """
+        with self._rdk_lock:
+            if self._rdk is not None and self._rdk.is_running:
+                return self._rdk
+            if not self._sn:
+                raise RdkBridgeError(
+                    "没有机械臂 SN，无法连接 RDK",
+                    "在 config/site.yaml 里设置 robot.sn（如 Rizon4-063989），"
+                    "重启本节点",
+                )
+            client = RdkClient(
+                sn=self._sn,
+                iface=self._iface,
+                python=str(self.get_parameter("rdk_python").value) or None,
+                script=(
+                    _nonempty(self.get_parameter("rdk_script").value) or None
+                ),
+                log_path=str(self.get_parameter("rdk_log").value),
+                reply_timeout=float(self.get_parameter("rdk_reply_timeout").value),
+                connect_timeout=float(self.get_parameter("rdk_connect_timeout").value),
+            )
+            client.start(connect=True)
+            self._rdk = client
+            self.get_logger().info(f"RDK 桥已连接（{self._sn}）")
+            return client
+
+    def _rdk_error_reply(self, what: str, exc: Exception) -> str:
+        """把 RDK 异常转成给服务调用方看的一行说明，并记日志。"""
+        message = f"{what}失败: {exc}"
+        self.get_logger().error(message)
+        return message
+
+    # ------------------------------------------------------------------
+    # 服务回调
+    # ------------------------------------------------------------------
+
+    def _on_enable(self, request: SetBool.Request, response: SetBool.Response):
+        if not request.data:
+            response.success = False
+            response.message = (
+                "本驱动不支持下使能：非夕 RDK 没有安全的『伺服断开』状态，"
+                "下使能会让臂自由下落。要停止运动请用 /set_named_pose 回安全位，"
+                "或用示教器急停。"
+            )
+            self.get_logger().warn(response.message)
+            return response
+        try:
+            reply = self._ensure_rdk().enable()
+        except Exception as exc:  # noqa: BLE001
+            response.success = False
+            response.message = self._rdk_error_reply("使能", exc)
+            return response
+
+        response.success = True
+        response.message = (
+            f"已使能（operational={reply.get('operational')}, "
+            f"mode={reply.get('mode', '?')}）"
+        )
+        self.get_logger().info(response.message)
+        return response
+
+    def _on_clear_fault(self, _request: Trigger.Request, response: Trigger.Response):
+        try:
+            reply = self._ensure_rdk().clear_fault()
+        except Exception as exc:  # noqa: BLE001
+            response.success = False
+            response.message = self._rdk_error_reply("清故障", exc)
+            return response
+
+        response.success = True
+        response.message = (
+            f"故障已清除（fault={reply.get('fault')}, "
+            f"operational={reply.get('operational')}）"
+        )
+        self.get_logger().info(response.message)
+        return response
+
+    def _on_switch_tool(self, request: SwitchTool.Request, response: SwitchTool.Response):
+        name = (request.tool_name or "").strip()
+        sn = (request.sn or "").strip()
+        if sn and sn != self._sn:
+            response.success = False
+            response.message = (
+                f"本节点只服务 sn={self._sn}，请求里给的是 {sn}。"
+                "多臂场景请为每台臂各起一个驱动节点。"
+            )
+            return response
+
+        try:
+            reply = self._ensure_rdk().switch_tool(name)
+        except Exception as exc:  # noqa: BLE001
+            response.success = False
+            response.message = self._rdk_error_reply("切换工具", exc)
+            return response
+
+        response.current_tool = str(reply.get("current", ""))
+        response.tool_mass_kg = float(reply.get("mass_kg", 0.0) or 0.0)
+        com = list(reply.get("com_m", []) or [])
+        response.tool_com_m = [float(v) for v in (com + [0.0, 0.0, 0.0])[:3]]
+        response.success = True
+
+        if not name:
+            response.message = f"当前工具: {response.current_tool or '(空)'}"
+        elif reply.get("already"):
+            response.message = f"已经在工具 [{response.current_tool}] 上，无需切换"
+        else:
+            response.message = f"已切换到工具 [{response.current_tool}]"
+
+        self.get_logger().info(response.message)
+        return response
+
+    def _on_soft_drag(self, request: SetBool.Request, response: SetBool.Response):
+        try:
+            client = self._ensure_rdk()
+            if request.data:
+                reply = client.drag_on()
+                response.message = (
+                    f"软拖拽已开启（mode={reply.get('mode', 'soft')}）。"
+                    "现在可以手动推动手臂。"
+                )
+            else:
+                client.drag_off()
+                response.message = "软拖拽已关闭，已切回 IDLE。"
+        except Exception as exc:  # noqa: BLE001
+            response.success = False
+            response.message = self._rdk_error_reply("软拖拽", exc)
+            return response
+
+        response.success = True
+        self.get_logger().info(response.message)
+        return response
+
+    def _on_home(self, _request: Trigger.Request, response: Trigger.Response):
+        """回初始位。
+
+        顺序很重要：先确保工具坐标系正确，再回位。否则回位后的姿态是按错误的
+        TCP 算的，后续装配会从错的位置开始。
+        """
+        tool_name = (self._tool_name or "").strip()
+        if tool_name and bool(self.get_parameter("switch_tool_on_home").value):
+            try:
+                reply = self._ensure_rdk().switch_tool(tool_name)
+                self.get_logger().info(
+                    f"回位前工具确认: {reply.get('current') or tool_name}"
+                )
+            except Exception as exc:  # noqa: BLE001
+                # 工具切不过去就让调用方知道 —— 静默继续会让装配在错误 TCP 下跑。
+                response.success = False
+                response.message = (
+                    f"回位前的工具切换失败，已中止回位以免在错误工具系下运动: {exc}"
+                )
+                self.get_logger().error(response.message)
+                return response
+
+        if not self._named_pose_client.wait_for_service(timeout_sec=5.0):
+            response.success = False
+            response.message = (
+                f"等不到服务 {self._named_pose_client.srv_name}。"
+                "先启动机械臂控制：bash scripts/run_real.sh（或 mock 控制栈）"
+            )
+            self.get_logger().error(response.message)
+            return response
+
+        pose = str(self.get_parameter("home_pose").value)
+        speed = float(self.get_parameter("home_speed").value)
+        req = SetNamedPose.Request()
+        req.pose_name = pose
+        req.velocity_scale = speed
+        req.acceleration_scale = 0.0
+
+        future = self._named_pose_client.call_async(req)
+        # 本节点的回调跑在 ReentrantCallbackGroup 上，所以这里的等待不会自锁。
+        if not _wait_future(future, timeout_sec=60.0):
+            response.success = False
+            response.message = f"回初始位超时（pose={pose}, speed={speed}）"
+            self.get_logger().error(response.message)
+            return response
+
+        try:
+            result = future.result()
+        except Exception as exc:  # noqa: BLE001
+            response.success = False
+            response.message = f"回初始位调用异常: {exc}"
+            self.get_logger().error(response.message)
+            return response
+
+        response.success = bool(result.success)
+        response.message = (
+            f"已回初始位 {pose}: {result.message}"
+            if result.success
+            else f"回初始位失败: {result.message}"
+        )
+        self.get_logger().info(response.message)
+        return response
+
+
+def _nonempty(value: Any) -> str:
+    """把 ROS 参数值转成去掉首尾空白的字符串（空串 -> ""）。"""
+    return str(value).strip() if value is not None else ""
+
+
+def _wait_future(future: Any, timeout_sec: float) -> bool:
+    """在回调线程里等一个 future 完成。
+
+    不能用 ``rclpy.spin_until_future_complete`` —— 那需要拿到 executor，而在服务
+    回调里再 spin 会自锁。这里用事件等待，配合 ReentrantCallbackGroup 让应答能在
+    另一个线程被处理。
+    """
+    import time
+
+    deadline = time.monotonic() + timeout_sec
+    while not future.done() and time.monotonic() < deadline:
+        time.sleep(0.01)
+    return future.done()
+
+
+def main(argv: Optional[list[str]] = None) -> int:
+    rclpy.init(args=argv)
+    node = FlexivArmDriver()
+    executor = MultiThreadedExecutor(num_threads=4)
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        with node._rdk_lock:  # noqa: SLF001 - 关闭时清理子进程
+            if node._rdk is not None:
+                node._rdk.stop()
+                node._rdk = None
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/arm_driver_flexiv/arm_driver_flexiv/rdk_client.py

@@ -0,0 +1,385 @@
+"""RDK 桥客户端：管理 `rdk_drag_bridge.py` 长驻子进程。
+
+为什么需要这一层
+----------------
+`flexivrdk` 与 ROS 环境的 Python 3.11 ABI 不兼容，且 RDK 对同一台机器人是独占
+访问。所以驱动节点不直接连 RDK，而是拉起一个跑在 `rdk_py310` 下的长驻桥进程，
+用 JSON 行协议与它通信。本模块把这套子进程 + 协议交互封装成一个对象。
+
+设计取舍
+--------
+* **同步请求**：每条命令"发一行、等一行"。RDK 操作本来就是串行的，异步化只会
+  引入竞态。用一个锁保证多线程调用安全。
+* **超时必抛**：桥进程卡住时不能让调用方无限等。超时抛
+  :class:`RdkBridgeError`，消息里带上原始命令，便于定位。
+* **可以只启动不连接**：没有真机时（CI / 纯接口测试）用
+  ``start(connect=False)``，接口照样能起来，只是调用 RDK 的命令会失败。
+"""
+
+from __future__ import annotations
+
+import json
+import os
+import select
+import subprocess
+import threading
+import time
+from pathlib import Path
+from typing import Any, Dict, List, Optional
+
+#: 桥进程必须用这个解释器 —— RDK 绑定只在这里有。
+DEFAULT_RDK_PYTHON = "/home/cc/rdk_py310/bin/python"
+
+#: 桥脚本位置（相对工作区根）。
+BRIDGE_SCRIPT_REL = "scripts/rdk_drag_bridge.py"
+
+#: 未指定时使用的默认日志文件。
+DEFAULT_LOG = "/tmp/flexiv_arm_driver_rdk.log"
+
+
+class RdkBridgeError(RuntimeError):
+    """桥进程不可用、命令失败或超时。
+
+    Attributes
+    ----------
+    hint:
+        给操作员的修复建议（文件路径、检查项）。可能为空。
+    """
+
+    def __init__(self, message: str, hint: str = "") -> None:
+        super().__init__(message)
+        self.hint = hint
+
+    def __str__(self) -> str:
+        if self.hint:
+            return f"{super().__str__()}\n  怎么修: {self.hint}"
+        return super().__str__()
+
+
+def default_rdk_python() -> str:
+    """桥进程解释器路径，可用 ``RDK_PYTHON`` 覆盖。"""
+    return os.environ.get("RDK_PYTHON", "").strip() or DEFAULT_RDK_PYTHON
+
+
+def default_bridge_script() -> Path:
+    """桥脚本路径。可用 ``SITE_ROS2_ROOT`` / ``ROS2_WS`` 指向别的工作区。"""
+    root = (
+        os.environ.get("SITE_ROS2_ROOT", "").strip()
+        or os.environ.get("ROS2_WS", "").strip()
+    )
+    base = Path(root).expanduser() if root else Path.home() / "ros2"
+    return base / BRIDGE_SCRIPT_REL
+
+
+class RdkClient:
+    """与 RDK 桥子进程的一条长连接。
+
+    典型用法::
+
+        client = RdkClient(sn="Rizon4-063989", iface="enp4s0")
+        client.start()                       # 拉起桥并 connect
+        client.switch_tool("tool1")          # 切到 tool1
+        client.stop()
+    """
+
+    def __init__(
+        self,
+        sn: str,
+        iface: str = "",
+        *,
+        python: Optional[str] = None,
+        script: Optional[Path] = None,
+        log_path: Optional[str] = None,
+        reply_timeout: float = 10.0,
+        connect_timeout: float = 25.0,
+    ) -> None:
+        self.sn = sn
+        self.iface = iface
+        self.python = python or default_rdk_python()
+        self.script = Path(script) if script else default_bridge_script()
+        self.log_path = log_path or DEFAULT_LOG
+        self.reply_timeout = reply_timeout
+        self.connect_timeout = connect_timeout
+
+        self._proc: Optional[subprocess.Popen] = None
+        self._lock = threading.Lock()
+        self._log_file = None
+
+    # -- 生命周期 ---------------------------------------------------------
+
+    @property
+    def is_running(self) -> bool:
+        return self._proc is not None and self._proc.poll() is None
+
+    def start(self, *, connect: bool = True) -> None:
+        """拉起桥进程。``connect=True`` 时同时连接机器人。
+
+        Raises
+        ------
+        RdkBridgeError
+            解释器或脚本不存在，或 ``connect=True`` 而连接失败。
+        """
+        if self.is_running:
+            return
+        if not Path(self.python).is_file():
+            raise RdkBridgeError(
+                f"找不到 RDK 解释器: {self.python}",
+                f"设置环境变量 RDK_PYTHON 指向装有 flexivrdk 的解释器；"
+                f"默认是 {DEFAULT_RDK_PYTHON}",
+            )
+        if not self.script.is_file():
+            raise RdkBridgeError(
+                f"找不到 RDK 桥脚本: {self.script}",
+                "确认工作区完整（scripts/rdk_drag_bridge.py 存在），"
+                "或用 SITE_ROS2_ROOT 指向正确的工作区",
+            )
+
+        Path(self.log_path).parent.mkdir(parents=True, exist_ok=True)
+        self._log_file = open(self.log_path, "a", encoding="utf-8", buffering=1)
+        self._log_file.write(
+            f"\n=== {time.strftime('%Y-%m-%d %H:%M:%S')} start "
+            f"sn={self.sn} iface={self.iface or '-'} ===\n"
+        )
+
+        self._proc = subprocess.Popen(
+            [self.python, str(self.script)],
+            stdin=subprocess.PIPE,
+            stdout=subprocess.PIPE,
+            stderr=self._log_file,
+            # 二进制 + 无缓冲：配合 _read_reply 里的 os.read 自己切行。
+            # 用 text=True 的话 readline() 会为了等换行符而阻塞，
+            # select 的"可读"并不等于"有完整一行"，超时就会失效。
+            bufsize=0,
+        )
+
+        # 先 ping 确认桥活着（能捕获"解释器起不来/语法错"这类问题）。
+        self.request("ping", timeout=10.0)
+
+        if connect:
+            payload: Dict[str, Any] = {"sn": self.sn}
+            if self.iface:
+                payload["iface"] = self.iface
+            reply = self.request("connect", timeout=self.connect_timeout, **payload)
+            if not reply.get("ok"):
+                error = reply.get("error", "未知原因")
+                self.stop()
+                raise RdkBridgeError(
+                    f"连接机械臂 {self.sn} 失败: {error}",
+                    f"检查网线 / `network.prefer_iface`、示教器是否 AUTO(Remote)、"
+                    f"是否已松开急停。桥日志: {self.log_path}",
+                )
+
+    def stop(self, timeout: float = 5.0) -> None:
+        """请求桥退出并回收进程。幂等。"""
+        proc = self._proc
+        if proc is None:
+            return
+        try:
+            if proc.poll() is None:
+                self._write({"cmd": "quit"})
+                try:
+                    self._read_reply(timeout)
+                except Exception:
+                    pass
+        except Exception:
+            pass
+
+        try:
+            proc.wait(timeout=timeout)
+        except subprocess.TimeoutExpired:
+            proc.kill()
+            try:
+                proc.wait(timeout=2.0)
+            except subprocess.TimeoutExpired:
+                pass
+
+        for stream in (proc.stdin, proc.stdout):
+            try:
+                if stream is not None:
+                    stream.close()
+            except Exception:
+                pass
+        if self._log_file is not None:
+            try:
+                self._log_file.close()
+            except Exception:
+                pass
+            self._log_file = None
+        self._proc = None
+
+    def __enter__(self) -> "RdkClient":
+        self.start()
+        return self
+
+    def __exit__(self, *_exc: Any) -> None:
+        self.stop()
+
+    # -- 协议 -------------------------------------------------------------
+
+    def request(
+        self, cmd: str, timeout: Optional[float] = None, **kwargs: Any
+    ) -> Dict[str, Any]:
+        """发一条命令并等应答。
+
+        Raises
+        ------
+        RdkBridgeError
+            桥未运行、进程已退出、应答超时，或应答不是合法 JSON。
+        """
+        with self._lock:
+            if not self.is_running:
+                raise RdkBridgeError(
+                    "RDK 桥未运行",
+                    f"先调用 RdkClient.start()；若刚崩溃，看日志 {self.log_path}",
+                )
+            payload = {"cmd": cmd, **kwargs}
+            self._write(payload)
+            return self._read_reply(
+                timeout if timeout is not None else self.reply_timeout
+            )
+
+    def request_ok(
+        self, cmd: str, timeout: Optional[float] = None, **kwargs: Any
+    ) -> Dict[str, Any]:
+        """同 :meth:`request`，但 ``ok`` 为假时直接抛错。"""
+        reply = self.request(cmd, timeout=timeout, **kwargs)
+        if not reply.get("ok"):
+            raise RdkBridgeError(
+                f"命令 {cmd} 失败: {reply.get('error', '未提供原因')}",
+                f"桥日志: {self.log_path}",
+            )
+        return reply
+
+    # -- 便捷包装 ---------------------------------------------------------
+
+    def enable(self) -> Dict[str, Any]:
+        """使能 + 清故障（幂等）。"""
+        return self.request_ok("enable")
+
+    def clear_fault(self) -> Dict[str, Any]:
+        """清故障并使能。"""
+        return self.request_ok("clear_fault")
+
+    def switch_tool(self, name: str = "", timeout: float = 20.0) -> Dict[str, Any]:
+        """切换工具。``name`` 为空时只查询当前状态。"""
+        return self.request_ok("switch_tool", timeout=timeout, name=name)
+
+    def status(self) -> Dict[str, Any]:
+        """查询 operational / fault / mode / 当前工具。"""
+        return self.request_ok("status")
+
+    def drag_on(self, mode: str = "soft") -> Dict[str, Any]:
+        """开启软拖拽（手眼走点用）。"""
+        return self.request_ok("drag_on", timeout=15.0, mode=mode)
+
+    def drag_off(self) -> Dict[str, Any]:
+        """关闭软拖拽并切回 IDLE。"""
+        return self.request_ok("drag_off", timeout=15.0)
+
+    def joints(self) -> List[float]:
+        """读关节角（拖拽期间也能读，走桥内缓存）。"""
+        reply = self.request_ok("joints")
+        return [float(v) for v in (reply.get("q") or [])]
+
+    # -- 内部 -------------------------------------------------------------
+
+    def _write(self, payload: Dict[str, Any]) -> None:
+        proc = self._proc
+        if proc is None or proc.stdin is None:
+            raise RdkBridgeError("RDK 桥的 stdin 不可用")
+        line = json.dumps(payload, ensure_ascii=False)
+        try:
+            proc.stdin.write((line + "\n").encode("utf-8"))
+            proc.stdin.flush()
+        except (BrokenPipeError, OSError) as exc:
+            raise RdkBridgeError(
+                f"写入 RDK 桥失败（进程可能已退出）: {exc}",
+                f"查看桥日志 {self.log_path}",
+            ) from exc
+
+    def _read_reply(self, timeout: float) -> Dict[str, Any]:
+        """带超时读一行 JSON。
+
+        为什么不用 ``readline()``
+        -------------------------
+        ``select`` 说"可读"只代表**有字节**，不代表有**完整一行**。桥上如果
+        只写了半行（原生库中途崩、或者 JSON 被截断），``readline()`` 会一直
+        阻塞等那个永远不来的换行符 —— 超时机制完全失效，整个节点挂死。
+        所以这里改用 ``os.read`` 按块读，行切分自己做：读多少算多少，
+        没有完整行就回到 ``select`` 继续等，直到超时。
+
+        非 JSON 行（RDK 原生日志混进 stdout）直接跳过，不判协议损坏。
+        """
+        proc = self._proc
+        if proc is None or proc.stdout is None:
+            raise RdkBridgeError("RDK 桥的 stdout 不可用")
+
+        deadline = time.monotonic() + max(0.1, timeout)
+        buffered = b""
+        while True:
+            # 缓冲里可能已经有完整行（一次 read 常常带回多行），先消化掉。
+            while b"\n" in buffered:
+                raw, buffered = buffered.split(b"\n", 1)
+                reply = self._parse_line(raw)
+                if reply is not None:
+                    return reply
+
+            remaining = deadline - time.monotonic()
+            if remaining <= 0:
+                raise RdkBridgeError(
+                    f"等待 RDK 桥应答超时（{timeout:.1f}s）",
+                    f"机器人可能未响应；查看桥日志 {self.log_path}",
+                )
+            ready, _, _ = select.select([proc.stdout], [], [], min(remaining, 0.5))
+            if not ready:
+                if proc.poll() is not None:
+                    raise RdkBridgeError(
+                        f"RDK 桥进程已退出（returncode={proc.returncode}）",
+                        f"查看桥日志 {self.log_path}",
+                    )
+                continue
+
+            try:
+                chunk = os.read(proc.stdout.fileno(), 65536)
+            except (BlockingIOError, InterruptedError):
+                continue
+            except OSError as exc:
+                raise RdkBridgeError(
+                    f"读取 RDK 桥失败: {exc}",
+                    f"查看桥日志 {self.log_path}",
+                ) from exc
+            if not chunk:
+                # EOF：进程走了，或者 stdout 被关了。
+                if proc.poll() is not None:
+                    raise RdkBridgeError(
+                        f"RDK 桥进程已退出（returncode={proc.returncode}）",
+                        f"查看桥日志 {self.log_path}",
+                    )
+                continue
+            buffered += chunk
+
+    def _parse_line(self, raw: bytes) -> Optional[Dict[str, Any]]:
+        """把一行的原始字节变成应答 dict；该行不是应答时返回 ``None``。
+
+        返回 ``None`` 的两种情况：空行、以及不带 ``{`` 的原生日志行。
+        后者要跳过而不是报错 —— 桥脚本已经把 ``sys.stdout`` 换成 stderr 来
+        隔离 RDK 日志，但原生库可能直接写 fd 1 绕过 Python 层。
+        """
+        text = raw.decode("utf-8", errors="replace").strip()
+        if not text:
+            return None
+        if "{" not in text:
+            return None
+        try:
+            reply = json.loads(text)
+        except json.JSONDecodeError as exc:
+            raise RdkBridgeError(
+                f"RDK 桥返回了非 JSON 内容: {text[:200]!r}",
+                f"桥日志 {self.log_path}（RDK 自身的日志混进 stdout 通常是原因）",
+            ) from exc
+        if not isinstance(reply, dict):
+            raise RdkBridgeError(
+                f"RDK 桥的应答不是 JSON 对象: {reply!r}",
+                f"查看桥日志 {self.log_path}",
+            )
+        return reply

src/arm_driver_flexiv/arm_driver_flexiv/site_config.py

@@ -0,0 +1,97 @@
+"""从 ``config/site.yaml`` 读取驱动需要的默认值。
+
+为什么不让 launch 注入全部参数
+------------------------------
+现有节点的做法是 launch 文件先用 ``export_site_env.py`` 读 site.yaml，再把值塞
+成 ROS 参数。那条路径的问题是有两套加载逻辑（``site_io.py`` 和
+``export_site_env.py``），且 ``ros2 run`` 直接起节点时参数全是空的。
+
+这里改为：节点自己用 ``arm_interfaces`` 的加载器读 site.yaml 作为**参数默认
+值**，launch 仍然可以覆盖。于是
+``ros2 run arm_driver_flexiv flexiv_arm_driver`` 直接起也能工作，
+既没有重复的加载器，也保留了 ROS 的参数覆盖惯例。
+
+多臂支持
+--------
+``robot.sn`` 选定当前服务的臂，臂专属的值（IP / 网卡）去 ``robot.robots[]``
+里按 SN 找。找不到时回退到全局段 —— 这与现有节点的行为一致。
+"""
+
+from __future__ import annotations
+
+from typing import Any, Dict, Optional
+
+from arm_interfaces import load_site
+
+
+class SiteConfig:
+    """site.yaml 的一个只读视图，只暴露驱动需要的键。"""
+
+    def __init__(self, data: Optional[Dict[str, Any]] = None) -> None:
+        if data is None:
+            try:
+                data = load_site()
+            except Exception:
+                # 契约层读不了配置不该让驱动起不来 —— 参数会有默认值，
+                # 真正需要配置的操作会在调用时报出具体缺什么。
+                data = {}
+        self._data: Dict[str, Any] = data if isinstance(data, dict) else {}
+
+    # -- 机械臂 -----------------------------------------------------------
+
+    def robot_sn(self) -> str:
+        """当前服务的机械臂 SN（``robot.sn``）。"""
+        return str(self._robot().get("sn", "") or "").strip()
+
+    def tool_name(self) -> str:
+        """回位前要切换到的工具名（``robot.tool_name``）。空=不切换。"""
+        return str(self._robot().get("tool_name", "") or "").strip()
+
+    def robot_entry(self, sn: str = "") -> Dict[str, Any]:
+        """取 ``robot.robots[]`` 中匹配 SN 的条目；找不到返回空 dict。"""
+        target = (sn or self.robot_sn()).strip()
+        for entry in self._robot().get("robots") or []:
+            if isinstance(entry, dict) and str(entry.get("sn", "")).strip() == target:
+                return entry
+        return {}
+
+    def net_iface(self) -> str:
+        """本臂的网卡名：优先 ``robot.robots[].iface``，回退 ``network.prefer_iface``。"""
+        entry = self.robot_entry()
+        iface = str(entry.get("iface", "") or "").strip()
+        if iface:
+            return iface
+        return str((self._data.get("network") or {}).get("prefer_iface", "") or "").strip()
+
+    def robot_ip(self) -> str:
+        """本臂的控制器 IP（``robot.robots[].ip``）。"""
+        return str(self.robot_entry().get("ip", "") or "").strip()
+
+    # -- 回位 -------------------------------------------------------------
+
+    def home_pose(self) -> str:
+        """回位用的命名位姿（``go_home.pose``，回退 ``servo.home_pose_name``）。"""
+        pose = str((self._data.get("go_home") or {}).get("pose", "") or "").strip()
+        if pose:
+            return pose
+        return str(self._servo_base().get("home_pose_name", "") or "").strip() or "home"
+
+    def home_speed(self) -> float:
+        """回位速度比例（``go_home.speed``，回退 ``servo.home_pose_speed``）。"""
+        value = (self._data.get("go_home") or {}).get("speed")
+        if value is None:
+            value = self._servo_base().get("home_pose_speed")
+        try:
+            return float(value)
+        except (TypeError, ValueError):
+            return 0.25
+
+    # -- 内部 -------------------------------------------------------------
+
+    def _robot(self) -> Dict[str, Any]:
+        robot = self._data.get("robot")
+        return robot if isinstance(robot, dict) else {}
+
+    def _servo_base(self) -> Dict[str, Any]:
+        servo = self._data.get("servo")
+        return servo if isinstance(servo, dict) else {}

src/arm_driver_flexiv/package.xml

@@ -0,0 +1,34 @@
+<?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_driver_flexiv</name>
+  <version>0.1.0</version>
+  <description>
+    非夕 Flexiv 机械臂驱动适配器。补齐 manifest 声明但现有节点没提供的能力：
+    home / enable / clear_fault / switch_tool / soft_drag / compliant_descent。
+    已经存在的能力（get_arm_state / follow_joint_trajectory / set_named_pose /
+    视觉伺服）由 arm_control 与 arm_application 的现有节点提供，本包不重复实现。
+    所有需要独占 RDK 的操作都经由 rdk_py310 下的长驻桥进程，本包自身不 import
+    flexivrdk（ABI 不兼容）。
+  </description>
+  <maintainer email="dev@example.com">arm_dev</maintainer>
+  <license>Apache-2.0</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>arm_msgs</exec_depend>
+  <exec_depend>arm_interfaces</exec_depend>
+  <exec_depend>arm_kinematics</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>trajectory_msgs</exec_depend>
+  <exec_depend>control_msgs</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/arm_driver_flexiv/setup.cfg

@@ -0,0 +1,10 @@
+[develop]
+script_dir=$base/lib/arm_driver_flexiv
+[install]
+install_scripts=$base/lib/arm_driver_flexiv
+
+[tool:pytest]
+# ROS 2 Humble 的两个 launch-testing pytest 插件用了旧版 hook 签名，
+# 当前 pytest 会在收集阶段直接报错。本包只跑纯 Python 单测，禁掉它们。
+addopts = -p no:launch_testing -p no:launch_ros
+testpaths = test

src/arm_driver_flexiv/setup.py

@@ -0,0 +1,26 @@
+from setuptools import find_packages, setup
+
+package_name = 'arm_driver_flexiv'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools', 'PyYAML'],
+    zip_safe=True,
+    maintainer='arm_dev',
+    maintainer_email='dev@example.com',
+    description='非夕 Flexiv 机械臂驱动适配器',
+    license='Apache-2.0',
+    entry_points={
+        'console_scripts': [
+            # 主驱动节点：home / enable / clear_fault / switch_tool / soft_drag
+            # 以及 compliant_descent action。
+            'flexiv_arm_driver = arm_driver_flexiv.flexiv_arm_driver:main',
+        ],
+    },
+)

src/arm_driver_flexiv/test/test_descent_control.py

@@ -0,0 +1,438 @@
+"""``DescentController`` 的纯逻辑单测。
+
+下压逻辑里最容易出错、也最危险的部分全在这里：接触判据的优先级、安全上限、
+触觉断流时的行为、超时与行程上限。这些都能用注入的假回调完整覆盖，
+不需要机械臂、JTC、TF。
+
+假时钟很关键：超时判定如果真用 ``time.sleep`` 测，一套测试要跑几十秒；
+换成手动推进的假时钟后毫秒级完成，且不依赖机器快慢。
+"""
+
+from __future__ import annotations
+
+from dataclasses import replace
+from typing import List, Optional
+
+import numpy as np
+import pytest
+
+from arm_driver_flexiv.compliant_descent import (
+    DescentConfig,
+    DescentController,
+    DescentOutcome,
+    DescentProgress,
+    TactileSample,
+    _R_to_quat_xyzw,
+    _quat_xyzw_to_R,
+    select_tangential,
+)
+
+import numpy as np
+
+
+# ---------------------------------------------------------------------------
+# 测试替身
+# ---------------------------------------------------------------------------
+
+
+class FakeClock:
+    """手动推进的时钟。``sleep`` 只把时间往前拨，不真的等。"""
+
+    def __init__(self) -> None:
+        self.t = 0.0
+
+    def now(self) -> float:
+        return self.t
+
+    def sleep(self, dt: float) -> None:
+        self.t += max(0.0, dt)
+
+
+class ScriptedTactile:
+    """按预设脚本给出触觉读数。
+
+    脚本是一个 ``(切向, 法向)`` 列表，交给 ``Decider`` 决定每次返回什么。
+    用列表而不是固定值，是为了模拟"下压若干步之后才接触"。
+    """
+
+    def __init__(self, script, *, age: float = 0.0, stale_after: Optional[int] = None):
+        self.script = script
+        self.age = age
+        self.stale_after = stale_after
+        self.calls = 0
+
+    def __call__(self) -> Optional[TactileSample]:
+        idx = self.calls
+        self.calls += 1
+        if callable(self.script):
+            value = self.script(idx)
+        else:
+            value = self.script[min(idx, len(self.script) - 1)]
+        if value is None:
+            return None
+        tangential, normal = value
+        age = self.age
+        if self.stale_after is not None and idx >= self.stale_after:
+            age = 999.0
+        return TactileSample(tangential=tangential, normal=normal, age_sec=age)
+
+
+def make_controller(
+    cfg: DescentConfig,
+    tactile,
+    *,
+    step_ok=None,
+    clock: Optional[FakeClock] = None,
+    cancelled=None,
+    on_progress=None,
+    log: Optional[List[float]] = None,
+) -> DescentController:
+    """搭一个控制器，把"下压一步"换成往 log 里记一笔。"""
+    clock = clock or FakeClock()
+    steps = log if log is not None else []
+
+    def step_down(dz: float) -> bool:
+        if step_ok is not None and not step_ok(len(steps)):
+            return False
+        steps.append(dz)
+        return True
+
+    return DescentController(
+        cfg,
+        read_tactile=tactile,
+        step_down=step_down,
+        now=clock.now,
+        sleep=clock.sleep,
+        is_cancelled=(cancelled or (lambda: False)),
+        on_progress=on_progress,
+    )
+
+
+BASE = DescentConfig(
+    contact_axis=0,
+    contact_delta=100.0,
+    max_normal_delta=0.0,
+    descent_step_m=0.0005,
+    descent_speed_mps=0.005,
+    max_descent_m=0.05,
+    settle_sec=0.0,
+    timeout_sec=10.0,
+)
+
+
+# ---------------------------------------------------------------------------
+# 接触判据
+# ---------------------------------------------------------------------------
+
+
+class TestContactDetection:
+    def test_already_in_contact_reports_zero_travel(self):
+        """起手就已经接触：不该先压一步再判断，否则会多压 0.5mm。"""
+        log: List[float] = []
+        ctrl = make_controller(BASE, ScriptedTactile([(150.0, 5.0)]), log=log)
+        result = ctrl.run()
+        assert result.outcome is DescentOutcome.CONTACT
+        assert result.descended_m == 0.0
+        assert log == []
+
+    def test_contact_after_several_steps(self):
+        """前 4 次读数低于阈值，之后跳变 —— 应当正好压 4 步。"""
+        script = [(10.0, 1.0), (20.0, 1.0), (30.0, 1.0), (40.0, 1.0), (200.0, 1.0)]
+        log: List[float] = []
+        ctrl = make_controller(BASE, ScriptedTactile(script), log=log)
+        result = ctrl.run()
+        assert result.outcome is DescentOutcome.CONTACT
+        # 循环顺序是「先读、再判、最后走步」，所以第 5 次读到时已经走了 4 步。
+        assert len(log) == 4
+        assert result.descended_m == pytest.approx(4 * 0.0005)
+        assert result.tangential_delta == 200.0
+
+    def test_negative_tangential_also_counts(self):
+        """切向力往负方向跳同样算接触 —— 判据用绝对值。"""
+        ctrl = make_controller(BASE, ScriptedTactile([(-150.0, 1.0)]))
+        assert ctrl.run().outcome is DescentOutcome.CONTACT
+
+    def test_exactly_at_threshold_counts(self):
+        """刚好等于阈值算接触（>= 而不是 >），避免边界上漏检。"""
+        ctrl = make_controller(BASE, ScriptedTactile([(100.0, 1.0)]))
+        assert ctrl.run().outcome is DescentOutcome.CONTACT
+
+    def test_axis_does_not_affect_pure_logic(self):
+        """纯逻辑只接收已经选好的切向值 —— contact_axis 在它这里不改变结果。
+
+        通道选择本身由 ``select_tangential`` 负责（见 TestSelectTangential）。
+        这条测试把边界钉住：如果你试图在纯逻辑里再按 axis 挑一次，就会重复选择
+        并挑错通道。
+        """
+        for axis in (0, 1):
+            ctrl = make_controller(
+                replace(BASE, contact_axis=axis),
+                ScriptedTactile([(500.0, 1.0)]),
+            )
+            assert ctrl.run().outcome is DescentOutcome.CONTACT
+
+    def test_success_flag_matches_outcome(self):
+        ctrl = make_controller(BASE, ScriptedTactile([(150.0, 1.0)]))
+        result = ctrl.run()
+        assert result.success is True
+        assert result.status == "contacting"
+
+
+# ---------------------------------------------------------------------------
+# 安全停止条件
+# ---------------------------------------------------------------------------
+
+
+class TestSafetyStops:
+    def test_normal_limit_beats_contact(self):
+        """法向力超限和切向接触同时满足时，必须先报安全限 —— 硬顶比接触更危险。"""
+        ctrl = make_controller(
+            replace(BASE, max_normal_delta=50.0),
+            ScriptedTactile([(500.0, 80.0)]),
+        )
+        result = ctrl.run()
+        assert result.outcome is DescentOutcome.NORMAL_LIMIT
+        assert result.success is False
+        assert result.normal_delta == 80.0
+
+    def test_normal_limit_disabled_when_zero(self):
+        """max_normal_delta <= 0 表示不检查法向力。"""
+        ctrl = make_controller(replace(BASE, max_normal_delta=0.0), ScriptedTactile([(10.0, 99999.0)]))
+        assert ctrl.run().outcome is DescentOutcome.MAX_DESCENT
+
+    def test_max_descent_when_never_contacting(self):
+        log: List[float] = []
+        ctrl = make_controller(BASE, ScriptedTactile([(1.0, 1.0)]), log=log)
+        result = ctrl.run()
+        assert result.outcome is DescentOutcome.MAX_DESCENT
+        # 行程上限 0.05 / 步长 0.0005 = 100 步；不能超过
+        assert len(log) == 100
+        assert result.descended_m == pytest.approx(0.05)
+        assert "行程上限" in result.message
+
+    def test_timeout_stops_before_travel_limit(self):
+        """超时必须能提前中断 —— 即使行程还没走完。"""
+        cfg = replace(BASE, timeout_sec=0.6)
+        log: List[float] = []
+        ctrl = make_controller(cfg, ScriptedTactile([(1.0, 1.0)]), log=log)
+        result = ctrl.run()
+        assert result.outcome is DescentOutcome.TIMEOUT
+        assert len(log) < 100  # 没走到行程上限
+        assert result.success is False
+
+    def test_tactile_disconnect_stops_immediately(self):
+        """读不到触觉时必须立刻停 —— 无反馈下压是危险的。"""
+        log: List[float] = []
+        ctrl = make_controller(BASE, ScriptedTactile([None]), log=log)
+        result = ctrl.run()
+        assert result.outcome is DescentOutcome.TACTILE_STALE
+        assert log == []
+        assert result.success is False
+
+    def test_tactile_disconnect_after_some_steps(self):
+        """前 3 次有数据，之后断流：应当正好走了 3 步就停。"""
+        script = lambda i: (1.0, 1.0) if i < 3 else None  # noqa: E731
+        log: List[float] = []
+        ctrl = make_controller(BASE, ScriptedTactile(script), log=log)
+        assert ctrl.run().outcome is DescentOutcome.TACTILE_STALE
+        assert len(log) == 3
+
+    def test_stale_tactile_stops(self):
+        """数据有值但已经过期（传感器卡死）也必须停。
+
+        循环顺序是「先读触觉、后走步」，所以第 4 次读（idx=3）才发现过期，
+        此时已经走了 3 步。
+        """
+        log: List[float] = []
+        ctrl = make_controller(
+            BASE, ScriptedTactile([(1.0, 1.0)], stale_after=3), log=log
+        )
+        result = ctrl.run()
+        assert result.outcome is DescentOutcome.TACTILE_STALE
+        assert "过期" in result.message
+        assert len(log) == 3
+
+    def test_motion_failure_stops(self):
+        """某一步没走成就停下，不要假装走出了这个距离。"""
+        log: List[float] = []
+        ctrl = make_controller(
+            BASE, ScriptedTactile([(1.0, 1.0)]), step_ok=lambda n: n < 2, log=log
+        )
+        result = ctrl.run()
+        assert result.outcome is DescentOutcome.MOTION_FAILED
+        assert result.descended_m == pytest.approx(2 * 0.0005)
+
+    def test_cancel_is_honoured(self):
+        """走 3 步后拉取消标志：应当在下一次循环开头就停下。
+
+        注意取消检查在 ``read_tactile`` **之前**，所以读到第 4 帧、走完第 4 步
+        之后才会真正退出。
+        """
+        log: List[float] = []
+        flag = {"v": False}
+
+        def read():
+            if len(log) >= 3:
+                flag["v"] = True
+            return TactileSample(1.0, 1.0, 0.0)
+
+        ctrl = make_controller(BASE, read, cancelled=lambda: flag["v"], log=log)
+        result = ctrl.run()
+        assert result.outcome is DescentOutcome.CANCELLED
+        assert result.success is False
+        assert len(log) == 4
+
+
+# ---------------------------------------------------------------------------
+# feedback
+# ---------------------------------------------------------------------------
+
+
+class TestProgress:
+    def test_progress_reported_and_monotonic(self):
+        seen: List[DescentProgress] = []
+        ctrl = make_controller(
+            BASE, ScriptedTactile([(1.0, 1.0)]),
+            on_progress=seen.append,
+        )
+        result = ctrl.run()
+        assert result.outcome is DescentOutcome.MAX_DESCENT
+        # 每走一步 1 次 + 结束 1 次
+        assert len(seen) == 101
+        fractions = [p.fraction for p in seen]
+        assert fractions == sorted(fractions)
+        assert fractions[-1] == pytest.approx(1.0)
+        assert seen[-1].status == "error"
+
+    def test_progress_on_immediate_contact_is_zero(self):
+        seen: List[DescentProgress] = []
+        ctrl = make_controller(BASE, ScriptedTactile([(500.0, 1.0)]), on_progress=seen.append)
+        ctrl.run()
+        assert len(seen) == 1
+        assert seen[0].fraction == 0.0
+        assert seen[0].status == "contacting"
+
+
+# ---------------------------------------------------------------------------
+# 切向通道选择
+# ---------------------------------------------------------------------------
+
+
+class TestSelectTangential:
+    """``select_tangential`` 决定装配时"看哪一路切向力"。
+
+    选错通道 = 接触永远检测不到，机械臂会一路压到行程上限 —— 这在真机上可能
+    压坏通止规或工件，所以值得单独测。
+    """
+
+    def test_axis_zero_picks_first_channel(self):
+        assert select_tangential(shearx=11.0, sheary=22.0, axis=0) == 11.0
+
+    def test_axis_one_picks_second_channel(self):
+        assert select_tangential(shearx=11.0, sheary=22.0, axis=1) == 22.0
+
+    def test_unexpected_axis_falls_back_to_first(self):
+        """轴号只可能是 0/1；出现别的值（配置写错）时按 0 处理而不是抛异常。"""
+        assert select_tangential(shearx=11.0, sheary=22.0, axis=7) == 11.0
+        assert select_tangential(shearx=11.0, sheary=22.0, axis=-1) == 11.0
+
+    def test_result_is_always_float(self):
+        """ROS 消息里是 float32，纯逻辑侧统一成 float，避免 numpy 类型混进来。"""
+        assert isinstance(select_tangential(1, 2, 0), float)
+
+    def test_zero_on_chosen_channel_is_preserved(self):
+        """选中的那一路恰好是 0（静止）时必须返回 0 —— 这正是"未接触"的正常读数，
+        不能因为 0 是假值就回退到另一路。"""
+        assert select_tangential(shearx=0.0, sheary=99.0, axis=0) == 0.0
+
+
+# ---------------------------------------------------------------------------
+# 配置解析
+# ---------------------------------------------------------------------------
+
+
+class _Goal:
+    """假的 action goal，字段与 CompliantDescent.Goal 对齐。"""
+
+    def __init__(self, **kw):
+        self.contact_axis = kw.get("contact_axis", 0.0)
+        self.contact_delta = kw.get("contact_delta", 0.0)
+        self.max_normal_delta = kw.get("max_normal_delta", 0.0)
+        self.descent_step_m = kw.get("descent_step_m", 0.0)
+        self.descent_speed_mps = kw.get("descent_speed_mps", 0.0)
+        self.max_descent_m = kw.get("max_descent_m", 0.0)
+        self.settle_sec = kw.get("settle_sec", 0.0)
+        self.timeout_sec = kw.get("timeout_sec", 0.0)
+
+
+class TestConfigFromGoal:
+    def test_zeros_fall_back_to_defaults(self):
+        cfg = DescentConfig.from_goal(_Goal(contact_delta=250.0))
+        assert cfg.descent_step_m == 0.0005
+        assert cfg.descent_speed_mps == 0.005
+        assert cfg.max_descent_m == 0.05
+        assert cfg.contact_delta == 250.0
+
+    def test_nan_falls_back_to_default(self):
+        cfg = DescentConfig.from_goal(_Goal(descent_step_m=float("nan")))
+        assert cfg.descent_step_m == 0.0005
+
+    def test_step_is_clamped_to_sane_range(self):
+        """手滑把 mm 当 m 填时要被夹住，不能真的走 5mm 一步。"""
+        big = DescentConfig.from_goal(_Goal(descent_step_m=5.0))
+        assert big.descent_step_m <= 0.01
+        tiny = DescentConfig.from_goal(_Goal(descent_step_m=1e-9))
+        assert tiny.descent_step_m >= 1e-5
+
+    def test_axis_only_accepts_0_or_1(self):
+        assert DescentConfig.from_goal(_Goal(contact_axis=1)).contact_axis == 1
+        assert DescentConfig.from_goal(_Goal(contact_axis=7)).contact_axis == 0
+
+    def test_step_interval_accounts_for_settle(self):
+        cfg = DescentConfig(
+            descent_step_m=0.001, descent_speed_mps=0.01, settle_sec=0.05
+        )
+        assert cfg.step_interval_sec == pytest.approx(0.1 + 0.05)
+
+    def test_step_interval_guards_zero_speed(self):
+        cfg = DescentConfig(descent_speed_mps=0.0, settle_sec=0.03)
+        assert cfg.step_interval_sec == pytest.approx(0.03)
+
+    def test_timeout_derived_when_unspecified(self):
+        cfg = DescentConfig(
+            max_descent_m=0.05, descent_speed_mps=0.01, timeout_sec=0.0
+        )
+        # 名义 5s，留 3 倍余量
+        assert cfg.effective_timeout_sec == pytest.approx(15.0)
+
+    def test_timeout_has_floor(self):
+        cfg = DescentConfig(max_descent_m=0.001, descent_speed_mps=1.0, timeout_sec=0.0)
+        assert cfg.effective_timeout_sec >= 2.0
+
+    def test_explicit_timeout_wins(self):
+        cfg = DescentConfig(timeout_sec=3.5)
+        assert cfg.effective_timeout_sec == pytest.approx(3.5)
+
+
+# ---------------------------------------------------------------------------
+# 四元数互转（结果位姿要用）
+# ---------------------------------------------------------------------------
+
+
+class TestQuaternionRoundTrip:
+    def test_round_trip_arbitrary_rotation(self):
+        R = _quat_xyzw_to_R([0.3, -0.2, 0.5, 0.78])
+        back = _quat_xyzw_to_R(_R_to_quat_xyzw(R))
+        assert np.allclose(R, back, atol=1e-9)
+
+    def test_identity(self):
+        q = _R_to_quat_xyzw(np.eye(3))
+        assert np.allclose(q, [0.0, 0.0, 0.0, 1.0], atol=1e-9)
+
+    def test_180_deg_cases_are_stable(self):
+        """180° 旋转是 Shepperd 法最容易踩坑的分支（trace <= 0），逐一验证。"""
+        for axis in range(3):
+            R = -np.eye(3)
+            R[axis, axis] = 1.0  # 绕该轴 180°
+            q = _R_to_quat_xyzw(R)
+            assert np.allclose(_quat_xyzw_to_R(q), R, atol=1e-9), f"axis={axis}"

src/arm_driver_flexiv/test/test_rdk_client.py

@@ -0,0 +1,341 @@
+"""``RdkClient`` 的协议层单测。
+
+为什么值得测
+------------
+``RdkClient`` 是驱动与 RDK 之间唯一的通道，而它的失败模式全都很难在真机上
+复现：桥进程卡住、返回半行 JSON、被 RDK 自身的日志污染 stdout、进程意外退出。
+这些情况一旦在装配现场发生，表现是"节点卡死"或"莫名其妙的解析错误"，
+排查成本极高。所以这里用一个假桥脚本把这些路径全部走一遍。
+
+假桥是个真实的子进程（真的走 stdin/stdout 管道与超时逻辑），只是不连机器人。
+用 ``sys.executable`` 保证在任何环境都能跑。
+"""
+
+from __future__ import annotations
+
+import sys
+from pathlib import Path
+
+import pytest
+
+from arm_driver_flexiv.rdk_client import RdkBridgeError, RdkClient
+
+
+# ---------------------------------------------------------------------------
+# 假桥
+# ---------------------------------------------------------------------------
+
+_HANDSHAKE = """
+def _reply(obj):
+    print(json.dumps(obj))
+    sys.stdout.flush()
+"""
+
+_LOOP = """
+for line in sys.stdin:
+    line = line.strip()
+    if not line:
+        continue
+    try:
+        msg = json.loads(line)
+    except Exception:
+        _reply({"ok": False, "error": "bad json"})
+        continue
+    cmd = msg.get("cmd", "")
+    if cmd == "ping":
+        PING()
+    elif cmd == "connect":
+        CONNECT()
+    elif cmd == "quit":
+        _reply({"ok": True})
+        break
+    elif cmd == "hang":
+        pass                      # 故意不回，用来测超时
+    elif cmd == "half":
+        sys.stdout.write('{"ok": true')   # 半行 JSON，永不补全
+        sys.stdout.flush()
+    elif cmd == "noisy":
+        sys.stdout.write("RDK: some native log line\\n")
+        sys.stdout.flush()
+        _reply({"ok": True, "after_noise": True})
+    elif cmd == "boom":
+        _reply({"ok": False, "error": "故意失败"})
+    elif cmd == "suicide":
+        sys.exit(7)
+    else:
+        _reply({"ok": True, "cmd": cmd, "echo": msg.get("name", ""),
+                "req": {k: v for k, v in msg.items() if k != "cmd"}})
+"""
+
+_MODES = {
+    "ok": dict(ping='_reply({"ok": True, "py": "fake"})',
+               connect='_reply({"ok": True, "sn": msg.get("sn"), "q": [0.0] * 7})'),
+    "connectfail": dict(ping='_reply({"ok": True, "py": "fake"})',
+                        connect='_reply({"ok": False, "error": "网线没插"})'),
+    "badjson_on_ping": dict(ping='sys.stdout.write("{not valid json\\n"); sys.stdout.flush()',
+                            connect='_reply({"ok": True})'),
+    "die_on_ping": dict(ping="sys.exit(9)", connect='_reply({"ok": True})'),
+}
+
+
+def make_bridge(tmp_path: Path, mode: str = "ok") -> Path:
+    """把假桥脚本写进 tmp 目录，返回路径。"""
+    parts = _MODES[mode]
+    src = "import json, sys\n" + _HANDSHAKE + _LOOP
+    src = src.replace("PING()", parts["ping"]).replace("CONNECT()", parts["connect"])
+    path = tmp_path / f"fake_bridge_{mode}.py"
+    path.write_text(src, encoding="utf-8")
+    return path
+
+
+def client_for(tmp_path: Path, mode: str = "ok", **kw) -> RdkClient:
+    return RdkClient(
+        sn="Rizon4-TEST",
+        iface="",
+        python=sys.executable,
+        script=make_bridge(tmp_path, mode),
+        log_path=str(tmp_path / "bridge.log"),
+        reply_timeout=kw.pop("reply_timeout", 2.0),
+        connect_timeout=kw.pop("connect_timeout", 5.0),
+        **kw,
+    )
+
+
+# ---------------------------------------------------------------------------
+# 启动与关闭
+# ---------------------------------------------------------------------------
+
+
+class TestLifecycle:
+    def test_start_connects_and_reports_running(self, tmp_path):
+        client = client_for(tmp_path)
+        client.start()
+        try:
+            assert client.is_running
+        finally:
+            client.stop()
+        assert not client.is_running
+
+    def test_start_without_connect_leaves_link_up(self, tmp_path):
+        """connect=False 用于没有真机的场合：桥要起来，但不连机器人。"""
+        client = client_for(tmp_path)
+        client.start(connect=False)
+        try:
+            assert client.is_running
+            assert client.request("ping")["ok"] is True
+        finally:
+            client.stop()
+
+    def test_stop_is_idempotent(self, tmp_path):
+        client = client_for(tmp_path)
+        client.start()
+        client.stop()
+        client.stop()  # 不应抛
+        assert not client.is_running
+
+    def test_stop_before_start_is_safe(self, tmp_path):
+        client_for(tmp_path).stop()
+
+    def test_context_manager_closes(self, tmp_path):
+        with client_for(tmp_path) as client:
+            assert client.is_running
+        assert not client.is_running
+
+    def test_missing_interpreter_reports_how_to_fix(self, tmp_path):
+        client = RdkClient(
+            sn="x", python=str(tmp_path / "nope" / "python"), script=make_bridge(tmp_path)
+        )
+        with pytest.raises(RdkBridgeError) as exc:
+            client.start()
+        # hint 必须可执行：告诉用户设 RDK_PYTHON
+        assert "RDK_PYTHON" in exc.value.hint
+
+    def test_missing_script_reports_where_to_look(self, tmp_path):
+        client = RdkClient(
+            sn="x", python=sys.executable, script=tmp_path / "nope.py"
+        )
+        with pytest.raises(RdkBridgeError) as exc:
+            client.start()
+        assert "SITE_ROS2_ROOT" in exc.value.hint
+
+    def test_connect_failure_includes_operator_hint(self, tmp_path):
+        client = client_for(tmp_path, "connectfail")
+        with pytest.raises(RdkBridgeError) as exc:
+            client.start()
+        assert "网线没插" in str(exc.value)
+        assert "示教器" in exc.value.hint or "AUTO" in exc.value.hint
+
+    def test_connect_failure_cleans_up_the_process(self, tmp_path):
+        """连接失败后不能留下孤儿桥进程 —— 否则 RDK 的独占访问会被它占住。"""
+        client = client_for(tmp_path, "connectfail")
+        with pytest.raises(RdkBridgeError):
+            client.start()
+        assert not client.is_running
+        assert client._proc is None  # noqa: SLF001
+
+    def test_dying_bridge_on_start_is_reported(self, tmp_path):
+        client = client_for(tmp_path, "die_on_ping")
+        with pytest.raises(RdkBridgeError):
+            client.start()
+
+    def test_bad_json_from_bridge_is_reported(self, tmp_path):
+        """RDK 的原生日志混进 stdout 会破坏协议 —— 报错要指向桥日志。"""
+        client = client_for(tmp_path, "badjson_on_ping")
+        with pytest.raises(RdkBridgeError) as exc:
+            client.start()
+        assert "非 JSON" in str(exc.value)
+        assert "bridge.log" in str(exc.value)
+
+    def test_request_before_start_raises(self, tmp_path):
+        client = client_for(tmp_path)
+        with pytest.raises(RdkBridgeError) as exc:
+            client.request("ping")
+        assert "未运行" in str(exc.value)
+
+
+# ---------------------------------------------------------------------------
+# 协议交互
+# ---------------------------------------------------------------------------
+
+
+class TestProtocol:
+    def test_request_returns_parsed_dict(self, tmp_path):
+        with client_for(tmp_path) as client:
+            reply = client.request("ping")
+        assert reply == {"ok": True, "py": "fake"}
+
+    def test_unknown_command_still_round_trips(self, tmp_path):
+        with client_for(tmp_path) as client:
+            reply = client.request("whatever", name="hello")
+        assert reply["ok"] is True
+        assert reply["echo"] == "hello"
+
+    def test_request_ok_raises_on_failure(self, tmp_path):
+        with client_for(tmp_path) as client:
+            with pytest.raises(RdkBridgeError) as exc:
+                client.request_ok("boom")
+        assert "故意失败" in str(exc.value)
+        assert "bridge.log" in exc.value.hint
+
+    def test_request_without_ok_flag_does_not_raise(self, tmp_path):
+        """``request`` 是低层的，不替调用方判断 ok —— 有些命令要自己看字段。"""
+        with client_for(tmp_path) as client:
+            reply = client.request("boom")
+        assert reply["ok"] is False
+
+    def test_timeout_on_silent_bridge(self, tmp_path):
+        with client_for(tmp_path) as client:
+            with pytest.raises(RdkBridgeError) as exc:
+                client.request("hang", timeout=0.4)
+        assert "超时" in str(exc.value)
+
+    def test_timeout_mentions_log_path(self, tmp_path):
+        with client_for(tmp_path) as client:
+            with pytest.raises(RdkBridgeError) as exc:
+                client.request("hang", timeout=0.3)
+        assert "bridge.log" in exc.value.hint
+
+    def test_half_line_json_is_not_accepted(self, tmp_path):
+        """只收到半行时不能当成应答 —— 必须等到换行或超时。"""
+        with client_for(tmp_path) as client:
+            with pytest.raises(RdkBridgeError) as exc:
+                client.request("half", timeout=0.4)
+        assert "超时" in str(exc.value)
+
+    def test_native_log_lines_before_json_are_skipped(self, tmp_path):
+        """RDK 会把原生日志写到 stderr，但万一混进 stdout，非 JSON 行要能跳过
+        并继续读到真正的 JSON 应答，而不是直接判协议损坏。"""
+        with client_for(tmp_path) as client:
+            reply = client.request("noisy", timeout=1.0)
+        assert reply["after_noise"] is True
+
+    def test_bridge_death_during_request_is_detected(self, tmp_path):
+        with client_for(tmp_path) as client:
+            with pytest.raises(RdkBridgeError) as exc:
+                client.request("suicide", timeout=1.5)
+        assert "已退出" in str(exc.value)
+
+    def test_reply_after_bridge_death_is_refused(self, tmp_path):
+        with client_for(tmp_path) as client:
+            with pytest.raises(RdkBridgeError):
+                client.request("suicide", timeout=1.5)
+            # 进程已死，后续请求必须立刻失败而不是挂住
+            with pytest.raises(RdkBridgeError):
+                client.request("ping", timeout=0.5)
+
+
+# ---------------------------------------------------------------------------
+# 便捷方法是否转发到正确的命令名
+# ---------------------------------------------------------------------------
+
+
+class TestConvenienceWrappers:
+    def test_enable_sends_enable(self, tmp_path):
+        with client_for(tmp_path) as client:
+            assert client.enable()["cmd"] == "enable"
+
+    def test_clear_fault_sends_clear_fault(self, tmp_path):
+        with client_for(tmp_path) as client:
+            assert client.clear_fault()["cmd"] == "clear_fault"
+
+    def test_status_sends_status(self, tmp_path):
+        with client_for(tmp_path) as client:
+            assert client.status()["cmd"] == "status"
+
+    def test_switch_tool_forwards_the_name(self, tmp_path):
+        with client_for(tmp_path) as client:
+            assert client.switch_tool("tool1")["echo"] == "tool1"
+
+    def test_switch_tool_with_empty_name_queries_only(self, tmp_path):
+        with client_for(tmp_path) as client:
+            assert client.switch_tool("")["echo"] == ""
+
+    def test_drag_on_forwards_mode(self, tmp_path):
+        with client_for(tmp_path) as client:
+            assert client.drag_on("floating")["req"]["mode"] == "floating"
+
+    def test_drag_off_sends_drag_off(self, tmp_path):
+        with client_for(tmp_path) as client:
+            assert client.drag_off()["cmd"] == "drag_off"
+
+    def test_joints_reads_the_q_list(self, tmp_path):
+        with client_for(tmp_path) as client:
+            q = client.joints()
+        # 假桥对未知命令回 {"ok":True,"cmd":"joints","echo":""}，没有 q 字段，
+        # 所以这里应当得到空列表而不是抛异常。
+        assert q == []
+
+
+# ---------------------------------------------------------------------------
+# 默认值解析
+# ---------------------------------------------------------------------------
+
+
+class TestDefaults:
+    def test_default_python_is_overridable_by_env(self, monkeypatch):
+        from arm_driver_flexiv import rdk_client
+
+        monkeypatch.setenv("RDK_PYTHON", "/opt/custom/python")
+        assert rdk_client.default_rdk_python() == "/opt/custom/python"
+
+    def test_default_python_falls_back_to_hardcoded(self, monkeypatch):
+        from arm_driver_flexiv import rdk_client
+
+        monkeypatch.delenv("RDK_PYTHON", raising=False)
+        assert rdk_client.default_rdk_python() == rdk_client.DEFAULT_RDK_PYTHON
+
+    def test_bridge_script_follows_workspace_env(self, monkeypatch):
+        from arm_driver_flexiv import rdk_client
+
+        monkeypatch.setenv("SITE_ROS2_ROOT", "/tmp/other_ws")
+        assert str(rdk_client.default_bridge_script()) == str(
+            Path("/tmp/other_ws") / rdk_client.BRIDGE_SCRIPT_REL
+        )
+
+    def test_error_str_includes_hint(self):
+        err = RdkBridgeError("出错了", hint="这么修")
+        assert "出错了" in str(err)
+        assert "这么修" in str(err)
+
+    def test_error_without_hint_is_just_the_message(self):
+        assert str(RdkBridgeError("只有消息")) == "只有消息"

src/arm_interfaces/arm_interfaces/capabilities.py

@@ -57,11 +57,11 @@ ROBOT_CAPABILITIES: Dict[str, CapabilityInfo] = {
     "compliant_descent": CapabilityInfo(
         "robot", "柔顺下臂 —— 带接触力约束的下压。装配接触阶段（S07）依赖它。"
     ),
-    "set_tool": CapabilityInfo(
-        "robot", "设置工具坐标系（TCP + 负载）。全局工具系锁定依赖它。"
-    ),
     "switch_tool": CapabilityInfo(
-        "robot", "在已保存的工具坐标系之间切换（如切到 tool1）。"
+        "robot",
+        "切换到已保存的工具坐标系（如切到 tool1）。"
+        "RDK 只提供按名字切换（Tool.Switch），工具的参数（质量/质心/TCP）在 Studio 里预配好。"
+        "全局工具系锁定依赖它。",
     ),
     "home": CapabilityInfo("robot", "回到初始位姿。每个装配循环开始与异常复位依赖它。"),
     "enable": CapabilityInfo("robot", "伺服使能。启动自检依赖它。"),
@@ -159,7 +159,6 @@ ROS_INTERFACE: Dict[str, RosInterface] = {
     "compliant_descent": RosInterface(
         "", "compliant_descent", "action", "arm_msgs/action/CompliantDescent"
     ),
-    "set_tool": RosInterface("", "set_tool", "service", "arm_msgs/srv/SetTool"),
     "switch_tool": RosInterface("", "switch_tool", "service", "arm_msgs/srv/SwitchTool"),
     "home": RosInterface("", "home", "service", "std_srvs/srv/Trigger"),
     "enable": RosInterface("", "enable", "service", "std_srvs/srv/SetBool"),
@@ -235,7 +234,6 @@ REQUIREMENTS: Dict[str, Requirement] = {
             "set_named_pose",
             "servo_cartesian",
             "compliant_descent",
-            "set_tool",
             "switch_tool",
             "home",
             "enable",

src/arm_kinematics/arm_kinematics/__init__.py

@@ -0,0 +1,33 @@
+"""纯几何 / 运动学工具（只依赖 numpy，不依赖 ROS）。
+
+目前提供基于 TF 实测位形的几何雅可比与阻尼最小二乘逆解，
+见 :mod:`arm_kinematics.geom_ik`。
+
+单独成包的动机：这套东西同时被应用层（视觉伺服、搜孔装配）和驱动层
+（柔顺下压）需要。原先它放在 ``arm_application`` 里，导致驱动层要反过来依赖
+应用层 —— 层次倒置。提取到这里之后两边都依赖一个无 ROS 依赖的底层包。
+"""
+
+from .geom_ik import (
+    RIZON_LINK_AXES,
+    dls_position_step,
+    dls_translate_hold_ori,
+    dls_twist,
+    geometric_jacobian,
+    load_handeye_T,
+    position_jacobian,
+    quat_xyzw_to_R,
+)
+
+__version__ = "0.1.0"
+
+__all__ = [
+    "RIZON_LINK_AXES",
+    "quat_xyzw_to_R",
+    "geometric_jacobian",
+    "position_jacobian",
+    "dls_position_step",
+    "dls_twist",
+    "dls_translate_hold_ori",
+    "load_handeye_T",
+]

src/arm_kinematics/arm_kinematics/geom_ik.py

@@ -0,0 +1,186 @@
+#!/usr/bin/env python3
+"""基于 TF 实测位形的几何雅可比 + 阻尼最小二乘（DLS）位置级逆解。
+
+为什么不需要 URDF / 正运动学
+---------------------------
+本模块做的是**速度级**逆解，不是位置级解析逆解：
+
+1. 从 TF 直接读每个关节 link 的当前位姿（``link_R`` / ``link_p``）
+2. 用固定的关节轴（``RIZON_LINK_AXES``，来自 Rizon4 URDF）构建几何雅可比 J
+3. 解 ``J dq = dp`` 得到小步 ``dq``，积分即可
+
+雅可比只是**当前位形**的函数，所以用 TF 实测位形算它是正确且实时的；不需要
+"给定关节角预测末端位姿"的正运动学。代价是只能走小步（大位移要迭代多次），
+而这恰好符合装配场景的需求 —— 每次挪 1mm 以内，边动边看力反馈。
+
+依赖：只依赖 numpy。不 import ROS，因此可被驱动层与测试直接复用。
+
+坐标系约定：所有输入输出都在**基座系**，末端位置 ``ee_p`` 与各 link 位置
+``link_p`` 必须同一参考系。
+"""
+
+from __future__ import annotations
+
+from typing import List, Optional, Sequence, Tuple
+
+import numpy as np
+
+#: Rizon4 各关节轴在**子 link 系**下的表达（来自 URDF）。
+#: 顺序对应 joint1..joint7 —— 相邻关节轴交替，标准 7 自由度布局。
+RIZON_LINK_AXES = [
+    (0.0, 0.0, 1.0),  # joint1 -> link1
+    (0.0, 1.0, 0.0),  # joint2 -> link2
+    (0.0, 0.0, 1.0),
+    (0.0, 1.0, 0.0),
+    (0.0, 0.0, 1.0),
+    (0.0, 1.0, 0.0),
+    (0.0, 0.0, 1.0),
+]
+
+
+def quat_xyzw_to_R(q: Sequence[float]) -> np.ndarray:
+    """四元数 (x, y, z, w) 转 3x3 旋转矩阵。自动归一化，零四元数也不炸。"""
+    x, y, z, w = [float(v) for v in q]
+    n = np.sqrt(x * x + y * y + z * z + w * w) + 1e-12
+    x, y, z, w = x / n, y / n, z / n, w / n
+    return np.array(
+        [
+            [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
+            [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
+            [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
+        ],
+        dtype=float,
+    )
+
+
+def geometric_jacobian(
+    link_R: Sequence[np.ndarray],
+    link_p: Sequence[np.ndarray],
+    ee_p: np.ndarray,
+    axes_local: Sequence[Tuple[float, float, float]] = RIZON_LINK_AXES,
+) -> np.ndarray:
+    """构建 6x7 基座系几何雅可比。前 3 行是线速度，后 3 行是角速度。
+
+    参数
+    ----
+    link_R:
+        各关节 link 在基座系下的 3x3 姿态，长度 = 关节数。
+    link_p:
+        各关节 link 在基座系下的 3 维位置。
+    ee_p:
+        末端（法兰）在基座系下的 3 维位置。
+    """
+    n = len(axes_local)
+    J = np.zeros((6, n), dtype=float)
+    pee = np.asarray(ee_p, dtype=float).reshape(3)
+    for i in range(n):
+        R = np.asarray(link_R[i], dtype=float).reshape(3, 3)
+        p = np.asarray(link_p[i], dtype=float).reshape(3)
+        z = R @ np.asarray(axes_local[i], dtype=float).reshape(3)
+        # 平动部分：关节轴 × (末端 − 关节点)；转动部分：关节轴本身
+        J[:3, i] = np.cross(z, pee - p)
+        J[3:, i] = z
+    return J
+
+
+def position_jacobian(
+    link_R: Sequence[np.ndarray],
+    link_p: Sequence[np.ndarray],
+    ee_p: np.ndarray,
+    axes_local: Sequence[Tuple[float, float, float]] = RIZON_LINK_AXES,
+) -> np.ndarray:
+    """只取几何雅可比的平动部分（3x7）。"""
+    return geometric_jacobian(link_R, link_p, ee_p, axes_local)[:3, :]
+
+
+def dls_position_step(
+    J: np.ndarray,
+    dp_base: np.ndarray,
+    *,
+    damping: float = 0.05,
+    max_dq: float = 0.05,
+) -> np.ndarray:
+    """解 ``J dq = dp``（阻尼最小二乘），并把 ``|dq|`` 逐元素限幅。
+
+    阻尼项 ``λ²I`` 的作用：接近奇异位形时避免关节速度爆掉。代价是有跟踪误差，
+    但对小幅步进（毫米级）可以忽略。
+    """
+    dp = np.asarray(dp_base, dtype=float).reshape(3)
+    JT = J.T
+    A = J @ JT + (damping ** 2) * np.eye(3)
+    dq = JT @ np.linalg.solve(A, dp)
+    dq = np.clip(dq, -abs(max_dq), abs(max_dq))
+    return dq
+
+
+def dls_twist(
+    J6: np.ndarray,
+    v_base: np.ndarray,
+    *,
+    w_base: Optional[np.ndarray] = None,
+    damping: float = 0.08,
+    max_qdot: float = 0.80,
+    ori_weight: float = 1.2,
+) -> np.ndarray:
+    """解 ``J qdot = [v; w]``，默认把角速度压到 0（保持姿态）。
+
+    ``ori_weight`` 给姿态分量额外的权重，让姿态在数值上更"硬"，减少平移时
+    末端姿态的漂移。
+    """
+    dx = np.zeros(6, dtype=float)
+    dx[:3] = np.asarray(v_base, dtype=float).reshape(3)
+    if w_base is not None:
+        dx[3:] = np.asarray(w_base, dtype=float).reshape(3)
+    w = np.array([1.0, 1.0, 1.0, ori_weight, ori_weight, ori_weight], dtype=float)
+    Jw = J6 * w[:, None]
+    dxw = dx * w
+    JT = Jw.T
+    A = Jw @ JT + (damping ** 2) * np.eye(6)
+    qdot = JT @ np.linalg.solve(A, dxw)
+    return np.clip(qdot, -abs(max_qdot), abs(max_qdot))
+
+
+def dls_translate_hold_ori(
+    J6: np.ndarray,
+    dp_base: np.ndarray,
+    *,
+    damping: float = 0.08,
+    max_dq: float = 0.05,
+    ori_weight: float = 1.2,
+) -> np.ndarray:
+    """平移末端 ``dp`` 同时保持姿态。"""
+    dx = np.zeros(6, dtype=float)
+    dx[:3] = np.asarray(dp_base, dtype=float).reshape(3)
+    w = np.array([1.0, 1.0, 1.0, ori_weight, ori_weight, ori_weight], dtype=float)
+    Jw = J6 * w[:, None]
+    dxw = dx * w
+    JT = Jw.T
+    A = Jw @ JT + (damping ** 2) * np.eye(6)
+    dq = JT @ np.linalg.solve(A, dxw)
+    return np.clip(dq, -abs(max_dq), abs(max_dq))
+
+
+def load_handeye_T(path: str) -> np.ndarray:
+    """从手眼结果 YAML 里读 4x4 齐次变换矩阵（键名 ``T_4x4``）。"""
+    import yaml
+
+    with open(path, "r", encoding="utf-8") as f:
+        data = yaml.safe_load(f)
+    if not isinstance(data, dict) or "T_4x4" not in data:
+        raise ValueError(f"No T_4x4 in hand-eye file: {path}")
+    T = np.array(data["T_4x4"], dtype=float)
+    if T.shape != (4, 4):
+        raise ValueError(f"T_4x4 shape {T.shape}, expected (4,4)")
+    return T
+
+
+__all__ = [
+    "RIZON_LINK_AXES",
+    "quat_xyzw_to_R",
+    "geometric_jacobian",
+    "position_jacobian",
+    "dls_position_step",
+    "dls_twist",
+    "dls_translate_hold_ori",
+    "load_handeye_T",
+]

src/arm_kinematics/package.xml

@@ -0,0 +1,22 @@
+<?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_kinematics</name>
+  <version>0.1.0</version>
+  <description>
+    纯几何 / 运动学工具：基于 TF 实测位形的几何雅可比与阻尼最小二乘逆解。
+    只依赖 numpy，不依赖 ROS —— 因此应用层（视觉伺服）与驱动层（柔顺下压）
+    都能复用它，而不会形成层次倒置。
+  </description>
+  <maintainer email="dev@example.com">arm_dev</maintainer>
+  <license>Apache-2.0</license>
+
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
+
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/arm_kinematics/setup.cfg

@@ -0,0 +1,5 @@
+[tool:pytest]
+# ROS 2 Humble 的两个 launch-testing pytest 插件用了旧版 hook 签名，
+# 当前 pytest 会在收集阶段直接报错。本包只跑纯 Python 单测，禁掉它们。
+addopts = -p no:launch_testing -p no:launch_ros
+testpaths = test

src/arm_kinematics/setup.py

@@ -0,0 +1,20 @@
+from setuptools import find_packages, setup
+
+package_name = 'arm_kinematics'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='arm_dev',
+    maintainer_email='dev@example.com',
+    description='纯几何 / 运动学工具（几何雅可比 + DLS 逆解）',
+    license='Apache-2.0',
+    tests_require=['pytest'],
+)

src/arm_kinematics/test/test_geom_ik.py

@@ -0,0 +1,219 @@
+"""``arm_kinematics.geom_ik`` 的数学单测。
+
+这些函数是整个运动链路的地基（视觉伺服、搜孔装配、柔顺下压都走它），
+但它们的正确性可以完全脱离硬件验证 —— 用已知的几何算例对答案即可。
+所以这里覆盖得比较细，尤其是不变量（限幅、奇异不炸、零输入）和解析可算的例子。
+"""
+
+from __future__ import annotations
+
+import math
+
+import numpy as np
+import pytest
+
+from arm_kinematics.geom_ik import (
+    RIZON_LINK_AXES,
+    dls_position_step,
+    dls_translate_hold_ori,
+    dls_twist,
+    geometric_jacobian,
+    position_jacobian,
+    quat_xyzw_to_R,
+)
+
+
+# ---------------------------------------------------------------------------
+# 四元数
+# ---------------------------------------------------------------------------
+
+class TestQuatToR:
+    def test_identity(self):
+        assert np.allclose(quat_xyzw_to_R([0.0, 0.0, 0.0, 1.0]), np.eye(3))
+
+    def test_90_deg_about_z(self):
+        """90° 绕 z：(x,y,z,w) = (0,0,sin45°,cos45°) → x 轴转到 y 轴。"""
+        s = math.sin(math.pi / 4)
+        c = math.cos(math.pi / 4)
+        R = quat_xyzw_to_R([0.0, 0.0, s, c])
+        assert np.allclose(R @ np.array([1.0, 0.0, 0.0]), [0.0, 1.0, 0.0], atol=1e-9)
+
+    def test_unnormalised_input_is_normalised(self):
+        """未归一化的四元数也应当得到合法旋转（很多驱动给的就是未归一化的）。"""
+        R = quat_xyzw_to_R([0.0, 0.0, 2.0, 2.0])  # 2x 的 90° 绕 z
+        assert np.allclose(R @ np.array([1.0, 0.0, 0.0]), [0.0, 1.0, 0.0], atol=1e-9)
+
+    def test_zero_quaternion_does_not_explode(self):
+        """零四元数（数据缺失）必须给出有限值，不能产生 NaN/Inf。"""
+        R = quat_xyzw_to_R([0.0, 0.0, 0.0, 0.0])
+        assert np.all(np.isfinite(R))
+
+    def test_result_is_orthonormal(self):
+        R = quat_xyzw_to_R([0.3, -0.2, 0.5, 0.78])
+        assert np.allclose(R @ R.T, np.eye(3), atol=1e-9)
+        assert np.isclose(np.linalg.det(R), 1.0)
+
+
+# ---------------------------------------------------------------------------
+# 几何雅可比
+# ---------------------------------------------------------------------------
+
+class TestGeometricJacobian:
+    def test_single_revolute_about_z(self):
+        """单关节绕 z 轴、关节点在原点、末端在 +x 1m 处。
+
+        关节轴 z=(0,0,1)，臂向量 pee - p = (1,0,0)，
+        故平动雅可比 = z × arm = (0,1,0)：关节正转使末端朝 +y 移动。
+        """
+        J = geometric_jacobian(
+            link_R=[np.eye(3)],
+            link_p=[np.zeros(3)],
+            ee_p=np.array([1.0, 0.0, 0.0]),
+            axes_local=[(0.0, 0.0, 1.0)],
+        )
+        assert J.shape == (6, 1)
+        assert np.allclose(J[:3, 0], [0.0, 1.0, 0.0], atol=1e-12)
+        assert np.allclose(J[3:, 0], [0.0, 0.0, 1.0], atol=1e-12)
+
+    def test_axis_follows_link_rotation(self):
+        """关节轴要跟着 link 的姿态转：link 绕 z 转 90° 后，关节轴仍在 z 上
+        （因为轴本身就是 z），但绕 x 轴转 90° 后轴应变成 -y。"""
+        s = math.sin(math.pi / 4)
+        c = math.cos(math.pi / 4)
+        R = quat_xyzw_to_R([s, 0.0, 0.0, c])  # 绕 x 转 90°：(0,0,1) -> (0,-1,0)
+        J = geometric_jacobian(
+            link_R=[R],
+            link_p=[np.zeros(3)],
+            ee_p=np.array([1.0, 0.0, 0.0]),
+            axes_local=[(0.0, 0.0, 1.0)],
+        )
+        assert np.allclose(J[3:, 0], [0.0, -1.0, 0.0], atol=1e-9)
+
+    def test_position_jacobian_is_the_linear_block(self):
+        args = dict(
+            link_R=[np.eye(3), np.eye(3)],
+            link_p=[np.zeros(3), np.array([0.0, 0.0, 0.3])],
+            ee_p=np.array([0.4, 0.0, 0.5]),
+            axes_local=[(0.0, 0.0, 1.0), (0.0, 1.0, 0.0)],
+        )
+        assert np.allclose(
+            position_jacobian(**args), geometric_jacobian(**args)[:3, :]
+        )
+
+    def test_seven_axis_shape(self):
+        """真实 Rizon4 是 7 轴；轴表长度必须与之一致，否则雅可比会错位。"""
+        assert len(RIZON_LINK_AXES) == 7
+        J = geometric_jacobian(
+            link_R=[np.eye(3)] * 7,
+            link_p=[np.zeros(3)] * 7,
+            ee_p=np.array([0.5, 0.0, 0.5]),
+        )
+        assert J.shape == (6, 7)
+
+    def test_axis_table_matches_axis_count(self):
+        """axes_local 长度决定雅可比列数，传错长度应当反映在形状上（不静默出错）。"""
+        J = geometric_jacobian(
+            link_R=[np.eye(3)] * 3,
+            link_p=[np.zeros(3)] * 3,
+            ee_p=np.array([0.1, 0.0, 0.1]),
+            axes_local=[(0.0, 0.0, 1.0)] * 3,
+        )
+        assert J.shape == (6, 3)
+
+
+# ---------------------------------------------------------------------------
+# DLS 逆解
+# ---------------------------------------------------------------------------
+
+class TestDlsPositionStep:
+    def test_recovers_known_solution(self):
+        """J = [[0],[1],[0]]，要求末端 +y 移动 0.1m → dq ≈ 0.1（阻尼带来的
+        误差极小）。"""
+        J = np.array([[0.0], [1.0], [0.0]])
+        dq = dls_position_step(J, np.array([0.0, 0.1, 0.0]), damping=0.05, max_dq=1.0)
+        assert dq.shape == (1,)
+        assert abs(float(dq[0]) - 0.1) < 1e-3
+
+    def test_direction_is_correct(self):
+        """要求 +y 移动，关节也必须朝正方向走（符号不能反）。"""
+        J = np.array([[0.0], [1.0], [0.0]])
+        dq = dls_position_step(J, np.array([0.0, 0.05, 0.0]))
+        assert float(dq[0]) > 0
+
+    def test_limits_large_step(self):
+        J = np.array([[0.0], [1.0], [0.0]])
+        dq = dls_position_step(J, np.array([0.0, 10.0, 0.0]), max_dq=0.02)
+        assert abs(float(dq[0])) <= 0.02 + 1e-12
+
+    def test_singular_jacobian_does_not_explode(self):
+        """奇异位形（J 全零）必须给出零步长而不是 NaN/Inf —— 阻尼项就是为了这个。"""
+        J = np.zeros((3, 7))
+        dq = dls_position_step(J, np.array([0.1, 0.0, 0.0]))
+        assert np.all(np.isfinite(dq))
+        assert np.allclose(dq, 0.0)
+
+    def test_near_singular_is_bounded(self):
+        """接近奇异时解会被阻尼压住，不应出现巨大关节速度。"""
+        J = np.array([[1e-8], [1e-8], [1e-8]])
+        dq = dls_position_step(J, np.array([0.01, 0.01, 0.01]), max_dq=0.5)
+        assert np.all(np.isfinite(dq))
+        assert abs(float(dq[0])) <= 0.5
+
+    def test_zero_displacement_gives_zero_step(self):
+        J = np.array([[0.0], [1.0], [0.0]])
+        dq = dls_position_step(J, np.zeros(3))
+        assert np.allclose(dq, 0.0)
+
+
+class TestDlsTwist:
+    def test_shape_matches_joint_count(self):
+        dq = dls_twist(np.zeros((6, 7)), np.zeros(3))
+        assert dq.shape == (7,)
+
+    def test_respects_qdot_limit(self):
+        J = np.zeros((6, 7))
+        J[0, 0] = 1.0
+        J[1, 1] = 1.0
+        J[2, 2] = 1.0
+        qdot = dls_twist(J, np.array([5.0, 5.0, 5.0]), max_qdot=0.3)
+        assert np.all(np.abs(qdot) <= 0.3 + 1e-12)
+
+    def test_orientation_hold_keeps_angular_velocity_small(self):
+        """默认 w=0 且姿态权重更高，角运动分量应当明显小于平动分量。"""
+        J = np.zeros((6, 3))
+        J[0, 0] = 1.0   # 关节0 产生 x 平动
+        J[4, 1] = 1.0   # 关节1 产生绕 y 转动
+        qdot = dls_twist(J, np.array([0.1, 0.0, 0.0]), max_qdot=1.0)
+        assert abs(float(qdot[1])) < abs(float(qdot[0]))
+
+    def test_explicit_angular_command_is_applied(self):
+        J = np.zeros((6, 1))
+        J[5, 0] = 1.0  # 关节0 绕 z 转动
+        qdot = dls_twist(J, np.zeros(3), w_base=np.array([0.0, 0.0, 0.2]))
+        assert float(qdot[0]) > 0
+
+    def test_singular_input_is_finite(self):
+        qdot = dls_twist(np.zeros((6, 7)), np.array([0.1, 0.1, 0.1]))
+        assert np.all(np.isfinite(qdot))
+
+
+class TestDlsTranslateHoldOri:
+    def test_pure_translation_command_ignores_rotation(self):
+        """只给平动分量时，转动通道不应当被激活。"""
+        J = np.zeros((6, 2))
+        J[0, 0] = 1.0
+        J[5, 1] = 1.0  # 只有关节1 影响转动
+        dq = dls_translate_hold_ori(J, np.array([0.05, 0.0, 0.0]), max_dq=1.0)
+        assert abs(float(dq[1])) < 1e-6
+
+    def test_translation_direction(self):
+        J = np.zeros((6, 1))
+        J[2, 0] = 1.0
+        dq = dls_translate_hold_ori(J, np.array([0.0, 0.0, -0.02]), max_dq=1.0)
+        assert float(dq[0]) < 0
+
+    def test_limit_applies(self):
+        J = np.zeros((6, 1))
+        J[2, 0] = 1.0
+        dq = dls_translate_hold_ori(J, np.array([0.0, 0.0, -1.0]), max_dq=0.01)
+        assert abs(float(dq[0])) <= 0.01 + 1e-12

src/arm_msgs/CMakeLists.txt

@@ -23,8 +23,10 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/SetNamedPose.srv"
   "srv/GetArmState.srv"
   "srv/GripperExecute.srv"
+  "srv/SwitchTool.srv"
   "action/MoveToPose.action"
   "action/FollowJointTrajectory.action"
+  "action/CompliantDescent.action"
   DEPENDENCIES std_msgs geometry_msgs builtin_interfaces sensor_msgs
 )
 

src/arm_msgs/action/CompliantDescent.action

@@ -0,0 +1,40 @@
+# 柔顺下臂：从当前悬停位姿沿工具系 −Z 缓慢下压，直到触觉检测到接触。
+#
+# 通止规装配 S07（下臂接触）专用原语。
+#
+# 为什么不用 RDK 的柔顺模式：非夕 RDK 的 compliant motion 会接管整臂阻抗，
+# 而装配的其它阶段（跟踪 / 对准 / 拧入）都走 JTC 位置控制；在两种控制权之间
+# 来回切换代价高、掉电风险大。这里用「小幅步进 + 力反馈」在应用层做等效：
+# 每步 0.2~1.0 mm，步间等稳定，一旦切向力相对基线跳变立即停住。
+# 对通止规这种「碰到就必须停」的场景足够，而且任何一步都可以安全中断。
+#
+# 力阈值全部是「相对静止基线的增量」：Orisys 读数是形变任意单位且基线会漂移，
+# 绝对值没有意义。基线由传感器节点的 measure_baseline 服务在启动时测定。
+
+# --- 目标与判据 ---
+float64 target_x_m          # 目标法兰 X（基座系，m）。NaN = 保持当前 XY 不动
+float64 target_y_m          # 目标法兰 Y（基座系，m）。NaN = 保持当前 XY 不动
+float64 contact_axis        # 监测哪条切向通道：0 = 第一路切向，1 = 第二路切向
+float64 contact_delta       # 接触判据：|切向力| 相对静止基线的增量（传感器原生单位）
+float64 max_normal_delta    # 法向力安全上限（相对基线增量）。<=0 = 不检查
+string side                 # 用哪只手指的触觉：left | right（装配时是持规那只）
+
+# --- 运动参数 ---
+float64 descent_step_m      # 每步下压量（m），建议 0.0002 ~ 0.0010
+float64 descent_speed_mps   # 名义下压速度（m/s）。步进间隔 = step / speed
+float64 max_descent_m       # 最多下压距离（m）。安全上限，到了判失败并停住
+float64 settle_sec          # 每步后等待稳定（s）。0 = 不等，靠触觉话题新鲜度
+float64 timeout_sec         # 总超时（s）。0 = 由 max_descent_m / descent_speed_mps 推算
+---
+bool success                # true = 检测到接触且已停住
+string message              # 失败原因：超时 / 触觉断流 / 达到行程上限 / 法向力超限
+geometry_msgs/PoseStamped contact_pose   # 接触瞬间的法兰位姿（基座系）
+float64 descended_m                      # 实际下压距离（m）
+float64 contact_delta_observed           # 触发瞬间的切向增量
+float64 normal_delta_observed            # 触发瞬间的法向增量
+---
+float64 progress                         # 0.0 ~ 1.0（descended_m / max_descent_m）
+float64 descended_m                      # 当前已下压距离（m）
+float64 tangential_delta                 # 当前切向增量（相对基线）
+float64 normal_delta                     # 当前法向增量（相对基线）
+string status                            # approaching | contacting | retracting | done | error

src/arm_msgs/srv/SwitchTool.srv

@@ -0,0 +1,17 @@
+# 切换到已保存的工具坐标系（如 tool1）。
+#
+# 背景：非夕的工具坐标系（TCP + 质量 + 质心）在 Studio 里预先配好并命名，
+# RDK 只提供「按名字切换」一个操作。通止规装配要求工具系全程垂直向下锁定，
+# 所以在流程开始前必须确认已切到正确的工具。
+#
+# 实现：arm_driver_flexiv 调用 scripts/rdk_switch_tool.py（子进程，rdk_py310）。
+# 注意 RDK 是独占访问，切换时臂必须处于 IDLE 且没有其它进程连着 RDK。
+string tool_name            # Studio 里已保存的工具名。空字符串 = 只查询当前状态，不切换
+
+string sn                   # 目标臂 SN（如 Rizon4-063989）。空 = 用 site.yaml 的 robot.sn
+---
+bool success                # 是否已达到目标状态（已在该工具上也算成功）
+string message              # 失败原因；成功时为当前工具名说明
+string current_tool         # 操作后的当前工具名
+float64 tool_mass_kg        # 该工具的质量（kg）；查询不到为 0
+float64[3] tool_com_m       # 该工具的质心（法兰系，m）；查询不到为全 0