feat: add peripherals control

README.md

@@ -186,6 +186,8 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 - `left_robot_ip`：左臂 IP，默认 `192.168.192.18`。
 - `right_robot_ip`：右臂 IP，默认 `192.168.192.19`。
 - `robot_port`：RM75 TCP 端口，默认 `8080`。
+- `enable_tool_control`：是否在遥操作节点内启用末端工具控制 topic，默认 `true`。
+- `configure_peripheral_on_connect`：遥操作节点连接真机后是否配置末端外设，默认 `true`；工具控制会复用同一个 RealMan 连接，避免两个进程同时抢占同一机械臂。
 - `move_to_initial_pose_on_connect`：连接后是否执行 `movej`/`movel` 初始化，默认 `false`。
 
 ## 配置文件说明
@@ -197,6 +199,22 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
 
 `left_arm_rm75.yaml` 和 `right_arm_rm75.yaml` 用于 `arm_debug.launch.py arm:=left/right` 的单臂调试，因为单臂节点名是 `single_arm_velocity_teleop`。
 
+`xr_rm_bringup/config/peripherals_rm75.yaml` 保存末端工具坐标、负载和左右臂外设选择。当前配置为左臂 `scissorgripper=1`、右臂 `scissorgripper=2`，并且只在真机连接阶段初始化外设，不在主遥操作循环里控制开合。
+
+## 末端工具开合
+
+真机 launch 默认会在遥操作节点内启用工具控制。启动后可以用 Bool 话题控制开合，`true` 表示打开，`false` 表示闭合：
+
+```bash
+ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
+ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: false}"
+
+ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
+ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: false}"
+```
+
+桌面 UI 的 `Left Arm`、`Right Arm` 和 `Dual Arm` 模式里也有对应的 Tool Open/Close 命令项。
+
 重点参数：
 
 - `controller_topic`：订阅的手柄话题。

xr_rm_bringup/config/dual_arm_rm75.yaml

@@ -10,6 +10,7 @@
 # 按下握持键时锁定当前手柄位姿和 TCP 位姿，之后只跟随相对位移。
 # acDual-arm 项目使用的是戴盟绝对 PoseStamped 重映射，因此这里只迁移
 # 坐标标定和安全参数，不迁移其绝对位姿控制链路。
+# 末端外设由 peripherals_rm75.yaml 配置，launch 只在真机连接阶段初始化。
 
 left_arm_teleop:
   ros__parameters:

xr_rm_bringup/config/left_arm_rm75.yaml

@@ -1,4 +1,5 @@
-# 左臂单独调试配置：无末端执行器，仅 XR 相对位移控制 RM75 TCP。
+# 左臂单独调试配置：XR 相对位移控制 RM75 TCP。
+# 末端外设由 peripherals_rm75.yaml 配置，launch 只在真机连接阶段初始化。
 
 single_arm_velocity_teleop:
   ros__parameters:

xr_rm_bringup/config/peripherals_rm75.yaml

