feat: add peripherals control

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"),