diff --git a/README.md b/README.md
index 1f5d1ec..0baf465 100755
--- a/README.md
+++ b/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`:订阅的手柄话题。
diff --git a/xr_rm_bringup/config/dual_arm_rm75.yaml b/xr_rm_bringup/config/dual_arm_rm75.yaml
index 0ee7b8d..39b82b2 100755
--- a/xr_rm_bringup/config/dual_arm_rm75.yaml
+++ b/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:
diff --git a/xr_rm_bringup/config/left_arm_rm75.yaml b/xr_rm_bringup/config/left_arm_rm75.yaml
index 152a1c7..90ef2a5 100644
--- a/xr_rm_bringup/config/left_arm_rm75.yaml
+++ b/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:
diff --git a/xr_rm_bringup/config/peripherals_rm75.yaml b/xr_rm_bringup/config/peripherals_rm75.yaml
new file mode 100644
index 0000000..59f9eaa
--- /dev/null
+++ b/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
diff --git a/xr_rm_bringup/config/right_arm_rm75.yaml b/xr_rm_bringup/config/right_arm_rm75.yaml
index 26f87a8..3cb19d8 100644
--- a/xr_rm_bringup/config/right_arm_rm75.yaml
+++ b/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:
diff --git a/xr_rm_bringup/launch/arm_debug.launch.py b/xr_rm_bringup/launch/arm_debug.launch.py
index 1e2a735..fbe23fe 100644
--- a/xr_rm_bringup/launch/arm_debug.launch.py
+++ b/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 等运行时参数动态生成节点。
diff --git a/xr_rm_bringup/tools/launcher_ui.py b/xr_rm_bringup/tools/launcher_ui.py
index 1063d8f..9b8d61f 100755
--- a/xr_rm_bringup/tools/launcher_ui.py
+++ b/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"),
diff --git a/xr_rm_teleop/package.xml b/xr_rm_teleop/package.xml
index ef25793..63210d1 100755
--- a/xr_rm_teleop/package.xml
+++ b/xr_rm_teleop/package.xml
@@ -11,6 +11,7 @@
geometry_msgs
rclpy
+ python3-yaml
std_msgs
xr_rm_interfaces
diff --git a/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py b/xr_rm_teleop/xr_rm_teleop/fun_peripheral.py
new file mode 100644
index 0000000..2462fd9
--- /dev/null
+++ b/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)
diff --git a/xr_rm_teleop/xr_rm_teleop/realman_adapter.py b/xr_rm_teleop/xr_rm_teleop/realman_adapter.py
index 250fcec..8a4f142 100755
--- a/xr_rm_teleop/xr_rm_teleop/realman_adapter.py
+++ b/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
diff --git a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py b/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
index ca13be7..7c568f7 100755
--- a/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
+++ b/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()