@@ -0,0 +1,29 @@
+# RM75 末端外设配置。
+#
+# scissorgripper 与 tools_in_ee 的顺序保持和 acRealman 一致：
+# 0 -> scissor，1 -> omnipic，2 -> minisci，-1 -> no_tool。
+# 当前阶段只做连接后的外设初始化，不在遥操作主循环中控制开合。
+
+set_initial_tool_state: false
+
+tools_in_ee:
+  scissor:
+    # x, y, z, qx, qy, qz, qw
+    pose: [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
+    # mass, center_x, center_y, center_z, reserved...
+    load: [0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]
+  omnipic:
+    pose: [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0]
+    load: [0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]
+  minisci:
+    pose: [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
+    load: [0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]
+  no_tool:
+    pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+    load: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+
+arms:
+  left:
+    scissorgripper: 2
+  right:
+    scissorgripper: 1

xr_rm_bringup/config/right_arm_rm75.yaml

@@ -1,4 +1,5 @@
-# 右臂单独调试配置：无末端执行器，仅 XR 相对位移控制 RM75 TCP。
+# 右臂单独调试配置：XR 相对位移控制 RM75 TCP。
+# 末端外设由 peripherals_rm75.yaml 配置，launch 只在真机连接阶段初始化。
 
 single_arm_velocity_teleop:
   ros__parameters:

xr_rm_bringup/launch/arm_debug.launch.py

@@ -49,10 +49,13 @@ def _single_arm_node(
     avoid_singularity: int,
     frame_type: int,
     configure_safety_limits: bool,
+    enable_tool_control: bool,
+    configure_peripheral_on_connect: bool,
 ) -> Node:
     """创建单臂调试节点；左/右臂分别使用独立 YAML，节点名保持单臂默认名。"""
     config_name = "left_arm_rm75.yaml" if arm == "left" else "right_arm_rm75.yaml"
     robot_ip = LaunchConfiguration("left_robot_ip" if arm == "left" else "right_robot_ip")
+    arm_name = _arm_name(arm)
     return Node(
         package="xr_rm_teleop",
         executable="single_arm_velocity_teleop",
@@ -68,11 +71,20 @@ def _single_arm_node(
                 "frame_type": frame_type,
                 "configure_safety_limits": configure_safety_limits,
                 "move_to_initial_pose_on_connect": move_to_initial_pose,
+                "enable_tool_control": enable_tool_control,
+                "configure_peripheral_on_connect": configure_peripheral_on_connect,
+                "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
+                "peripheral_arm": arm,
+                "tool_command_topic": f"/xr_rm/{arm_name}/tool_enable",
             },
         ],
     )
 
 
+def _arm_name(arm: str) -> str:
+    return "left_rm75" if arm == "left" else "right_rm75"
+
+
 def _dual_arm_nodes(
     use_mock: bool,
     move_to_initial_pose: bool,
@@ -80,6 +92,8 @@ def _dual_arm_nodes(
     right_avoid_singularity: int,
     frame_type: int,
     configure_safety_limits: bool,
+    enable_tool_control: bool,
+    configure_peripheral_on_connect: bool,
 ) -> list[Node]:
     """创建双臂节点；两个节点共用双臂 YAML，但节点名区分左右臂参数命名空间。"""
     config_file = _config_file("dual_arm_rm75.yaml")
@@ -99,6 +113,11 @@ def _dual_arm_nodes(
                     "frame_type": frame_type,
                     "configure_safety_limits": configure_safety_limits,
                     "move_to_initial_pose_on_connect": move_to_initial_pose,
+                    "enable_tool_control": enable_tool_control,
+                    "configure_peripheral_on_connect": configure_peripheral_on_connect,
+                    "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
+                    "peripheral_arm": "left",
+                    "tool_command_topic": "/xr_rm/left_rm75/tool_enable",
                 },
             ],
         ),
@@ -117,6 +136,11 @@ def _dual_arm_nodes(
                     "frame_type": frame_type,
                     "configure_safety_limits": configure_safety_limits,
                     "move_to_initial_pose_on_connect": move_to_initial_pose,
+                    "enable_tool_control": enable_tool_control,
+                    "configure_peripheral_on_connect": configure_peripheral_on_connect,
+                    "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
+                    "peripheral_arm": "right",
+                    "tool_command_topic": "/xr_rm/right_rm75/tool_enable",
                 },
             ],
         ),
@@ -142,6 +166,12 @@ def _launch_setup(context, *args, **kwargs):
     configure_safety_limits = _as_bool(
         LaunchConfiguration("configure_safety_limits").perform(context)
     )
+    configure_peripheral_on_connect = _as_bool(
+        LaunchConfiguration("configure_peripheral_on_connect").perform(context)
+    )
+    enable_tool_control = _as_bool(
+        LaunchConfiguration("enable_tool_control").perform(context)
+    )
 
     if arm not in ("left", "right", "both"):
         raise ValueError("arm must be one of: left, right, both")
