From fe3d80cd86abe196d0d819a22b6064959ea3eaa0 Mon Sep 17 00:00:00 2001 From: YikaiFu-cart Date: Tue, 26 May 2026 18:16:08 +0800 Subject: [PATCH] feat: add peripherals control --- README.md | 18 ++ xr_rm_bringup/config/dual_arm_rm75.yaml | 1 + xr_rm_bringup/config/left_arm_rm75.yaml | 3 +- xr_rm_bringup/config/peripherals_rm75.yaml | 29 +++ xr_rm_bringup/config/right_arm_rm75.yaml | 3 +- xr_rm_bringup/launch/arm_debug.launch.py | 38 +++ xr_rm_bringup/tools/launcher_ui.py | 10 + xr_rm_teleop/package.xml | 1 + xr_rm_teleop/xr_rm_teleop/fun_peripheral.py | 240 ++++++++++++++++++ xr_rm_teleop/xr_rm_teleop/realman_adapter.py | 37 +++ .../single_arm_velocity_teleop.py | 43 ++++ 11 files changed, 421 insertions(+), 2 deletions(-) create mode 100644 xr_rm_bringup/config/peripherals_rm75.yaml create mode 100644 xr_rm_teleop/xr_rm_teleop/fun_peripheral.py 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()