别人克隆后没有 install，source 会直接失败。把展开后的 install 放进仓库，符号链接和指向本机 build 的 egg-link 收成真实文件，Python 入口改走当前环境的解释器，这样不用先 colcon build 也能启动。

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-10-15 11:04:43 +08:00
parent 3c86441fb0
commit 926a7f9796
1492 changed files with 1302154 additions and 2 deletions
@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-driver-flexiv','console_scripts','flexiv_arm_driver'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm-driver-flexiv'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm-driver-flexiv', 'console_scripts', 'flexiv_arm_driver')())
@@ -0,0 +1,14 @@
+Metadata-Version: 2.4
+Name: arm_driver_flexiv
+Version: 0.1.0
+Summary: 非夕 Flexiv 机械臂驱动适配器
+Maintainer: arm_dev
+Maintainer-email: dev@example.com
+License: Apache-2.0
+Requires-Dist: setuptools
+Requires-Dist: PyYAML
+Dynamic: license
+Dynamic: maintainer
+Dynamic: maintainer-email
+Dynamic: requires-dist
+Dynamic: summary
@@ -0,0 +1,23 @@
+package.xml
+setup.cfg
+setup.py
+../../build/arm_driver_flexiv/arm_driver_flexiv.egg-info/PKG-INFO
+../../build/arm_driver_flexiv/arm_driver_flexiv.egg-info/SOURCES.txt
+../../build/arm_driver_flexiv/arm_driver_flexiv.egg-info/dependency_links.txt
+../../build/arm_driver_flexiv/arm_driver_flexiv.egg-info/entry_points.txt
+../../build/arm_driver_flexiv/arm_driver_flexiv.egg-info/requires.txt
+../../build/arm_driver_flexiv/arm_driver_flexiv.egg-info/top_level.txt
+../../build/arm_driver_flexiv/arm_driver_flexiv.egg-info/zip-safe
+arm_driver_flexiv/__init__.py
+arm_driver_flexiv/compliant_descent.py
+arm_driver_flexiv/flexiv_arm_driver.py
+arm_driver_flexiv/rdk_client.py
+arm_driver_flexiv/site_config.py
+arm_driver_flexiv.egg-info/PKG-INFO
+arm_driver_flexiv.egg-info/SOURCES.txt
+arm_driver_flexiv.egg-info/dependency_links.txt
+arm_driver_flexiv.egg-info/entry_points.txt
+arm_driver_flexiv.egg-info/requires.txt
+arm_driver_flexiv.egg-info/top_level.txt
+arm_driver_flexiv.egg-info/zip-safe
+resource/arm_driver_flexiv
@@ -0,0 +1 @@
+
@@ -0,0 +1,2 @@
+[console_scripts]
+flexiv_arm_driver = arm_driver_flexiv.flexiv_arm_driver:main
@@ -0,0 +1,2 @@
+setuptools
+PyYAML
@@ -0,0 +1 @@
+arm_driver_flexiv
@@ -0,0 +1 @@
+
@@ -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"
@@ -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]
@@ -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())
@@ -0,0 +1,393 @@
+"""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
+
+def default_rdk_python() -> str:
+    """RDK 解释器在当前用户家目录，不写死某一台机器的用户名。"""
+    env = os.environ.get("RDK_PYTHON", "").strip()
+    if env:
+        return env
+    return str(Path.home() / "rdk_py310" / "bin" / "python")
+
+
+#: 桥进程必须用这个解释器 —— RDK 绑定只在这里有。
+DEFAULT_RDK_PYTHON = default_rdk_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
@@ -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 {}