@@ -156,6 +186,8 @@ def _launch_setup(context, *args, **kwargs):
                 right_avoid_singularity,
                 frame_type,
                 configure_safety_limits,
+                enable_tool_control,
+                configure_peripheral_on_connect,
             )
         )
     else:
@@ -168,6 +200,8 @@ def _launch_setup(context, *args, **kwargs):
                 avoid_singularity,
                 frame_type,
                 configure_safety_limits,
+                enable_tool_control,
+                configure_peripheral_on_connect,
             )
         )
     return nodes
@@ -194,6 +228,10 @@ def generate_launch_description() -> LaunchDescription:
         DeclareLaunchArgument("avoid_singularity", default_value=""),
         DeclareLaunchArgument("frame_type", default_value="1"),
         DeclareLaunchArgument("configure_safety_limits", default_value="true"),
+        # 工具控制通过遥操作节点复用同一个 RealMan 连接，避免两个进程抢同一机械臂连接。
+        DeclareLaunchArgument("enable_tool_control", default_value="true"),
+        # 连接成功后是否配置外设；关闭后仅订阅开合话题，但开合前需要另行完成外设配置。
+        DeclareLaunchArgument("configure_peripheral_on_connect", default_value="true"),
         # 默认不自动移动到初始点，确认安全区后再显式打开。
         DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"),
         # OpaqueFunction 允许根据 arm/use_mock 等运行时参数动态生成节点。

xr_rm_bringup/tools/launcher_ui.py

@@ -96,6 +96,12 @@ def _sample_udp_sender_command(
     return command
 
 
+def _tool_command(arm: str, open_tool: bool) -> str:
+    arm_name = "left_rm75" if arm == "left" else "right_rm75"
+    value = "true" if open_tool else "false"
+    return f"ros2 topic pub --once /xr_rm/{arm_name}/tool_enable std_msgs/msg/Bool '{{data: {value}}}'"
+
+
 def _find_workspace_root() -> Path:
     # 优先使用显式环境变量，便于从安装目录、源码目录或桌面快捷方式启动。
     env_workspace = os.environ.get("XR_RM_WS")
@@ -232,6 +238,8 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
                 f"left_robot_ip:={DEFAULT_LEFT_IP} "
                 "move_to_initial_pose_on_connect:=false",
             ),
+            ("Left Tool Open", _tool_command("left", True)),
+            ("Left Tool Close", _tool_command("left", False)),
             (
                 "Sample UDP Sender (Left, 30s)",
                 _sample_udp_sender_command("left"),
@@ -247,6 +255,8 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
                 f"right_robot_ip:={DEFAULT_RIGHT_IP} "
                 "move_to_initial_pose_on_connect:=false",
             ),
