ZhengLiu-cart/acRealman_xr_from_Yikai
1
0
Fork 0
forked from YikaiFu-cart/acRealman_xr
Code Pull Requests Activity

feat: add peripherals control

Browse Source
This commit is contained in:
YikaiFu-cart
2026-05-26 18:16:08 +08:00
parent f137e28ed7
commit fe3d80cd86
11 changed files with 421 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
xr_rm_bringup/config/dual_arm_rm75.yaml
View File

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

3
xr_rm_bringup/config/left_arm_rm75.yaml
View File

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

29
xr_rm_bringup/config/peripherals_rm75.yaml Normal file
View File

@ -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

3
xr_rm_bringup/config/right_arm_rm75.yaml
View File

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

38
xr_rm_bringup/launch/arm_debug.launch.py
View File

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

10
xr_rm_bringup/tools/launcher_ui.py
View File

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