别人克隆后没有 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-control==0.1.0','console_scripts','arm_status_node'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm-control==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm-control==0.1.0', 'console_scripts', 'arm_status_node')())
@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-control==0.1.0','console_scripts','mock_arm_driver'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm-control==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm-control==0.1.0', 'console_scripts', 'mock_arm_driver')())
@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-control==0.1.0','console_scripts','named_pose_server'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm-control==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm-control==0.1.0', 'console_scripts', 'named_pose_server')())
@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-control==0.1.0','console_scripts','real_trajectory_adapter'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm-control==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm-control==0.1.0', 'console_scripts', 'real_trajectory_adapter')())
@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-control==0.1.0','console_scripts','trajectory_bridge'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'arm-control==0.1.0'
+
+try:
+    from importlib.metadata import distribution
+except ImportError:
+    try:
+        from importlib_metadata import distribution
+    except ImportError:
+        from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+    dist_name, _, _ = spec.partition('==')
+    matches = (
+        entry_point
+        for entry_point in distribution(dist_name).entry_points
+        if entry_point.group == group and entry_point.name == name
+    )
+    return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+    sys.exit(load_entry_point('arm-control==0.1.0', 'console_scripts', 'trajectory_bridge')())
@@ -0,0 +1,13 @@
+Metadata-Version: 2.4
+Name: arm_control
+Version: 0.1.0
+Summary: Arm control nodes for mock and real hardware.
+Maintainer: arm_dev
+Maintainer-email: dev@example.com
+License: Apache-2.0
+Requires-Dist: setuptools
+Dynamic: license
+Dynamic: maintainer
+Dynamic: maintainer-email
+Dynamic: requires-dist
+Dynamic: summary
@@ -0,0 +1,23 @@
+package.xml
+setup.cfg
+setup.py
+../../build/arm_control/arm_control.egg-info/PKG-INFO
+../../build/arm_control/arm_control.egg-info/SOURCES.txt
+../../build/arm_control/arm_control.egg-info/dependency_links.txt
+../../build/arm_control/arm_control.egg-info/entry_points.txt
+../../build/arm_control/arm_control.egg-info/requires.txt
+../../build/arm_control/arm_control.egg-info/top_level.txt
+../../build/arm_control/arm_control.egg-info/zip-safe
+arm_control/__init__.py
+arm_control/arm_status_node.py
+arm_control/mock_arm_driver.py
+arm_control/named_pose_server.py
+arm_control/real_trajectory_adapter.py
+arm_control/trajectory_bridge.py
+config/controllers.yaml
+config/named_poses.yaml
+config/named_poses_89.yaml
+config/named_poses_91.yaml
+launch/mock_control.launch.py
+launch/ros2_control.launch.py
+resource/arm_control
@@ -0,0 +1 @@
+
@@ -0,0 +1,6 @@
+[console_scripts]
+arm_status_node = arm_control.arm_status_node:main
+mock_arm_driver = arm_control.mock_arm_driver:main
+named_pose_server = arm_control.named_pose_server:main
+real_trajectory_adapter = arm_control.real_trajectory_adapter:main
+trajectory_bridge = arm_control.trajectory_bridge:main
@@ -0,0 +1 @@
+setuptools
@@ -0,0 +1 @@
+arm_control
@@ -0,0 +1 @@
+
@@ -0,0 +1,13 @@
+Metadata-Version: 2.4
+Name: arm_control
+Version: 0.1.0
+Summary: Arm control nodes for mock and real hardware.
+Maintainer: arm_dev
+Maintainer-email: dev@example.com
+License: Apache-2.0
+Requires-Dist: setuptools
+Dynamic: license
+Dynamic: maintainer
+Dynamic: maintainer-email
+Dynamic: requires-dist
+Dynamic: summary
@@ -0,0 +1,23 @@
+package.xml
+setup.cfg
+setup.py
+../../build/arm_control/arm_control.egg-info/PKG-INFO
+../../build/arm_control/arm_control.egg-info/SOURCES.txt
+../../build/arm_control/arm_control.egg-info/dependency_links.txt
+../../build/arm_control/arm_control.egg-info/entry_points.txt
+../../build/arm_control/arm_control.egg-info/requires.txt
+../../build/arm_control/arm_control.egg-info/top_level.txt
+../../build/arm_control/arm_control.egg-info/zip-safe
+arm_control/__init__.py
+arm_control/arm_status_node.py
+arm_control/mock_arm_driver.py
+arm_control/named_pose_server.py
+arm_control/real_trajectory_adapter.py
+arm_control/trajectory_bridge.py
+config/controllers.yaml
+config/named_poses.yaml
+config/named_poses_89.yaml
+config/named_poses_91.yaml
+launch/mock_control.launch.py
+launch/ros2_control.launch.py
+resource/arm_control
@@ -0,0 +1 @@
+
@@ -0,0 +1,6 @@
+[console_scripts]
+arm_status_node = arm_control.arm_status_node:main
+mock_arm_driver = arm_control.mock_arm_driver:main
+named_pose_server = arm_control.named_pose_server:main
+real_trajectory_adapter = arm_control.real_trajectory_adapter:main
+trajectory_bridge = arm_control.trajectory_bridge:main
@@ -0,0 +1 @@
+setuptools
@@ -0,0 +1 @@
+arm_control
@@ -0,0 +1 @@
+
@@ -0,0 +1 @@
+"""arm_control 的包初始化。"""
@@ -0,0 +1,124 @@
+#!/usr/bin/env python3
+"""根据 joint_states、TF 与 Flexiv 的 tcp_pose 发布 ArmStatus，并提供 GetArmState 服务。"""
+
+from __future__ import annotations
+
+from typing import Optional
+
+import rclpy
+from geometry_msgs.msg import PoseStamped
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from tf2_ros import Buffer, TransformListener
+
+from arm_msgs.msg import ArmStatus
+from arm_msgs.srv import GetArmState
+
+
+class ArmStatusNode(Node):
+    def __init__(self) -> None:
+        super().__init__('arm_status_node')
+        self.declare_parameter('base_frame', 'base_link')
+        self.declare_parameter('ee_frame', 'tcp')
+        self.declare_parameter('tcp_pose_topic', '')
+        self.declare_parameter('publish_rate', 20.0)
+
+        self.base_frame = str(self.get_parameter('base_frame').value)
+        self.ee_frame = str(self.get_parameter('ee_frame').value)
+        self.tcp_pose_topic = str(self.get_parameter('tcp_pose_topic').value).strip()
+        self._last_js: Optional[JointState] = None
+        self._last_tcp: Optional[PoseStamped] = None
+        self._mode = 'idle'
+
+        self._tf = Buffer()
+        TransformListener(self._tf, self)
+        self._pub = self.create_publisher(ArmStatus, 'arm_status', 10)
+        self.create_subscription(JointState, 'joint_states', self._on_js, 10)
+        if self.tcp_pose_topic:
+            self.create_subscription(
+                PoseStamped, self.tcp_pose_topic, self._on_tcp, 10
+            )
+        self.create_service(GetArmState, 'get_arm_state', self._on_get)
+        rate = float(self.get_parameter('publish_rate').value)
+        self.create_timer(1.0 / rate, self._tick)
+        self.get_logger().info(
+            f'ArmStatus: base={self.base_frame} ee={self.ee_frame} '
+            f'tcp_topic={self.tcp_pose_topic or "（无）"}'
+        )
+
+    def _on_js(self, msg: JointState) -> None:
+        self._last_js = msg
+        moving = any(abs(v) > 1e-3 for v in msg.velocity) if msg.velocity else False
+        self._mode = 'moving' if moving else 'idle'
+
+    def _on_tcp(self, msg: PoseStamped) -> None:
+        self._last_tcp = msg
+
+    def _ee(self) -> Optional[PoseStamped]:
+        # 优先用 Flexiv 发布的 TCP 位姿（真机栈上更可靠）
+        if self._last_tcp is not None:
+            return self._last_tcp
+        try:
+            tf = self._tf.lookup_transform(
+                self.base_frame, self.ee_frame, rclpy.time.Time()
+            )
+        except Exception:  # noqa: BLE001
+            return None
+        pose = PoseStamped()
+        pose.header = tf.header
+        pose.pose.position.x = tf.transform.translation.x
+        pose.pose.position.y = tf.transform.translation.y
+        pose.pose.position.z = tf.transform.translation.z
+        pose.pose.orientation = tf.transform.rotation
+        return pose
+
+    def _build(self) -> ArmStatus:
+        s = ArmStatus()
+        s.header.stamp = self.get_clock().now().to_msg()
+        s.header.frame_id = self.base_frame
+        s.mode = self._mode
+        s.is_moving = self._mode == 'moving'
+        s.is_homed = True
+        if self._last_js is not None:
+            s.joint_positions = list(self._last_js.position)
+            s.joint_velocities = list(self._last_js.velocity)
+        ee = self._ee()
+        if ee is not None:
+            s.ee_pose = ee
+        return s
+
+    def _tick(self) -> None:
+        self._pub.publish(self._build())
+
+    def _on_get(self, _req, response):
+        s = self._build()
+        response.success = True
+        response.mode = s.mode
+        response.joint_positions = s.joint_positions
+        response.ee_pose = s.ee_pose
+        if (
+            abs(s.ee_pose.pose.position.x) < 1e-9
+            and abs(s.ee_pose.pose.position.y) < 1e-9
+            and abs(s.ee_pose.pose.position.z) < 1e-9
+        ):
+            response.message = 'ok（末端位姿不可用 —— 请检查 TF/tcp_pose）'
+        else:
+            response.message = 'ok'
+        return response
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = ArmStatusNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
@@ -0,0 +1,172 @@
+#!/usr/bin/env python3
+"""Mock 7-DOF Rizon4 driver: joint_states + FollowJointTrajectory + JointCommand."""
+
+from __future__ import annotations
+
+import time
+from typing import List
+
+import rclpy
+from rclpy.action import ActionServer, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+from arm_msgs.action import FollowJointTrajectory
+from arm_msgs.msg import JointCommand
+
+DEFAULT_JOINTS = [
+    'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
+]
+
+
+class MockArmDriver(Node):
+    def __init__(self) -> None:
+        super().__init__('mock_arm_driver')
+        self.declare_parameter('joint_names', DEFAULT_JOINTS)
+        self.declare_parameter('joint_prefix', '')
+        self.declare_parameter('publish_rate', 50.0)
+        self.declare_parameter(
+            'default_positions', [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
+        )
+
+        prefix = str(self.get_parameter('joint_prefix').value or '')
+        base = list(
+            self.get_parameter('joint_names').get_parameter_value().string_array_value
+            or DEFAULT_JOINTS
+        )
+        self.joint_names: List[str] = [f'{prefix}{n}' for n in base]
+        rate = float(self.get_parameter('publish_rate').value)
+        defaults = list(
+            self.get_parameter('default_positions').get_parameter_value().double_array_value
+            or [0.0] * len(self.joint_names)
+        )
+        while len(defaults) < len(self.joint_names):
+            defaults.append(0.0)
+
+        self.positions = defaults[: len(self.joint_names)]
+        self.velocities = [0.0] * len(self.joint_names)
+        self._busy = False
+        self._cb = ReentrantCallbackGroup()
+
+        self._js_pub = self.create_publisher(JointState, 'joint_states', 10)
+        self.create_subscription(JointCommand, 'joint_command', self._on_cmd, 10)
+        self.create_timer(1.0 / rate, self._publish)
+
+        ActionServer(
+            self,
+            FollowJointTrajectory,
+            'follow_joint_trajectory',
+            execute_callback=self._execute,
+            goal_callback=self._goal_cb,
+            cancel_callback=self._cancel_cb,
+            callback_group=self._cb,
+        )
+        self.get_logger().info(f'Mock Rizon4 ready: {self.joint_names}')
+
+    def _goal_cb(self, _req):
+        if self._busy:
+            self.get_logger().warn('拒绝：上一个目标还在执行')
+            return GoalResponse.REJECT
+        return GoalResponse.ACCEPT
+
+    def _cancel_cb(self, _gh):
+        return CancelResponse.ACCEPT
+
+    def _on_cmd(self, msg: JointCommand) -> None:
+        if self._busy:
+            return
+        for i, name in enumerate(msg.joint_names):
+            if name in self.joint_names and i < len(msg.positions):
+                idx = self.joint_names.index(name)
+                self.positions[idx] = float(msg.positions[i])
+                self.velocities[idx] = 0.0
+
+    def _publish(self) -> None:
+        msg = JointState()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.name = self.joint_names
+        msg.position = list(self.positions)
+        msg.velocity = list(self.velocities)
+        self._js_pub.publish(msg)
+
+    def _execute(self, goal_handle):
+        self._busy = True
+        goal = goal_handle.request
+        result = FollowJointTrajectory.Result()
+        feedback = FollowJointTrajectory.Feedback()
+
+        n_j = len(goal.joint_names)
+        n_p = int(goal.num_points)
+        if n_j == 0 or n_p == 0 or len(goal.positions) < n_j * n_p:
+            result.success = False
+            result.message = 'Invalid / empty trajectory'
+            goal_handle.abort()
+            self._busy = False
+            return result
+
+        start = time.monotonic()
+        times = list(goal.time_from_start) if goal.time_from_start else [
+            (i + 1) * 0.5 for i in range(n_p)
+        ]
+        try:
+            for p in range(n_p):
+                if goal_handle.is_cancel_requested:
+                    goal_handle.canceled()
+                    result.success = False
+                    result.message = 'Canceled'
+                    result.final_positions = list(self.positions)
+                    self._busy = False
+                    return result
+                target = goal.positions[p * n_j:(p + 1) * n_j]
+                deadline = start + times[p]
+                while time.monotonic() < deadline:
+                    for name, tval in zip(goal.joint_names, target):
+                        if name not in self.joint_names:
+                            continue
+                        idx = self.joint_names.index(name)
+                        err = tval - self.positions[idx]
+                        self.velocities[idx] = err * 5.0
+                        self.positions[idx] += 0.15 * err
+                    feedback.current_point = p
+                    feedback.progress = (p + 1) / n_p
+                    feedback.status = 'moving'
+                    goal_handle.publish_feedback(feedback)
+                    time.sleep(0.02)
+                for name, tval in zip(goal.joint_names, target):
+                    if name in self.joint_names:
+                        self.positions[self.joint_names.index(name)] = tval
+                self.velocities = [0.0] * len(self.joint_names)
+
+            goal_handle.succeed()
+            result.success = True
+            result.message = 'Trajectory completed'
+            result.final_positions = list(self.positions)
+        except Exception as exc:  # noqa: BLE001
+            result.success = False
+            result.message = str(exc)
+            goal_handle.abort()
+        finally:
+            self._busy = False
+            self.velocities = [0.0] * len(self.joint_names)
+        return result
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = MockArmDriver()
+    executor = MultiThreadedExecutor()
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
@@ -0,0 +1,159 @@
+#!/usr/bin/env python3
+"""服务端：SetNamedPose -> FollowJointTrajectory（与底层后端无关）。"""
+
+from __future__ import annotations
+
+import time
+from typing import Dict, List
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+
+from arm_msgs.action import FollowJointTrajectory
+from arm_msgs.srv import SetNamedPose
+from std_srvs.srv import Trigger
+
+DEFAULT_JOINTS = [
+    'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
+]
+DEFAULT_POSES: Dict[str, List[float]] = {
+    'home': [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0],
+    'ready': [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0],
+    'inspect': [0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698],
+    'pos_left': [-0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698],
+}
+
+
+def _wait_future(future, timeout_sec: float) -> bool:
+    deadline = time.monotonic() + timeout_sec
+    while not future.done():
+        if time.monotonic() > deadline:
+            return False
+        time.sleep(0.01)
+    return True
+
+
+class NamedPoseServer(Node):
+    def __init__(self) -> None:
+        super().__init__('named_pose_server')
+        self.declare_parameter('joint_names', DEFAULT_JOINTS)
+        self.declare_parameter('joint_prefix', '')
+        for name, vals in DEFAULT_POSES.items():
+            self.declare_parameter(f'named_poses.{name}', vals)
+
+        prefix = str(self.get_parameter('joint_prefix').value or '')
+        base = list(
+            self.get_parameter('joint_names').get_parameter_value().string_array_value
+            or DEFAULT_JOINTS
+        )
+        self.joint_names = [f'{prefix}{n}' for n in base]
+        self.poses = {
+            name: list(self.get_parameter(f'named_poses.{name}').value)
+            for name in DEFAULT_POSES
+        }
+
+        self._cb = ReentrantCallbackGroup()
+        self._client = ActionClient(
+            self,
+            FollowJointTrajectory,
+            'follow_joint_trajectory',
+            callback_group=self._cb,
+        )
+        self.create_service(
+            SetNamedPose, 'set_named_pose', self._on_set, callback_group=self._cb
+        )
+        # ``/home``（std_srvs/Trigger）：契约里的"回初始位"入口。
+        #
+        # **为什么这里也要有一个**：真机上 ``/home`` 原本由 ``arm_driver_flexiv``
+        # 提供（它只是个包装，内部还是调 ``/set_named_pose``），但 real 模式的
+        # bringup 起的是 ``flexiv_bringup + named_pose_server + arm_status_node``，
+        # **没有任何 launch 文件会起 arm_driver_flexiv** —— 那个驱动还会自己开
+        # RDK 连接，与 ros2_control 的 flexiv_hardware 抢独占（RDK 是单客户端）。
+        # 于是真机上 ``/home`` 根本不存在，通止规装配的 S00 必检因此永远过不了
+        # （``gauge_boot.py`` 的 ``gauge_home`` 项 target=/home，
+        # ``gauge_assembly_node`` 的 ``_call_trigger("/home")`` 同）。
+        # 放在这里最合适：本节点已经握着 ``/set_named_pose`` 的实现与 JTC 客户端，
+        # 不需要再起一个进程、也不碰 RDK。
+        self.declare_parameter('home_pose', 'home')
+        self.declare_parameter('home_speed', 0.30)
+        self.create_service(
+            Trigger, 'home', self._on_home, callback_group=self._cb
+        )
+        self.get_logger().info(
+            f'命名位姿 {list(self.poses)} joints={self.joint_names}'
+        )
+
+    def _go_to_pose(self, name: str, velocity_scale: float):
+        """把 ``name`` 这个命名位姿走完。返回 ``(成功?, 说明)``。
+
+        两个服务（``/set_named_pose`` 与 ``/home``）共用这一段：分两份写的话，
+        以后只改一处就会出现"手动回位能动、装配回位不能动"这类难查的偏差。
+        """
+        if name not in self.poses:
+            return False, f'未知位姿"{name}"。可用: {list(self.poses)}'
+        if not self._client.wait_for_server(timeout_sec=3.0):
+            return False, 'follow_joint_trajectory action 不可用'
+
+        goal = FollowJointTrajectory.Goal()
+        goal.joint_names = self.joint_names
+        goal.num_points = 1
+        goal.positions = list(self.poses[name])
+        duration = max(0.5, 1.0 / max(float(velocity_scale), 0.1))
+        goal.time_from_start = [duration]
+        goal.velocity_scale = float(velocity_scale)
+
+        self.get_logger().info(f'-> 位姿 [{name}] T={duration:.2f}s')
+        send_fut = self._client.send_goal_async(goal)
+        if not _wait_future(send_fut, 5.0):
+            return False, '等待目标被接受超时'
+        handle = send_fut.result()
+        if handle is None or not handle.accepted:
+            return False, '目标被拒绝'
+        res_fut = handle.get_result_async()
+        if not _wait_future(res_fut, 60.0):
+            return False, '等待轨迹结果超时'
+        wrapped = res_fut.result()
+        if wrapped is None:
+            return False, '结果为空'
+        result = wrapped.result
+        return bool(result.success), (result.message or f'已移动到位姿 {name}')
+
+    def _on_set(self, request: SetNamedPose.Request, response: SetNamedPose.Response):
+        response.success, response.message = self._go_to_pose(
+            request.pose_name, float(request.velocity_scale)
+        )
+        return response
+
+    def _on_home(self, _request: Trigger.Request, response: Trigger.Response):
+        """``/home``：回到 ``home_pose`` 指定的命名位姿（默认 ``home``）。"""
+        pose = str(self.get_parameter('home_pose').value)
+        speed = float(self.get_parameter('home_speed').value)
+        ok, message = self._go_to_pose(pose, speed)
+        if not ok:
+            self.get_logger().error(f'回初始位失败（pose={pose}）: {message}')
+        response.success = ok
+        response.message = message
+        return response
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = NamedPoseServer()
+    executor = MultiThreadedExecutor(num_threads=4)
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        executor.shutdown()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
@@ -0,0 +1,136 @@
+#!/usr/bin/env python3
+"""对应用侧暴露 arm_msgs/FollowJointTrajectory；实际驱动 Flexiv JTC（control_msgs）。"""
+
+from __future__ import annotations
+
+import time
+
+import rclpy
+from control_msgs.action import FollowJointTrajectory as CtrlFJT
+from rclpy.action import ActionClient, ActionServer, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+from arm_msgs.action import FollowJointTrajectory as ArmFJT
+
+
+def _wait(fut, timeout: float) -> bool:
+    end = time.monotonic() + timeout
+    while not fut.done():
+        if time.monotonic() > end:
+            return False
+        time.sleep(0.01)
+    return True
+
+
+class RealTrajectoryAdapter(Node):
+    def __init__(self) -> None:
+        super().__init__('real_trajectory_adapter')
+        self.declare_parameter('arm_action', 'follow_joint_trajectory')
+        self.declare_parameter(
+            'control_action', 'rizon_arm_controller/follow_joint_trajectory'
+        )
+        arm = str(self.get_parameter('arm_action').value)
+        ctrl = str(self.get_parameter('control_action').value)
+        self._cb = ReentrantCallbackGroup()
+        self._ctrl = ActionClient(self, CtrlFJT, ctrl, callback_group=self._cb)
+        ActionServer(
+            self,
+            ArmFJT,
+            arm,
+            execute_callback=self._execute,
+            goal_callback=lambda _g: GoalResponse.ACCEPT,
+            cancel_callback=lambda _g: CancelResponse.ACCEPT,
+            callback_group=self._cb,
+        )
+        self.get_logger().info(f'真机适配器 {arm} -> {ctrl}')
+
+    def _execute(self, goal_handle):
+        result = ArmFJT.Result()
+        req = goal_handle.request
+        if not req.joint_names or req.num_points < 1:
+            result.success = False
+            result.message = '目标非法'
+            goal_handle.abort()
+            return result
+        if not self._ctrl.wait_for_server(timeout_sec=5.0):
+            result.success = False
+            result.message = f'控制器 action 不可用'
+            goal_handle.abort()
+            return result
+
+        n_j = len(req.joint_names)
+        traj = JointTrajectory()
+        traj.joint_names = list(req.joint_names)
+        pos = list(req.positions)
+        times = list(req.time_from_start) or [1.0]
+        idx = 0
+        n_pts = min(int(req.num_points), len(times))
+        for i, t in enumerate(times[:n_pts]):
+            pt = JointTrajectoryPoint()
+            pt.positions = pos[idx : idx + n_j]
+            idx += n_j
+            # 点到点的命名位姿必须在静止状态下结束 —— 末速度非零
+            # 再叠加开环 JTC，会让机械臂到位后滑行/抖动。
+            pt.velocities = [0.0] * n_j
+            pt.accelerations = [0.0] * n_j
+            sec = int(t)
+            pt.time_from_start.sec = sec
+            pt.time_from_start.nanosec = int((t - sec) * 1e9)
+            traj.points.append(pt)
+
+        ctrl_goal = CtrlFJT.Goal()
+        ctrl_goal.trajectory = traj
+        send = self._ctrl.send_goal_async(ctrl_goal)
+        if not _wait(send, 5.0):
+            result.success = False
+            result.message = '发送给控制器超时'
+            goal_handle.abort()
+            return result
+        handle = send.result()
+        if handle is None or not handle.accepted:
+            result.success = False
+            result.message = '控制器拒绝了目标'
+            goal_handle.abort()
+            return result
+        traj_duration = 0.0
+        if traj.points:
+            last = traj.points[-1].time_from_start
+            traj_duration = last.sec + last.nanosec * 1e-9
+        res_fut = handle.get_result_async()
+        if not _wait(res_fut, max(120.0, traj_duration * 3.0)):
+            result.success = False
+            result.message = '等待控制器超时'
+            goal_handle.abort()
+            return result
+        ctrl_res = res_fut.result().result
+        ok = ctrl_res.error_code == CtrlFJT.Result.SUCCESSFUL
+        result.success = ok
+        result.message = 'OK' if ok else f'控制器 error_code={ctrl_res.error_code}'
+        if ok:
+            goal_handle.succeed()
+        else:
+            goal_handle.abort()
+        return result
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = RealTrajectoryAdapter()
+    ex = MultiThreadedExecutor(num_threads=4)
+    ex.add_node(node)
+    try:
+        ex.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        ex.shutdown()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
@@ -0,0 +1,116 @@
+#!/usr/bin/env python3
+"""把 control_msgs/FollowJointTrajectory 桥接到 arm_msgs/FollowJointTrajectory。"""
+
+from __future__ import annotations
+
+import time
+
+import rclpy
+from control_msgs.action import FollowJointTrajectory as CtrlFJT
+from rclpy.action import ActionClient, ActionServer, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+
+from arm_msgs.action import FollowJointTrajectory as ArmFJT
+
+
+def _wait(fut, timeout: float) -> bool:
+    end = time.monotonic() + timeout
+    while not fut.done():
+        if time.monotonic() > end:
+            return False
+        time.sleep(0.01)
+    return True
+
+
+class TrajectoryBridge(Node):
+    def __init__(self) -> None:
+        super().__init__('trajectory_bridge')
+        self.declare_parameter(
+            'control_action', 'rizon_arm_controller/follow_joint_trajectory'
+        )
+        self.declare_parameter('arm_action', 'follow_joint_trajectory')
+        ctrl = str(self.get_parameter('control_action').value)
+        arm = str(self.get_parameter('arm_action').value)
+        self._cb = ReentrantCallbackGroup()
+        self._arm = ActionClient(self, ArmFJT, arm, callback_group=self._cb)
+        ActionServer(
+            self,
+            CtrlFJT,
+            ctrl,
+            execute_callback=self._execute,
+            goal_callback=lambda _g: GoalResponse.ACCEPT,
+            cancel_callback=lambda _g: CancelResponse.ACCEPT,
+            callback_group=self._cb,
+        )
+        self.get_logger().info(f'桥接 {ctrl} -> {arm}')
+
+    def _execute(self, goal_handle):
+        result = CtrlFJT.Result()
+        traj = goal_handle.request.trajectory
+        if not traj.joint_names or not traj.points:
+            result.error_code = CtrlFJT.Result.INVALID_GOAL
+            goal_handle.abort()
+            return result
+        if not self._arm.wait_for_server(timeout_sec=3.0):
+            result.error_code = CtrlFJT.Result.INVALID_GOAL
+            goal_handle.abort()
+            return result
+
+        n_j = len(traj.joint_names)
+        goal = ArmFJT.Goal()
+        goal.joint_names = list(traj.joint_names)
+        goal.num_points = len(traj.points)
+        goal.velocity_scale = 1.0
+        positions = []
+        times = []
+        for pt in traj.points:
+            positions.extend(list(pt.positions)[:n_j])
+            t = pt.time_from_start.sec + pt.time_from_start.nanosec * 1e-9
+            times.append(max(t, 0.05))
+        goal.positions = positions
+        goal.time_from_start = times
+
+        send = self._arm.send_goal_async(goal)
+        if not _wait(send, 5.0):
+            result.error_code = CtrlFJT.Result.GOAL_TOLERANCE_VIOLATED
+            goal_handle.abort()
+            return result
+        handle = send.result()
+        if handle is None or not handle.accepted:
+            result.error_code = CtrlFJT.Result.INVALID_GOAL
+            goal_handle.abort()
+            return result
+        res_fut = handle.get_result_async()
+        if not _wait(res_fut, 120.0):
+            result.error_code = CtrlFJT.Result.GOAL_TOLERANCE_VIOLATED
+            goal_handle.abort()
+            return result
+        if res_fut.result().result.success:
+            result.error_code = CtrlFJT.Result.SUCCESSFUL
+            goal_handle.succeed()
+        else:
+            result.error_code = CtrlFJT.Result.GOAL_TOLERANCE_VIOLATED
+            goal_handle.abort()
+        return result
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = TrajectoryBridge()
+    ex = MultiThreadedExecutor(num_threads=4)
+    ex.add_node(node)
+    try:
+        ex.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        ex.shutdown()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()