+            ("Right Tool Open", _tool_command("right", True)),
+            ("Right Tool Close", _tool_command("right", False)),
             (
                 "Sample UDP Sender (Right, 30s)",
                 _sample_udp_sender_command("right"),

xr_rm_teleop/package.xml

@@ -11,6 +11,7 @@
 
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>rclpy</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>xr_rm_interfaces</exec_depend>
 

xr_rm_teleop/xr_rm_teleop/fun_peripheral.py

@@ -0,0 +1,240 @@
+"""RealMan 机械臂末端外设配置工具。
+
+scissorgripper 参数约定：
+0：工具板 IO 控制的剪刀夹爪
+1：Modbus 控制的电动夹爪
+2：控制器 DO3/DO4 控制的小型剪刀夹爪
+
+当前 XR 工程只在真机连接阶段做外设初始化；主遥操作流程不调用开合命令。
+"""
+
+from __future__ import annotations
+
+import time
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any
+
+import yaml
+
+
+@dataclass(frozen=True)
+class PeripheralConfig:
+    scissorgripper: int
+    tools_in_ee: dict[str, list[list[float]]]
+    set_initial_tool_state: bool = False
+
+
+def load_peripheral_config(config_file: str, arm: str) -> PeripheralConfig:
+    """从 bringup YAML 读取指定左右臂的外设配置。"""
+    path = Path(config_file).expanduser()
+    if not path.is_file():
+        raise FileNotFoundError(f"外设配置文件不存在：{path}")
+
+    with path.open("r", encoding="utf-8") as stream:
+        data = yaml.safe_load(stream) or {}
+    if not isinstance(data, dict):
+        raise ValueError(f"外设配置文件格式错误：{path}")
+
+    tools_in_ee = _load_tools_in_ee(data.get("tools_in_ee"), path)
+    arm_configs = data.get("arms", {})
+    if not isinstance(arm_configs, dict) or arm not in arm_configs:
+        raise ValueError(f"{path} 中缺少 arms.{arm} 外设配置")
+
+    arm_config = arm_configs[arm]
+    if not isinstance(arm_config, dict):
+        raise ValueError(f"{path} 中 arms.{arm} 必须是映射")
+
+    scissorgripper = int(arm_config["scissorgripper"])
+    tool_count = len(tools_in_ee)
+    if not -tool_count <= scissorgripper < tool_count:
+        raise ValueError(
+            f"arms.{arm}.scissorgripper={scissorgripper} 超出工具列表范围"
+        )
+
+    set_initial_tool_state = _as_bool(
+        arm_config.get(
+            "set_initial_tool_state",
+            data.get("set_initial_tool_state", False),
+        )
+    )
+    return PeripheralConfig(scissorgripper, tools_in_ee, set_initial_tool_state)
+
+
+def _load_tools_in_ee(raw_tools: Any, path: Path) -> dict[str, list[list[float]]]:
+    if not isinstance(raw_tools, dict) or not raw_tools:
+        raise ValueError(f"{path} 中缺少 tools_in_ee 配置")
+
+    tools_in_ee: dict[str, list[list[float]]] = {}
+    for tool_name, tool_config in raw_tools.items():
+        if not isinstance(tool_config, dict):
+            raise ValueError(f"tools_in_ee.{tool_name} 必须是映射")
+        pose = _float_list(tool_config.get("pose"), 7, f"tools_in_ee.{tool_name}.pose")
+        load = _float_list(tool_config.get("load"), 7, f"tools_in_ee.{tool_name}.load")
+        tools_in_ee[str(tool_name)] = [pose, load]
+    return tools_in_ee
+
+
+def _float_list(value: Any, expected_length: int, name: str) -> list[float]:
+    if not isinstance(value, (list, tuple)) or len(value) != expected_length:
+        raise ValueError(f"{name} 必须包含 {expected_length} 个数值")
+    return [float(item) for item in value]
+
+
+def _as_bool(value: Any) -> bool:
+    if isinstance(value, bool):
+        return value
+    if isinstance(value, str):
+        return value.strip().lower() in ("1", "true", "yes", "on")
+    return bool(value)
+
+
+def _tool_name_for_index(tools_in_ee: dict[str, list[list[float]]], scissorgripper: int) -> str:
+    return list(tools_in_ee.keys())[scissorgripper]
+
+
+def cal_tool_frame(handle, scissorgripper, tools_in_ee):
+    """根据末端工具配置生成 RealMan 工具坐标系。"""
+    from Robotic_Arm.rm_robot_interface import rm_frame_t
+
+    tool_name = _tool_name_for_index(tools_in_ee, scissorgripper)
+    tool_pose = tools_in_ee[tool_name][0]
+    tool_load = tools_in_ee[tool_name][1]
+    eu = handle.rm_algo_quaternion2euler([tool_pose[6], tool_pose[3], tool_pose[4], tool_pose[5]])
+    tool_frame = rm_frame_t(tool_name, [tool_pose[0], tool_pose[1], tool_pose[2], eu[0], eu[1], eu[2]], tool_load[0],
+                            tool_load[1], tool_load[2], tool_load[3])
+    return tool_frame, tool_name
+
+
+def alg_init(scissorgripper, tools_in_ee):
+    """初始化算法模型、工具坐标系和关节运动限制。"""
+    from Robotic_Arm.rm_robot_interface import Algo, rm_force_type_e, rm_robot_arm_model_e
+
+    arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E
+    force_type = rm_force_type_e.RM_MODEL_RM_B_E
+    algo_handle = Algo(arm_model, force_type)
+    tool_frame, tool_name = cal_tool_frame(algo_handle, scissorgripper, tools_in_ee)
+    algo_handle.rm_algo_set_toolframe(tool_frame)
+
+    algo_handle.rm_algo_set_joint_max_limit([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0])
+    algo_handle.rm_algo_set_joint_min_limit([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0])
+    algo_handle.rm_algo_set_joint_max_speed([60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0])
+    algo_handle.rm_algo_set_joint_max_acc([
+        166.6666717529297,
+        166.6666717529297,
+        166.6666717529297,
+        166.6666717529297,
+        166.6666717529297,
+        166.6666717529297,
+        166.6666717529297,
+    ])
+    algo_handle.rm_algo_set_redundant_parameter_traversal_mode(False)
+
+    return algo_handle
+
+
+def peripheral_cfg(
+    robot,
+    scissorgripper,
+    tools_in_ee,
+    *,
+    set_initial_tool_state: bool = False,
+):
+    """配置机械臂控制器、当前工具坐标系和末端工具通信方式。"""
+    # 控制器输出 12V；IO1/IO2 配置为输入，IO3/IO4 配置为输出。
+    robot.rm_set_voltage(2, True)
+    time.sleep(0.2)
+    robot.rm_set_io_mode(1, 0, 50, 2)
+    robot.rm_set_io_mode(2, 0, 50, 2)
+    robot.rm_set_io_mode(3, 1, 50, 2)
+    robot.rm_set_io_mode(4, 1, 50, 2)
+
+    time.sleep(0.2)
+    tool_frame, tool_name = cal_tool_frame(robot, scissorgripper, tools_in_ee)
+    robot.rm_set_manual_tool_frame(frame=tool_frame)
+    robot.rm_change_tool_frame(tool_name)
+
+    if scissorgripper == 0:
+        # 剪刀夹爪通过工具板数字输出控制。
+        robot.rm_set_tool_voltage(voltage_type=2)
+        time.sleep(0.2)
+        robot.rm_set_tool_IO_mode(1, 1)
+        robot.rm_set_tool_IO_mode(2, 1)
+
+        if set_initial_tool_state:
+            robot.rm_set_tool_do_state(1, 0)
+            robot.rm_set_tool_do_state(2, 1)
+    elif scissorgripper == 1:
+        # 电动夹爪通过工具板 Modbus 寄存器控制。
+        from Robotic_Arm.rm_robot_interface import rm_peripheral_read_write_params_t
+
+        modbus_sts = robot.rm_set_modbus_mode(port=1, baudrate=115200, timeout=2)
+        if modbus_sts != 0:
+            print(f"警告：Modbus 模式设置失败：{modbus_sts}")
+        else:
+            print("Modbus 模式配置成功")
+
+        addr = 1
+        # 依次设置目标速度、目标力矩、目标加速度和目标减速度。
+        reg_value = [255, 60, 255, 255]
+        for i, reg_addr in enumerate([11, 12, 13, 14]):
+            write_params = rm_peripheral_read_write_params_t(1, reg_addr, addr, 1)
+            robot.rm_write_single_register(write_params, reg_value[i])
+            time.sleep(0.5)
+
+        if set_initial_tool_state:
+            set_tool_position(robot, percent=0.75, device=1, scissorgripper=scissorgripper)
+            time.sleep(1.5)
+            set_tool_position(robot, percent=0.15, device=1, scissorgripper=scissorgripper)
+
+    elif scissorgripper == 2:
+        # 小型剪刀夹爪通过控制器 DO3/DO4 控制。
+        if set_initial_tool_state:
+            robot.rm_set_do_state(io_num=3, state=1)
+            robot.rm_set_do_state(io_num=4, state=0)
+            time.sleep(2)
+            robot.rm_set_do_state(io_num=3, state=0)
+            robot.rm_set_do_state(io_num=4, state=1)
+
+    return robot
+
+
+def set_tool_position(robot, percent, device=1, scissorgripper=0):
+    """设置末端工具开合位置：0.0 表示闭合，1.0 表示完全张开。"""
+    if scissorgripper == 1:
+        from Robotic_Arm.rm_robot_interface import rm_peripheral_read_write_params_t
+
+        pos_value = int(percent * 255)
+        write_params = rm_peripheral_read_write_params_t(1, 10, device, 1)
+        robot.rm_write_single_register(write_params, pos_value)
+        # 写触发寄存器后夹爪才会执行目标位置。
+        write_params = rm_peripheral_read_write_params_t(1, 15, device, 1)
+        time.sleep(0.5)
+        robot.rm_write_single_register(write_params, 0x01)
+        time.sleep(0.5)
+
+    elif scissorgripper == 0:
+        # 剪刀夹爪只有全闭合和全张开两个状态。
+        if percent == 0:
+            robot.rm_set_tool_do_state(1, 1)
+            robot.rm_set_tool_do_state(2, 0)
+        elif percent == 1:
+            robot.rm_set_tool_do_state(1, 0)
+            robot.rm_set_tool_do_state(2, 1)
+    elif scissorgripper == 2:
+        # 小型剪刀夹爪同样只有全闭合和全张开两个状态。
+        if percent == 0:
+            robot.rm_set_do_state(3, 1)
+            robot.rm_set_do_state(4, 0)
+        elif percent == 1:
+            robot.rm_set_do_state(3, 0)
+            robot.rm_set_do_state(4, 1)
+
+
+def tool_exe(robot, scissorgripper, cmd):
+    """根据布尔命令执行末端工具开合。"""
+    switch_state = cmd
+    if switch_state:
+        set_tool_position(robot, 1, device=1, scissorgripper=scissorgripper)
+    else:
+        set_tool_position(robot, 0, device=1, scissorgripper=scissorgripper)

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -32,6 +32,7 @@ class MockRealManAdapter:
         self._pose = ArmPose(*initial_pose[:6])
         self._dt = dt
         self.last_velocity = [0.0] * 6
+        self.last_tool_open: bool | None = None
 
     def connect(self) -> None:
         return
@@ -56,6 +57,12 @@ class MockRealManAdapter:
     def close(self) -> None:
         self.stop()
 
+    def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
+        del config_file, peripheral_arm
+
+    def set_tool_enabled(self, open_tool: bool) -> None:
+        self.last_tool_open = open_tool
+
 
 class RealManAdapter:
     """睿尔曼 Python API2 的笛卡尔速度透传适配层。"""
@@ -103,6 +110,7 @@ class RealManAdapter:
         self._command_mode = command_mode
         self._canfd_trajectory_mode = canfd_trajectory_mode
         self._canfd_radio = canfd_radio
+        self._scissorgripper: int | None = None
         self._arm: Any | None = None
         if self._command_mode not in ("velocity", "pose_canfd"):
             raise ValueError("command_mode must be one of: velocity, pose_canfd")
@@ -164,6 +172,35 @@ class RealManAdapter:
     def uses_pose_targets(self) -> bool:
         return self._command_mode == "pose_canfd"
 
+    def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
+        self._require_arm()
+        from .fun_peripheral import load_peripheral_config, peripheral_cfg
+
+        config = load_peripheral_config(config_file, peripheral_arm)
+        self._scissorgripper = config.scissorgripper
+        tool_name = list(config.tools_in_ee.keys())[config.scissorgripper]
+        self._log_info(
+            "开始配置 RealMan 末端外设："
+            f"arm={peripheral_arm}, scissorgripper={config.scissorgripper}, "
+            f"tool={tool_name}, set_initial_tool_state={config.set_initial_tool_state}"
+        )
+        peripheral_cfg(
+            self._arm,
+            config.scissorgripper,
+            config.tools_in_ee,
+            set_initial_tool_state=config.set_initial_tool_state,
+        )
+        self._log_info(f"RealMan 末端外设配置完成：arm={peripheral_arm}, tool={tool_name}")
+
+    def set_tool_enabled(self, open_tool: bool) -> None:
+        self._require_arm()
+        if self._scissorgripper is None:
+            raise RuntimeError("末端外设尚未初始化，无法执行开合命令")
+
+        from .fun_peripheral import tool_exe
+
+        tool_exe(self._arm, self._scissorgripper, open_tool)
+
     def stop(self) -> None:
         if self._arm is None:
             return

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -12,6 +12,7 @@ from typing import Iterable
 import rclpy
 from geometry_msgs.msg import PoseStamped, TwistStamped
 from rclpy.node import Node
+from std_msgs.msg import Bool
 
 from xr_rm_interfaces.msg import XrController
 
@@ -70,6 +71,11 @@ class SingleArmVelocityTeleop(Node):
         self.declare_parameter("command_mode", "velocity")
         self.declare_parameter("canfd_trajectory_mode", 2)
         self.declare_parameter("canfd_radio", 0)
+        self.declare_parameter("enable_tool_control", True)
+        self.declare_parameter("configure_peripheral_on_connect", True)
+        self.declare_parameter("peripheral_config_file", "")
+        self.declare_parameter("peripheral_arm", "")
+        self.declare_parameter("tool_command_topic", "")
         self.declare_parameter("debug_topic_prefix", "/xr_rm")
 
         self._arm_name = str(self.get_parameter("arm_name").value)
@@ -107,6 +113,7 @@ class SingleArmVelocityTeleop(Node):
 
         self._adapter = self._make_adapter()
         self._adapter.connect()
+        self._setup_tool_control()
 
         debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
         self._current_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/current_pose", 10)
@@ -148,6 +155,42 @@ class SingleArmVelocityTeleop(Node):
             canfd_radio=int(self.get_parameter("canfd_radio").value),
         )
 
