feat: add peripherals control

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 等运行时参数动态生成节点。