+    def _setup_tool_control(self) -> None:
+        if not self._bool_parameter("enable_tool_control"):
+            return
+
+        peripheral_arm = self._peripheral_arm_name()
+        config_file = str(self.get_parameter("peripheral_config_file").value)
+        if self._bool_parameter("configure_peripheral_on_connect"):
+            self._adapter.configure_peripheral(config_file, peripheral_arm)
+
+        topic = str(self.get_parameter("tool_command_topic").value).strip()
+        if not topic:
+            topic = f"{self._debug_topic_prefix}/{self._arm_name}/tool_enable"
+        self.create_subscription(Bool, topic, self._on_tool_command, 10)
+        self.get_logger().info(f"{self._arm_name} 工具控制已启用，监听：{topic} (true=open, false=close)")
+
+    def _on_tool_command(self, msg: Bool) -> None:
+        open_tool = bool(msg.data)
+        action = "open" if open_tool else "close"
+        try:
+            self._adapter.set_tool_enabled(open_tool)
+        except Exception as exc:
+            self.get_logger().error(f"{self._arm_name} tool {action} failed: {exc}")
+            return
+        self.get_logger().info(f"{self._arm_name} tool {action} command sent")
+
+    def _peripheral_arm_name(self) -> str:
+        configured = str(self.get_parameter("peripheral_arm").value).strip().lower()
+        if configured:
+            return configured
+        arm_name = self._arm_name.lower()
+        if arm_name.startswith("left"):
+            return "left"
+        if arm_name.startswith("right"):
+            return "right"
+        return arm_name
+
     def _on_controller(self, msg: XrController) -> None:
         self._last_msg = msg
         self._last_msg_time = self.get_clock().now()