YikaiFu-cart/acRealman_xr
Template
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add configuration files and launch scripts for left and right arm RM75 teleoperation

Browse Source
This commit is contained in:
YikaiFu-cart
2026-05-21 17:25:59 +08:00
parent 5a48619599
commit 4dea1ae530
16 changed files with 1576 additions and 122 deletions
Download Patch File Download Diff File Expand all files Collapse all files

81
README.md
View File

@ -6,7 +6,6 @@
PICO 4 Ultra 双手柄 UDP 位姿/按键 PICO 4 Ultra 双手柄 UDP 位姿/按键
-> /xr/left_controller 与 /xr/right_controller -> /xr/left_controller 与 /xr/right_controller
-> 左右 RM75 相对位移速度遥操 -> 左右 RM75 相对位移速度遥操
-> 右手 trigger 输出 OmniPicker 归一化力控指令
-> PC 端查看 D405/D405/D435 视频，后续再接记录节点 -> PC 端查看 D405/D405/D435 视频，后续再接记录节点
``` ```
@ -14,8 +13,8 @@ PICO 4 Ultra 双手柄 UDP 位姿/按键
## 当前范围 ## 当前范围
- 已纳入：PICO 双手柄输入、左右 RM75 相对位移控制、右手 OmniPicker trigger 指令出口。 - 已纳入：PICO 双手柄输入、左右 RM75 相对位移控制、左右臂独立/双臂调试 launch。
- 暂不纳入：AUBO 移动底盘、升降柱、剪刀手、XR 视频流、自动采摘算法。 - 暂不纳入：末端执行器、AUBO 移动底盘、升降柱、XR 视频流、自动采摘算法。
## 关键配置来源 ## 关键配置来源
@ -24,8 +23,10 @@ PICO 4 Ultra 双手柄 UDP 位姿/按键
- 左臂 IP：`192.168.192.18` - 左臂 IP：`192.168.192.18`
- 右臂 IP：`192.168.192.19` - 右臂 IP：`192.168.192.19`
- 左右臂 TCP 端口：`8080` - 左右臂 TCP 端口：`8080`
- 左右臂工作空间：`x[-0.50, 0.50] y[-0.60, -0.20] z[0.10, 0.50]` - 左臂工作空间：`x[-0.50, 0.50] y[-0.60, -0.20] z[0.10, 0.50]`
- 左右臂工作空间和 IP 沿用最新 acDual-arm 配置。 - 右臂工作空间：`x[-0.70, 0.50] y[-0.60, 0.40] z[0.10, 0.70]`
- 圆柱半径限制：左右臂均为 `r[0.20, 0.60]`，低高度区域最小半径收紧到 `0.21`
- 左右臂初始化关节/末端点位、工作空间和 IP 沿用最新 acRealman 配置。
`dual_arm_rm75.yaml` 里的左右 `xr_to_robot_matrix` 依据论文中的 OpenXR/PICO 手柄坐标和现场双臂 base 坐标图设置： `dual_arm_rm75.yaml` 里的左右 `xr_to_robot_matrix` 依据论文中的 OpenXR/PICO 手柄坐标和现场双臂 base 坐标图设置：
@ -47,9 +48,25 @@ colcon build --symlink-install
source install/setup.bash source install/setup.bash
``` ```
## UI 启动面板
项目内置了一个类似 acRealman 的 Tkinter 启动面板，包含仿真、左臂真机、右臂真机、双臂真机、sample UDP 发送、常用 topic 检查和清理按钮。
从工作空间根目录直接运行源码脚本：
```bash
python3 src/xr_rm_bringup/tools/launcher_ui.py
```
构建并 `source install/setup.bash` 后，也可以通过 ROS2 入口运行：
```bash
ros2 run xr_rm_bringup launcher_ui
```
## 优先验证：双臂 mock ## 优先验证：双臂 mock
mock 模式不会连接真实 RM75，适合先验证 UDP 输入、左右手柄路由、相对位移控制和 trigger 夹爪指令。 mock 模式不会连接真实 RM75，适合先验证 UDP 输入、左右手柄路由和相对位移控制。
终端 1： 终端 1：
@ -72,7 +89,16 @@ ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000
```bash ```bash
ros2 topic echo /xr/left_controller ros2 topic echo /xr/left_controller
ros2 topic echo /xr/right_controller ros2 topic echo /xr/right_controller
ros2 topic echo /omnipicker/right/force_ratio ros2 topic echo /xr_rm/left_rm75/cmd_vel
ros2 topic echo /xr_rm/right_rm75/cmd_vel
```
也可以使用统一调试入口分别验证：
```bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
``` ```
## 连接双 RM75 ## 连接双 RM75
@ -87,11 +113,30 @@ ros2 launch xr_rm_bringup dual_arm_realman.launch.py \
right_robot_ip:=192.168.192.19 right_robot_ip:=192.168.192.19
``` ```
更推荐用统一调试入口按阶段放开：
```bash
# 左臂单独调试
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false
# 右臂单独调试
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false
# 双臂一起调试
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
```
真机 launch 默认不会让机械臂自动移动到 acRealman 初始化点。若现场确认安全区已清空，可显式开启：
```bash
ros2 launch xr_rm_bringup dual_arm_realman.launch.py move_to_initial_pose_on_connect:=true
```
单臂入口仍保留，默认按右臂配置： 单臂入口仍保留，默认按右臂配置：
```bash ```bash
ros2 launch xr_rm_bringup single_arm_mock.launch.py ros2 launch xr_rm_bringup single_arm_mock.launch.py arm:=right
ros2 launch xr_rm_bringup single_arm_realman.launch.py robot_ip:=192.168.192.19 ros2 launch xr_rm_bringup single_arm_realman.launch.py arm:=right robot_ip:=192.168.192.19
``` ```
## UDP JSON 格式 ## UDP JSON 格式
@ -131,16 +176,6 @@ ros2 launch xr_rm_bringup single_arm_realman.launch.py robot_ip:=192.168.192.19
`grip` 是机械臂运动使能：按下时锁定当前手柄和 TCP 起点，之后只跟随相对位移；松开、超时或节点退出都会发送零速度。 `grip` 是机械臂运动使能：按下时锁定当前手柄和 TCP 起点，之后只跟随相对位移；松开、超时或节点退出都会发送零速度。
## OmniPicker 接口
`gripper_trigger_bridge` 会把右手 `trigger` 映射为：
```text
/omnipicker/right/force_ratio std_msgs/Float32
```
默认范围是 `0.0` 开爪、`0.15-0.45` 柔性闭合/保持。实际接 OmniPicker 驱动前，请根据番茄压伤测试收紧 `dual_arm_rm75.yaml` 里的 `max_close_command`。
## 真机前方向验证 ## 真机前方向验证
第一轮保持 `max_linear_speed <= 0.04`，先只上右臂： 第一轮保持 `max_linear_speed <= 0.04`，先只上右臂：
@ -150,6 +185,12 @@ ros2 launch xr_rm_bringup single_arm_realman.launch.py robot_ip:=192.168.192.19
3. 手柄缓慢左移/右移，确认 TCP 方向。 3. 手柄缓慢左移/右移，确认 TCP 方向。
4. 若任一方向相反，只改 `dual_arm_rm75.yaml` 中对应臂的 `xr_to_robot_matrix` 符号。 4. 若任一方向相反，只改 `dual_arm_rm75.yaml` 中对应臂的 `xr_to_robot_matrix` 符号。
5. 右臂确认后再开放左臂。 5. 右臂确认后再开放左臂。
6. 最后接 OmniPicker，并从低力控上限开始抓取番茄。
当前双臂是两个独立控制节点，没有跨臂碰撞模型；首次测试请让两臂工作区物理分离。 当前双臂是两个独立控制节点，没有跨臂碰撞模型；首次测试请让两臂工作区物理分离。
## 从 acRealman 迁入的优化
- 右臂工作空间改为 acRealman 独立右臂边界，不再沿用左臂窄范围。
- 速度遥操目标在笛卡尔边界外增加基座圆柱半径限制，减少低位近基座不可达目标。
- 真机连接后会下发 acRealman 同款 RM75 硬件速度、加速度和避奇异配置。
- 保留 acRealman 的左右臂 `movej`/`movel` 初始化点位，可通过 launch 参数显式启用。

6
xr_rm_bringup/CMakeLists.txt
View File

@ -9,4 +9,10 @@ install(DIRECTORY
DESTINATION share/${PROJECT_NAME} DESTINATION share/${PROJECT_NAME}
) )
install(PROGRAMS
tools/launcher_ui.py
DESTINATION lib/${PROJECT_NAME}
RENAME launcher_ui
)
ament_package() ament_package()

58
xr_rm_bringup/config/dual_arm_rm75.yaml
View File

@ -3,7 +3,7 @@
# 参数来源：acDual-arm-YikaiFuTest/realman_pkg/core/arg_cfg.py # 参数来源：acDual-arm-YikaiFuTest/realman_pkg/core/arg_cfg.py
# - 左臂 IP：192.168.192.18 # - 左臂 IP：192.168.192.18
# - 右臂 IP：192.168.192.19 # - 右臂 IP：192.168.192.19
# - 左右臂工作空间和 IP 沿用最新 acDual-arm 配置。 # - 左右臂工作空间、圆柱半径约束、初始化点位和 IP 沿用最新 acRealman 配置。
# - PICO 手柄位移映射根据论文 OpenXR 坐标和现场双臂 base 坐标图单独设置。 # - PICO 手柄位移映射根据论文 OpenXR 坐标和现场双臂 base 坐标图单独设置。
# #
# 注意：当前项目仍然采用“手柄相对位移”控制方式。 # 注意：当前项目仍然采用“手柄相对位移”控制方式。
@ -30,6 +30,9 @@ left_arm_teleop:
# x[-0.50, 0.50]，y[-0.60, -0.20]，z[0.10, 0.50]。 # x[-0.50, 0.50]，y[-0.60, -0.20]，z[0.10, 0.50]。
workspace_min: [-0.50, -0.60, 0.10] workspace_min: [-0.50, -0.60, 0.10]
workspace_max: [0.50, -0.20, 0.50] workspace_max: [0.50, -0.20, 0.50]
cyl_radius_limit: [0.20, 0.60]
low_z_threshold: 0.20
low_z_min_radius: 0.21
# PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。 # PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
# 左臂 base 坐标：+X 向下，+Z 向右，前方工作区对应 -Y。 # 左臂 base 坐标：+X 向下，+Z 向右，前方工作区对应 -Y。
@ -39,12 +42,25 @@ left_arm_teleop:
1.0, 0.0, 0.0] 1.0, 0.0, 0.0]
use_mock: false use_mock: false
mock_initial_pose: [-0.1560, -0.3476, 0.2689, -3.1100, -0.2840, -1.3180] mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
robot_ip: 192.168.192.18 robot_ip: 192.168.192.18
robot_port: 8080 robot_port: 8080
avoid_singularity: 1 avoid_singularity: 1
frame_type: 1 frame_type: 1
follow: false follow: false
configure_safety_limits: true
max_line_speed: 1.0
max_angular_speed: 1.5
max_line_acc: 1.0
max_angular_acc: 2.0
joint_max_speed: 180.0
joint_max_acc: 180.0
# 真机 launch 默认不自动移动；需要沿用 acRealman 上电初始化时再显式打开。
move_to_initial_pose_on_connect: false
initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
initial_tcp_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
init_move_speed: 20
debug_topic_prefix: /xr_rm
right_arm_teleop: right_arm_teleop:
ros__parameters: ros__parameters:
@ -60,9 +76,13 @@ right_arm_teleop:
max_linear_speed: 0.04 max_linear_speed: 0.04
enable_position_axes: [true, true, true] enable_position_axes: [true, true, true]
# 来自 acDual-arm 的 bounds_p[right]。 # 来自 acDual-arm 的 bounds_p[right]：
workspace_min: [-0.50, -0.60, 0.10] # x[-0.70, 0.50]，y[-0.60, 0.40]，z[0.10, 0.70]。
workspace_max: [0.50, -0.20, 0.50] workspace_min: [-0.70, -0.60, 0.10]
workspace_max: [0.50, 0.40, 0.70]
cyl_radius_limit: [0.20, 0.60]
low_z_threshold: 0.20
low_z_min_radius: 0.21
# PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。 # PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
# 右臂 base 坐标：+X 向上，+Z 向左，前方工作区对应 -Y。 # 右臂 base 坐标：+X 向上，+Z 向左，前方工作区对应 -Y。
@ -72,23 +92,21 @@ right_arm_teleop:
-1.0, 0.0, 0.0] -1.0, 0.0, 0.0]
use_mock: false use_mock: false
mock_initial_pose: [0.1924, -0.3025, 0.2923, 3.0560, 0.1310, 0.9070] mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
robot_ip: 192.168.192.19 robot_ip: 192.168.192.19
robot_port: 8080 robot_port: 8080
avoid_singularity: 1 avoid_singularity: 1
frame_type: 1 frame_type: 1
follow: false follow: false
configure_safety_limits: true
right_omnipicker_trigger_bridge: max_line_speed: 1.0
ros__parameters: max_angular_speed: 1.5
# 阶段一只使用右手两指夹爪；右手扳机键输出归一化力控指令。 max_line_acc: 1.0
controller_topic: /xr/right_controller max_angular_acc: 2.0
command_topic: /omnipicker/right/force_ratio joint_max_speed: 180.0
publish_rate_hz: 30.0 joint_max_acc: 180.0
stale_timeout_sec: 0.25 move_to_initial_pose_on_connect: false
require_grip: true initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67]
trigger_deadband: 0.05 initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
open_command: 0.0 init_move_speed: 20
min_close_command: 0.15 debug_topic_prefix: /xr_rm
max_close_command: 0.45
low_pass_alpha: 0.5

45
xr_rm_bringup/config/left_arm_rm75.yaml Normal file
View File

@ -0,0 +1,45 @@
# 左臂单独调试配置：无末端执行器，仅 XR 相对位移控制 RM75 TCP。
single_arm_velocity_teleop:
ros__parameters:
arm_name: left_rm75
controller_topic: /xr/left_controller
control_rate_hz: 50.0
command_timeout_sec: 0.12
scale: 0.75
kp_linear: 1.8
deadband_m: 0.002
low_pass_alpha: 0.35
max_linear_speed: 0.04
enable_position_axes: [true, true, true]
workspace_min: [-0.50, -0.60, 0.10]
workspace_max: [0.50, -0.20, 0.50]
cyl_radius_limit: [0.20, 0.60]
low_z_threshold: 0.20
low_z_min_radius: 0.21
xr_to_robot_matrix: [0.0, -1.0, 0.0,
0.0, 0.0, -1.0,
1.0, 0.0, 0.0]
use_mock: false
mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
robot_ip: 192.168.192.18
robot_port: 8080
avoid_singularity: 1
frame_type: 1
follow: false
configure_safety_limits: true
max_line_speed: 1.0
max_angular_speed: 1.5
max_line_acc: 1.0
max_angular_acc: 2.0
joint_max_speed: 180.0
joint_max_acc: 180.0
move_to_initial_pose_on_connect: false
initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
initial_tcp_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
init_move_speed: 20
debug_topic_prefix: /xr_rm

45
xr_rm_bringup/config/right_arm_rm75.yaml Normal file
View File

@ -0,0 +1,45 @@
# 右臂单独调试配置：无末端执行器，仅 XR 相对位移控制 RM75 TCP。
single_arm_velocity_teleop:
ros__parameters:
arm_name: right_rm75
controller_topic: /xr/right_controller
control_rate_hz: 50.0
command_timeout_sec: 0.12
scale: 0.75
kp_linear: 1.8
deadband_m: 0.002
low_pass_alpha: 0.35
max_linear_speed: 0.04
enable_position_axes: [true, true, true]
workspace_min: [-0.70, -0.60, 0.10]
workspace_max: [0.50, 0.40, 0.70]
cyl_radius_limit: [0.20, 0.60]
low_z_threshold: 0.20
low_z_min_radius: 0.21
xr_to_robot_matrix: [0.0, 1.0, 0.0,
0.0, 0.0, -1.0,
-1.0, 0.0, 0.0]
use_mock: false
mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
robot_ip: 192.168.192.19
robot_port: 8080
avoid_singularity: 1
frame_type: 1
follow: false
configure_safety_limits: true
max_line_speed: 1.0
max_angular_speed: 1.5
max_line_acc: 1.0
max_angular_acc: 2.0
joint_max_speed: 180.0
joint_max_acc: 180.0
move_to_initial_pose_on_connect: false
initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67]
initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
init_move_speed: 20
debug_topic_prefix: /xr_rm

21
xr_rm_bringup/config/single_arm_rm75.yaml
View File

@ -15,8 +15,11 @@ single_arm_velocity_teleop:
max_linear_speed: 0.04 max_linear_speed: 0.04
enable_position_axes: [true, true, true] enable_position_axes: [true, true, true]
workspace_min: [-0.50, -0.60, 0.10] workspace_min: [-0.70, -0.60, 0.10]
workspace_max: [0.50, -0.20, 0.50] workspace_max: [0.50, 0.40, 0.70]
cyl_radius_limit: [0.20, 0.60]
low_z_threshold: 0.20
low_z_min_radius: 0.21
# 右臂 PICO/OpenXR 相对位移到 RM75 base 坐标的初始映射： # 右臂 PICO/OpenXR 相对位移到 RM75 base 坐标的初始映射：
# 机器人位移增量 = [手柄y, -手柄z, -手柄x]。 # 机器人位移增量 = [手柄y, -手柄z, -手柄x]。
@ -25,9 +28,21 @@ single_arm_velocity_teleop:
-1.0, 0.0, 0.0] -1.0, 0.0, 0.0]
use_mock: false use_mock: false
mock_initial_pose: [0.1924, -0.3025, 0.2923, 3.0560, 0.1310, 0.9070] mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
robot_ip: 192.168.192.19 robot_ip: 192.168.192.19
robot_port: 8080 robot_port: 8080
avoid_singularity: 1 avoid_singularity: 1
frame_type: 1 frame_type: 1
follow: false follow: false
configure_safety_limits: true
max_line_speed: 1.0
max_angular_speed: 1.5
max_line_acc: 1.0
max_angular_acc: 2.0
joint_max_speed: 180.0
joint_max_acc: 180.0
move_to_initial_pose_on_connect: false
initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67]
initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
init_move_speed: 20
debug_topic_prefix: /xr_rm

121
xr_rm_bringup/launch/arm_debug.launch.py Normal file
View File

@ -0,0 +1,121 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def _as_bool(value: str) -> bool:
return value.strip().lower() in ("1", "true", "yes", "on")
def _config_file(name: str) -> PathJoinSubstitution:
return PathJoinSubstitution([
FindPackageShare("xr_rm_bringup"),
"config",
name,
])
def _udp_receiver_node() -> Node:
return Node(
package="xr_rm_input",
executable="udp_controller_receiver",
name="udp_controller_receiver",
output="screen",
parameters=[{
"udp_host": LaunchConfiguration("udp_host"),
"udp_port": LaunchConfiguration("udp_port"),
"left_topic": "/xr/left_controller",
"right_topic": "/xr/right_controller",
}],
)
def _single_arm_node(arm: str, use_mock: bool, move_to_initial_pose: bool) -> Node:
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")
return Node(
package="xr_rm_teleop",
executable="single_arm_velocity_teleop",
name="single_arm_velocity_teleop",
output="screen",
parameters=[
_config_file(config_name),
{
"use_mock": use_mock,
"robot_ip": robot_ip,
"robot_port": LaunchConfiguration("robot_port"),
"move_to_initial_pose_on_connect": move_to_initial_pose,
},
],
)
def _dual_arm_nodes(use_mock: bool, move_to_initial_pose: bool) -> list[Node]:
config_file = _config_file("dual_arm_rm75.yaml")
return [
Node(
package="xr_rm_teleop",
executable="single_arm_velocity_teleop",
name="left_arm_teleop",
output="screen",
parameters=[
config_file,
{
"use_mock": use_mock,
"robot_ip": LaunchConfiguration("left_robot_ip"),
"robot_port": LaunchConfiguration("robot_port"),
"move_to_initial_pose_on_connect": move_to_initial_pose,
},
],
),
Node(
package="xr_rm_teleop",
executable="single_arm_velocity_teleop",
name="right_arm_teleop",
output="screen",
parameters=[
config_file,
{
"use_mock": use_mock,
"robot_ip": LaunchConfiguration("right_robot_ip"),
"robot_port": LaunchConfiguration("robot_port"),
"move_to_initial_pose_on_connect": move_to_initial_pose,
},
],
),
]
def _launch_setup(context, *args, **kwargs):
del args, kwargs
arm = LaunchConfiguration("arm").perform(context).strip().lower()
use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context))
move_to_initial_pose = _as_bool(
LaunchConfiguration("move_to_initial_pose_on_connect").perform(context)
)
if arm not in ("left", "right", "both"):
raise ValueError("arm must be one of: left, right, both")
nodes = [_udp_receiver_node()]
if arm == "both":
nodes.extend(_dual_arm_nodes(use_mock, move_to_initial_pose))
else:
nodes.append(_single_arm_node(arm, use_mock, move_to_initial_pose))
return nodes
def generate_launch_description() -> LaunchDescription:
return LaunchDescription([
DeclareLaunchArgument("arm", default_value="right"),
DeclareLaunchArgument("use_mock", default_value="true"),
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
DeclareLaunchArgument("udp_port", default_value="15000"),
DeclareLaunchArgument("left_robot_ip", default_value="192.168.192.18"),
DeclareLaunchArgument("right_robot_ip", default_value="192.168.192.19"),
DeclareLaunchArgument("robot_port", default_value="8080"),
DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"),
OpaqueFunction(function=_launch_setup),
])

18
xr_rm_bringup/launch/dual_arm_mock.launch.py
View File

@ -1,5 +1,6 @@
from launch import LaunchDescription from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare from launch_ros.substitutions import FindPackageShare
@ -9,18 +10,23 @@ def generate_launch_description() -> LaunchDescription:
config_file = PathJoinSubstitution([ config_file = PathJoinSubstitution([
FindPackageShare("xr_rm_bringup"), FindPackageShare("xr_rm_bringup"),
"config", "config",
"dual_arm_rm75.yaml", LaunchConfiguration("robot_config"),
]) ])
return LaunchDescription([ return LaunchDescription([
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
DeclareLaunchArgument("udp_port", default_value="15000"), DeclareLaunchArgument("udp_port", default_value="15000"),
DeclareLaunchArgument("robot_config", default_value="dual_arm_rm75.yaml"),
DeclareLaunchArgument("run_udp", default_value="true"),
DeclareLaunchArgument("run_left_arm", default_value="true"),
DeclareLaunchArgument("run_right_arm", default_value="true"),
# 接收 PICO/XR 侧 UDP 数据，并按左右手字段分发到对应手柄话题。 # 接收 PICO/XR 侧 UDP 数据，并按左右手字段分发到对应手柄话题。
Node( Node(
package="xr_rm_input", package="xr_rm_input",
executable="udp_controller_receiver", executable="udp_controller_receiver",
name="udp_controller_receiver", name="udp_controller_receiver",
output="screen", output="screen",
condition=IfCondition(LaunchConfiguration("run_udp")),
parameters=[{ parameters=[{
"udp_host": LaunchConfiguration("udp_host"), "udp_host": LaunchConfiguration("udp_host"),
"udp_port": LaunchConfiguration("udp_port"), "udp_port": LaunchConfiguration("udp_port"),
@ -34,6 +40,7 @@ def generate_launch_description() -> LaunchDescription:
executable="single_arm_velocity_teleop", executable="single_arm_velocity_teleop",
name="left_arm_teleop", name="left_arm_teleop",
output="screen", output="screen",
condition=IfCondition(LaunchConfiguration("run_left_arm")),
parameters=[config_file, {"use_mock": True}], parameters=[config_file, {"use_mock": True}],
), ),
# 右臂模拟节点：与左臂共用同一配置文件，但读取右手柄话题。 # 右臂模拟节点：与左臂共用同一配置文件，但读取右手柄话题。
@ -42,14 +49,7 @@ def generate_launch_description() -> LaunchDescription:
executable="single_arm_velocity_teleop", executable="single_arm_velocity_teleop",
name="right_arm_teleop", name="right_arm_teleop",
output="screen", output="screen",
condition=IfCondition(LaunchConfiguration("run_right_arm")),
parameters=[config_file, {"use_mock": True}], parameters=[config_file, {"use_mock": True}],
), ),
# 右手扳机键到 OmniPicker 归一化力控指令的桥接，模拟阶段用于检查话题输出。
Node(
package="xr_rm_teleop",
executable="gripper_trigger_bridge",
name="right_omnipicker_trigger_bridge",
output="screen",
parameters=[config_file],
),
]) ])

24
xr_rm_bringup/launch/dual_arm_realman.launch.py
View File

@ -1,7 +1,9 @@
from launch import LaunchDescription from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.substitutions import FindPackageShare from launch_ros.substitutions import FindPackageShare
@ -19,11 +21,16 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument("right_robot_ip", default_value="192.168.192.19"), DeclareLaunchArgument("right_robot_ip", default_value="192.168.192.19"),
DeclareLaunchArgument("robot_port", default_value="8080"), DeclareLaunchArgument("robot_port", default_value="8080"),
DeclareLaunchArgument("robot_config", default_value="dual_arm_rm75.yaml"), DeclareLaunchArgument("robot_config", default_value="dual_arm_rm75.yaml"),
DeclareLaunchArgument("run_udp", default_value="true"),
DeclareLaunchArgument("run_left_arm", default_value="true"),
DeclareLaunchArgument("run_right_arm", default_value="true"),
DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"),
Node( Node(
package="xr_rm_input", package="xr_rm_input",
executable="udp_controller_receiver", executable="udp_controller_receiver",
name="udp_controller_receiver", name="udp_controller_receiver",
output="screen", output="screen",
condition=IfCondition(LaunchConfiguration("run_udp")),
parameters=[{ parameters=[{
"udp_host": LaunchConfiguration("udp_host"), "udp_host": LaunchConfiguration("udp_host"),
"udp_port": LaunchConfiguration("udp_port"), "udp_port": LaunchConfiguration("udp_port"),
@ -36,12 +43,17 @@ def generate_launch_description() -> LaunchDescription:
executable="single_arm_velocity_teleop", executable="single_arm_velocity_teleop",
name="left_arm_teleop", name="left_arm_teleop",
output="screen", output="screen",
condition=IfCondition(LaunchConfiguration("run_left_arm")),
parameters=[ parameters=[
config_file, config_file,
{ {
"use_mock": False, "use_mock": False,
"robot_ip": LaunchConfiguration("left_robot_ip"), "robot_ip": LaunchConfiguration("left_robot_ip"),
"robot_port": LaunchConfiguration("robot_port"), "robot_port": LaunchConfiguration("robot_port"),
"move_to_initial_pose_on_connect": ParameterValue(
LaunchConfiguration("move_to_initial_pose_on_connect"),
value_type=bool,
),
}, },
], ],
), ),
@ -50,20 +62,18 @@ def generate_launch_description() -> LaunchDescription:
executable="single_arm_velocity_teleop", executable="single_arm_velocity_teleop",
name="right_arm_teleop", name="right_arm_teleop",
output="screen", output="screen",
condition=IfCondition(LaunchConfiguration("run_right_arm")),
parameters=[ parameters=[
config_file, config_file,
{ {
"use_mock": False, "use_mock": False,
"robot_ip": LaunchConfiguration("right_robot_ip"), "robot_ip": LaunchConfiguration("right_robot_ip"),
"robot_port": LaunchConfiguration("robot_port"), "robot_port": LaunchConfiguration("robot_port"),
"move_to_initial_pose_on_connect": ParameterValue(
LaunchConfiguration("move_to_initial_pose_on_connect"),
value_type=bool,
),
}, },
], ],
), ),
Node(
package="xr_rm_teleop",
executable="gripper_trigger_bridge",
name="right_omnipicker_trigger_bridge",
output="screen",
parameters=[config_file],
),
]) ])

34
xr_rm_bringup/launch/single_arm_mock.launch.py
View File

@ -1,36 +1,28 @@
from launch import LaunchDescription from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare from launch_ros.substitutions import FindPackageShare
def generate_launch_description() -> LaunchDescription: def generate_launch_description() -> LaunchDescription:
config_file = PathJoinSubstitution([ arm_debug_launch = PathJoinSubstitution([
FindPackageShare("xr_rm_teleop"), FindPackageShare("xr_rm_bringup"),
"config", "launch",
"single_arm.yaml", "arm_debug.launch.py",
]) ])
return LaunchDescription([ return LaunchDescription([
DeclareLaunchArgument("arm", default_value="right"),
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
DeclareLaunchArgument("udp_port", default_value="15000"), DeclareLaunchArgument("udp_port", default_value="15000"),
Node( IncludeLaunchDescription(
package="xr_rm_input", PythonLaunchDescriptionSource(arm_debug_launch),
executable="udp_controller_receiver", launch_arguments={
name="udp_controller_receiver", "arm": LaunchConfiguration("arm"),
output="screen", "use_mock": "true",
parameters=[{
"udp_host": LaunchConfiguration("udp_host"), "udp_host": LaunchConfiguration("udp_host"),
"udp_port": LaunchConfiguration("udp_port"), "udp_port": LaunchConfiguration("udp_port"),
"topic": "/xr/right_controller", }.items(),
}],
),
Node(
package="xr_rm_teleop",
executable="single_arm_velocity_teleop",
name="single_arm_velocity_teleop",
output="screen",
parameters=[config_file, {"use_mock": True}],
), ),
]) ])

47
xr_rm_bringup/launch/single_arm_realman.launch.py
View File

@ -1,46 +1,37 @@
from launch import LaunchDescription from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare from launch_ros.substitutions import FindPackageShare
def generate_launch_description() -> LaunchDescription: def generate_launch_description() -> LaunchDescription:
config_file = PathJoinSubstitution([ arm_debug_launch = PathJoinSubstitution([
FindPackageShare("xr_rm_bringup"), FindPackageShare("xr_rm_bringup"),
"config", "launch",
LaunchConfiguration("robot_config"), "arm_debug.launch.py",
]) ])
return LaunchDescription([ return LaunchDescription([
DeclareLaunchArgument("arm", default_value="right"),
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"), DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
DeclareLaunchArgument("udp_port", default_value="15000"), DeclareLaunchArgument("udp_port", default_value="15000"),
DeclareLaunchArgument("left_robot_ip", default_value="192.168.192.18"),
# 兼容旧命令：robot_ip 仍作为右臂 IP 覆盖入口。
DeclareLaunchArgument("robot_ip", default_value="192.168.192.19"), DeclareLaunchArgument("robot_ip", default_value="192.168.192.19"),
DeclareLaunchArgument("robot_port", default_value="8080"), DeclareLaunchArgument("robot_port", default_value="8080"),
DeclareLaunchArgument("robot_config", default_value="single_arm_rm75.yaml"), DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"),
Node( IncludeLaunchDescription(
package="xr_rm_input", PythonLaunchDescriptionSource(arm_debug_launch),
executable="udp_controller_receiver", launch_arguments={
name="udp_controller_receiver", "arm": LaunchConfiguration("arm"),
output="screen", "use_mock": "false",
parameters=[{
"udp_host": LaunchConfiguration("udp_host"), "udp_host": LaunchConfiguration("udp_host"),
"udp_port": LaunchConfiguration("udp_port"), "udp_port": LaunchConfiguration("udp_port"),
"topic": "/xr/right_controller", "left_robot_ip": LaunchConfiguration("left_robot_ip"),
}], "right_robot_ip": LaunchConfiguration("robot_ip"),
), "robot_port": LaunchConfiguration("robot_port"),
Node( "move_to_initial_pose_on_connect": LaunchConfiguration("move_to_initial_pose_on_connect"),
package="xr_rm_teleop", }.items(),
executable="single_arm_velocity_teleop",
name="single_arm_velocity_teleop",
output="screen",
parameters=[
config_file,
{
"use_mock": False,
"robot_ip": LaunchConfiguration("robot_ip"),
"robot_port": LaunchConfiguration("robot_port"),
},
],
), ),
]) ])

1
xr_rm_bringup/package.xml
View File

@ -11,6 +11,7 @@
<exec_depend>xr_rm_input</exec_depend> <exec_depend>xr_rm_input</exec_depend>
<exec_depend>xr_rm_teleop</exec_depend> <exec_depend>xr_rm_teleop</exec_depend>
<exec_depend>python3-tk</exec_depend>
<export> <export>
<build_type>ament_cmake</build_type> <build_type>ament_cmake</build_type>

893
xr_rm_bringup/tools/launcher_ui.py Executable file
View File

@ -0,0 +1,893 @@
#!/usr/bin/env python3
from __future__ import annotations
import os
import shlex
import shutil
import signal
import subprocess
import tempfile
import time
from pathlib import Path
try:
import tkinter as tk
from tkinter import messagebox, ttk
except ImportError as exc:
print(f"Missing dependency: {exc}")
print("Install Tkinter with: sudo apt-get install python3-tk")
raise SystemExit(1)
DEFAULT_LEFT_IP = "192.168.192.18"
DEFAULT_RIGHT_IP = "192.168.192.19"
DEFAULT_PORT = "15000"
TERMINAL_TITLE_PREFIX = "XR-RM Terminal - "
TOPIC_MONITOR_TITLE = "XR-RM Topic Monitor"
TOPIC_MONITOR_ACTION = "__xr_rm_topic_monitor__"
ROS_GRAPH_MONITOR_TITLE = "XR-RM ROS Graph Monitor"
ROS_GRAPH_MONITOR_ACTION = "__xr_rm_ros_graph_monitor__"
TOPIC_MONITORS = [
("Left Controller", "/xr/left_controller"),
("Right Controller", "/xr/right_controller"),
]
ROS_GRAPH_MONITORS = [
("ROS Topic List", "ros2 topic list"),
("ROS Node List", "ros2 node list"),
]
MODES = [
"Simulation",
"Left Arm",
"Right Arm",
"Dual Arm",
"Diagnostics",
]
def _quote(value: str | Path) -> str:
return shlex.quote(str(value))
def _config_quote(value: str) -> str:
return '"' + value.replace("\\", "\\\\").replace('"', '\\"') + '"'
def _with_index(items: list[tuple[str, str]]) -> list[tuple[str, str]]:
return [(f"{idx}. {title}", cmd) for idx, (title, cmd) in enumerate(items, start=1)]
def _find_workspace_root() -> Path:
env_workspace = os.environ.get("XR_RM_WS")
if env_workspace:
return Path(env_workspace).expanduser().resolve()
here = Path(__file__).resolve()
for parent in (here.parent, *here.parents):
if (parent / "install" / "setup.bash").exists() and (parent / "src").is_dir():
return parent
if parent.name == "src" and (parent / "xr_rm_bringup" / "package.xml").exists():
return parent.parent
return Path.cwd().resolve()
def _source_lines(workspace_root: Path) -> list[str]:
ros_setup = Path("/opt/ros/humble/setup.bash")
install_setup = workspace_root / "install" / "setup.bash"
lines = [
"set +e",
f"cd {_quote(workspace_root)}",
]
if ros_setup.exists():
lines.append(f"source {_quote(ros_setup)}")
else:
lines.append("echo 'Warning: /opt/ros/humble/setup.bash was not found.'")
lines.extend([
(
f"if [ -f {_quote(install_setup)} ]; then "
f"source {_quote(install_setup)}; "
"else echo 'Warning: workspace install/setup.bash was not found. Run colcon build first.'; fi"
),
"echo",
])
return lines
def _one_click_mock(arm: str, hand: str) -> str:
return "\n".join([
f"ros2 launch xr_rm_bringup arm_debug.launch.py arm:={arm} use_mock:=true &",
"launch_pid=$!",
"sleep 2",
f"ros2 run xr_rm_input sample_udp_sender --hand {hand} --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 20",
"echo",
"echo 'Sample sender finished. The launch process is still running in this terminal.'",
"echo 'Press Ctrl-C here, or use the cleanup button in the launcher, to stop it.'",
"wait \"$launch_pid\"",
])
def _diagnostic_commands() -> list[tuple[str, str]]:
return [
("Open ROS Topic/Node List Monitor", ROS_GRAPH_MONITOR_ACTION),
]
def _topic_monitor_item() -> tuple[str, str]:
return ("Open Controller Topic Monitor", TOPIC_MONITOR_ACTION)
def _finalize_items(
items: list[tuple[str, str]],
one_click: tuple[str, str] | None = None,
) -> list[tuple[str, str]]:
final_items = items + _diagnostic_commands() + [_topic_monitor_item()]
if one_click is not None:
final_items.append(one_click)
return _with_index(final_items)
def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
if mode == "Simulation":
items = [
("Left Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true"),
("Right Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true"),
("Dual Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true"),
(
"Sample UDP Sender (Left, 10s)",
f"ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
),
(
"Sample UDP Sender (Right, 10s)",
f"ros2 run xr_rm_input sample_udp_sender --hand right --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
),
(
"Sample UDP Sender (Both, 10s)",
f"ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
),
("One-Click Left Mock Demo", _one_click_mock("left", "left")),
("One-Click Right Mock Demo", _one_click_mock("right", "right")),
("One-Click Dual Mock Demo", _one_click_mock("both", "both")),
]
one_click = None
elif mode == "Left Arm":
items = [
("Ping Left RM75", f"ping -c 4 {DEFAULT_LEFT_IP}"),
(
"Left Arm RealMan Launch",
"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false "
f"left_robot_ip:={DEFAULT_LEFT_IP} "
"move_to_initial_pose_on_connect:=false",
),
(
"Sample UDP Sender (Left, 10s)",
f"ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
),
]
one_click = None
elif mode == "Right Arm":
items = [
("Ping Right RM75", f"ping -c 4 {DEFAULT_RIGHT_IP}"),
(
"Right Arm RealMan Launch",
"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false "
f"right_robot_ip:={DEFAULT_RIGHT_IP} "
"move_to_initial_pose_on_connect:=false",
),
(
"Sample UDP Sender (Right, 10s)",
f"ros2 run xr_rm_input sample_udp_sender --hand right --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
),
]
one_click = None
elif mode == "Dual Arm":
items = [
("Ping Left RM75", f"ping -c 4 {DEFAULT_LEFT_IP}"),
("Ping Right RM75", f"ping -c 4 {DEFAULT_RIGHT_IP}"),
(
"Dual Arm RealMan Launch",
"ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false "
f"left_robot_ip:={DEFAULT_LEFT_IP} right_robot_ip:={DEFAULT_RIGHT_IP} "
"move_to_initial_pose_on_connect:=false",
),
(
"Sample UDP Sender (Both, 10s)",
f"ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port {DEFAULT_PORT} --seconds 10",
),
]
one_click = None
else:
items = [
("ROS Doctor Report", "ros2 doctor --report"),
("XR-RM Bringup Prefix", "ros2 pkg prefix xr_rm_bringup"),
("XR-RM Input Prefix", "ros2 pkg prefix xr_rm_input"),
("XR-RM Teleop Prefix", "ros2 pkg prefix xr_rm_teleop"),
]
one_click = None
return _finalize_items(items, one_click)
class LauncherApp:
def __init__(self, root: tk.Tk) -> None:
self.root = root
self.workspace_root = _find_workspace_root()
self.current_commands: list[tuple[str, str]] = []
self.root.title("XR-RM Teleop Launcher")
self.root.geometry("760x640")
self.root.minsize(640, 560)
self.root.columnconfigure(0, weight=1)
self.root.rowconfigure(0, weight=1)
main_frame = ttk.Frame(root, padding=(10, 8, 10, 8))
main_frame.grid(row=0, column=0, sticky="nsew")
main_frame.columnconfigure(0, weight=1)
main_frame.rowconfigure(2, weight=1)
title_lbl = ttk.Label(main_frame, text="XR-RM Teleop Launcher", font=("Helvetica", 12, "bold"))
title_lbl.grid(row=0, column=0, sticky="w")
mode_frame = ttk.Frame(main_frame)
mode_frame.grid(row=1, column=0, sticky="ew", pady=(8, 6))
mode_frame.columnconfigure(1, weight=1)
ttk.Label(mode_frame, text="Mode:", font=("Helvetica", 10)).grid(row=0, column=0, sticky="w", padx=(0, 6))
self.mode_var = tk.StringVar(value=MODES[0])
self.mode_combo = ttk.Combobox(
mode_frame,
textvariable=self.mode_var,
values=MODES,
width=24,
state="readonly",
)
self.mode_combo.grid(row=0, column=1, sticky="ew")
self.mode_combo.bind("<<ComboboxSelected>>", self.on_mode_changed)
list_frame = ttk.Frame(main_frame)
list_frame.grid(row=2, column=0, sticky="nsew")
list_frame.columnconfigure(0, weight=1)
list_frame.rowconfigure(0, weight=1)
self.listbox = tk.Listbox(list_frame, selectmode=tk.SINGLE, font=("Courier", 10), height=8, width=1)
self.listbox.grid(row=0, column=0, sticky="nsew")
list_scroll = ttk.Scrollbar(list_frame, orient=tk.VERTICAL, command=self.listbox.yview)
list_scroll.grid(row=0, column=1, sticky="ns")
self.listbox.configure(yscrollcommand=list_scroll.set)
self.listbox.bind("<Double-1>", self.run_command)
self.listbox.bind("<<ListboxSelect>>", self.on_selection_changed)
preview_label = ttk.Label(main_frame, text="Command Preview", anchor=tk.W, font=("Helvetica", 10, "bold"))
preview_label.grid(row=3, column=0, sticky="w", pady=(8, 2))
self.preview = tk.Text(main_frame, height=3, width=1, wrap=tk.WORD, font=("Courier", 9))
self.preview.grid(row=4, column=0, sticky="ew")
self.preview.configure(state=tk.DISABLED)
button_frame = ttk.Frame(main_frame)
button_frame.grid(row=5, column=0, sticky="ew", pady=(8, 0))
for index in range(3):
button_frame.columnconfigure(index, weight=1, uniform="actions")
run_btn = tk.Button(
button_frame,
text="Run Selected",
command=self.run_command,
bg="#2e7d32",
fg="white",
font=("Helvetica", 10, "bold"),
height=1,
)
run_btn.grid(row=0, column=0, sticky="ew", padx=(0, 5))
check_btn = tk.Button(
button_frame,
text="Check Env",
command=self.check_prerequisites,
bg="#1976d2",
fg="white",
font=("Helvetica", 10, "bold"),
height=1,
)
check_btn.grid(row=0, column=1, sticky="ew", padx=5)
kill_btn = tk.Button(
button_frame,
text="Stop All",
command=self.kill_launched_processes,
bg="#c62828",
fg="white",
font=("Helvetica", 10, "bold"),
height=1,
)
kill_btn.grid(row=0, column=2, sticky="ew", padx=(5, 0))
self.status = tk.Label(root, text="Ready", bd=1, relief=tk.SUNKEN, anchor=tk.W)
self.status.grid(row=1, column=0, sticky="ew")
self.refresh_command_list()
def refresh_command_list(self) -> None:
self.current_commands = build_commands_by_mode(self.mode_var.get())
self.listbox.delete(0, tk.END)
for name, _ in self.current_commands:
self.listbox.insert(tk.END, name)
if self.current_commands:
self.listbox.select_set(0)
self.update_preview()
def on_mode_changed(self, event=None) -> None:
self.refresh_command_list()
self.status.config(text=f"Mode: {self.mode_var.get()}")
def on_selection_changed(self, event=None) -> None:
self.update_preview()
def selected_command(self) -> tuple[str, str] | None:
selection = self.listbox.curselection()
if not selection:
return None
return self.current_commands[selection[0]]
def update_preview(self) -> None:
selected = self.selected_command()
text = selected[1] if selected else ""
if text == TOPIC_MONITOR_ACTION:
topics = ", ".join(topic for _title, topic in TOPIC_MONITORS)
text = f"Open one Terminator window split into controller topic panes:\n{topics}"
elif text == ROS_GRAPH_MONITOR_ACTION:
text = "Open one Terminator window split into 2 horizontal panes:\nros2 topic list | ros2 node list"
self.preview.configure(state=tk.NORMAL)
self.preview.delete("1.0", tk.END)
self.preview.insert(tk.END, text)
self.preview.configure(state=tk.DISABLED)
def build_terminal_script(self, title: str, command: str) -> str:
window_title = TERMINAL_TITLE_PREFIX + title
lines = _source_lines(self.workspace_root)
lines.extend([
"export XR_RM_LAUNCHER_SESSION=1",
f"printf '\\033]0;%s\\007' {_quote(window_title)}",
command,
"exit_code=$?",
"echo",
"echo \"Command exited with code ${exit_code}.\"",
"read -r -p 'Press Enter to close...'",
"exit ${exit_code}",
])
return "\n".join(lines)
def terminal_command(self, title: str, script: str) -> list[str] | None:
window_title = TERMINAL_TITLE_PREFIX + title
if shutil.which("x-terminal-emulator"):
return ["x-terminal-emulator", "--no-dbus", "--title", window_title, "-e", "bash", "-ic", script]
if shutil.which("gnome-terminal"):
return ["gnome-terminal", f"--title={window_title}", "--", "bash", "-ic", script]
if shutil.which("konsole"):
return ["konsole", "--new-tab", "-p", f"tabtitle={window_title}", "-e", "bash", "-ic", script]
if shutil.which("xfce4-terminal"):
return ["xfce4-terminal", "--title", window_title, "--command", f"bash -ic {_quote(script)}"]
if shutil.which("xterm"):
return ["xterm", "-T", window_title, "-e", "bash", "-ic", script]
return None
def run_command(self, event=None) -> None:
selected = self.selected_command()
if selected is None:
messagebox.showwarning("No Selection", "Please select a command first.")
return
name, command = selected
if command == TOPIC_MONITOR_ACTION:
self.launch_topic_monitor()
return
if command == ROS_GRAPH_MONITOR_ACTION:
self.launch_ros_graph_monitor()
return
terminal_cmd = self.terminal_command(name, self.build_terminal_script(name, command))
if terminal_cmd is None:
messagebox.showerror(
"Terminal Not Found",
"No supported terminal emulator was found. Install gnome-terminal, konsole, xfce4-terminal, or xterm.",
)
return
try:
subprocess.Popen(terminal_cmd, cwd=str(self.workspace_root))
self.status.config(text=f"Launched: {name}")
except Exception as exc:
messagebox.showerror("Launch Failed", f"Failed to launch command:\n{exc}")
self.status.config(text="Launch failed")
def _ros_package_available(self, package: str) -> bool:
script = "\n".join(_source_lines(self.workspace_root) + [f"ros2 pkg prefix {package} >/dev/null 2>&1"])
try:
result = subprocess.run(
["bash", "-lc", script],
cwd=str(self.workspace_root),
stdout=subprocess.DEVNULL,
stderr=subprocess.DEVNULL,
timeout=8,
check=False,
)
except Exception:
return False
return result.returncode == 0
def check_prerequisites(self) -> None:
ok: list[str] = []
warnings: list[str] = []
errors: list[str] = []
if Path("/opt/ros/humble/setup.bash").exists():
ok.append("[OK] ROS2 Humble setup found.")
else:
errors.append("[MISS] /opt/ros/humble/setup.bash not found.")
if (self.workspace_root / "src" / "xr_rm_bringup" / "package.xml").exists():
ok.append(f"[OK] Workspace root: {self.workspace_root}")
else:
warnings.append(f"[WARN] Workspace root guess may be wrong: {self.workspace_root}")
if (self.workspace_root / "install" / "setup.bash").exists():
ok.append("[OK] Workspace install/setup.bash found.")
else:
warnings.append("[WARN] Workspace has not been built yet, or install/setup.bash is missing.")
if self.terminal_command("test", "true") is not None:
ok.append("[OK] Supported terminal emulator found.")
else:
errors.append("[MISS] No supported terminal emulator found.")
if shutil.which("x-terminal-emulator"):
ok.append("[OK] x-terminal-emulator found.")
else:
errors.append("[MISS] x-terminal-emulator not found. Split monitors require it.")
terminal_target = self.x_terminal_target()
if terminal_target == "terminator":
ok.append("[OK] x-terminal-emulator uses Terminator split-layout support.")
elif terminal_target:
warnings.append(
f"[WARN] x-terminal-emulator points to {terminal_target}. "
"The split monitors are designed for Terminator."
)
else:
warnings.append("[WARN] Could not identify x-terminal-emulator target.")
for package in ("xr_rm_bringup", "xr_rm_input", "xr_rm_teleop"):
if self._ros_package_available(package):
ok.append(f"[OK] ROS package available: {package}")
else:
warnings.append(f"[WARN] ROS package not discoverable yet: {package}")
try:
import Robotic_Arm # noqa: F401
ok.append("[OK] Robotic_Arm Python package found for RealMan mode.")
except ImportError:
warnings.append("[WARN] Robotic_Arm Python package not found. Mock mode is still usable.")
sections = []
if errors:
sections.append("Errors:\n" + "\n".join(errors))
if warnings:
sections.append("Warnings:\n" + "\n".join(warnings))
if ok:
sections.append("Passed:\n" + "\n".join(ok))
message = "\n\n".join(sections) if sections else "No checks were run."
if errors:
self.show_text_dialog("Prerequisite Check", "Errors Found", message)
self.status.config(text="Prerequisite check failed")
elif warnings:
self.show_text_dialog("Prerequisite Check", "Warnings Found", message)
self.status.config(text="Prerequisite check passed with warnings")
else:
self.show_text_dialog("Prerequisite Check", "All Checks Passed", message)
self.status.config(text="Prerequisite check passed")
def show_text_dialog(self, title: str, heading: str, message: str) -> None:
dialog = tk.Toplevel(self.root)
dialog.title(title)
dialog.geometry("760x520")
dialog.minsize(620, 360)
dialog.transient(self.root)
dialog.grab_set()
dialog.columnconfigure(0, weight=1)
dialog.rowconfigure(0, weight=1)
frame = ttk.Frame(dialog, padding=(12, 10, 12, 12))
frame.grid(row=0, column=0, sticky="nsew")
frame.columnconfigure(0, weight=1)
frame.rowconfigure(1, weight=1)
ttk.Label(frame, text=heading, font=("Helvetica", 12, "bold")).grid(row=0, column=0, sticky="w")
text_frame = ttk.Frame(frame)
text_frame.grid(row=1, column=0, sticky="nsew", pady=(8, 10))
text_frame.columnconfigure(0, weight=1)
text_frame.rowconfigure(0, weight=1)
text = tk.Text(text_frame, width=88, height=22, wrap=tk.WORD, font=("Courier", 10))
text.grid(row=0, column=0, sticky="nsew")
y_scroll = ttk.Scrollbar(text_frame, orient=tk.VERTICAL, command=text.yview)
y_scroll.grid(row=0, column=1, sticky="ns")
text.configure(yscrollcommand=y_scroll.set)
text.insert("1.0", message)
text.configure(state=tk.DISABLED)
ok_btn = ttk.Button(frame, text="OK", command=dialog.destroy)
ok_btn.grid(row=2, column=0, sticky="e")
ok_btn.focus_set()
dialog.bind("<Escape>", lambda _event: dialog.destroy())
dialog.bind("<Return>", lambda _event: dialog.destroy())
@staticmethod
def x_terminal_target() -> str:
terminal = shutil.which("x-terminal-emulator")
if terminal is None:
return ""
try:
return Path(terminal).resolve().name
except OSError:
return Path(terminal).name
def topic_pane_command(self, title: str, topic: str) -> str:
pane_title = f"{TOPIC_MONITOR_TITLE} - {title}"
lines = _source_lines(self.workspace_root)
lines.extend([
"export XR_RM_LAUNCHER_SESSION=1",
f"printf '\\033]0;%s\\007' {_quote(pane_title)}",
"clear",
f"echo {_quote('=== ' + title + ' ===')}",
f"echo {_quote('ros2 topic echo ' + topic)}",
"echo",
f"ros2 topic echo {topic}",
"echo",
"echo 'Topic echo stopped. This pane is left open for inspection.'",
"exec bash",
])
return "\n".join(lines)
def ros_graph_pane_command(self, title: str, command: str) -> str:
pane_title = f"{ROS_GRAPH_MONITOR_TITLE} - {title}"
lines = _source_lines(self.workspace_root)
lines.extend([
"export XR_RM_LAUNCHER_SESSION=1",
f"printf '\\033]0;%s\\007' {_quote(pane_title)}",
"trap 'echo; echo Monitor stopped.; exec bash' INT",
"while true; do",
" clear",
f" echo {_quote('=== ' + title + ' ===')}",
f" echo {_quote(command + ' (refreshes every 1s)')}",
" date '+%H:%M:%S'",
" echo",
f" {command}",
" sleep 1",
"done",
])
return "\n".join(lines)
def write_topic_monitor_script(self, title: str, topic: str) -> Path:
script_file = tempfile.NamedTemporaryFile(
mode="w",
prefix="xr_rm_topic_monitor_",
suffix=".sh",
delete=False,
encoding="utf-8",
)
with script_file:
script_file.write(self.topic_pane_command(title, topic))
script_file.write("\n")
os.chmod(script_file.name, 0o755)
return Path(script_file.name)
def write_ros_graph_monitor_script(self, title: str, command: str) -> Path:
script_file = tempfile.NamedTemporaryFile(
mode="w",
prefix="xr_rm_ros_graph_monitor_",
suffix=".sh",
delete=False,
encoding="utf-8",
)
with script_file:
script_file.write(self.ros_graph_pane_command(title, command))
script_file.write("\n")
os.chmod(script_file.name, 0o755)
return Path(script_file.name)
def topic_monitor_rect(self) -> tuple[int, int, int, int]:
screen_width = max(self.root.winfo_screenwidth(), 1)
screen_height = max(self.root.winfo_screenheight(), 1)
width = min(screen_width - 80, max(900, int(screen_width * 0.75)))
height = min(screen_height - 80, max(560, int(screen_height * (2.0 / 3.0))))
width = max(width, min(screen_width, 640))
height = max(height, min(screen_height, 420))
left = max((screen_width - width) // 2, 0)
top = max((screen_height - height) // 2, 0)
return width, height, left, top
def write_topic_monitor_config(self) -> Path:
width, height, left, top = self.topic_monitor_rect()
left_script, right_script = [
self.write_topic_monitor_script(title, topic)
for title, topic in TOPIC_MONITORS
]
layout_name = "xr_rm_topic_monitor"
config_text = "\n".join([
"[global_config]",
" suppress_multiple_term_dialog = True",
"[keybindings]",
"[profiles]",
" [[default]]",
" scrollback_infinite = True",
"[layouts]",
f" [[{layout_name}]]",
" [[[window0]]]",
" type = Window",
" parent = \"\"",
f" title = {_config_quote(TOPIC_MONITOR_TITLE)}",
f" position = {_config_quote(f'{left}:{top}')}",
f" size = {width}, {height}",
" maximised = False",
" fullscreen = False",
" [[[split0]]]",
" type = HPaned",
" parent = window0",
" order = 0",
" ratio = 0.5",
" [[[terminal_left]]]",
" type = Terminal",
" parent = split0",
" order = 0",
f" command = {_config_quote('bash ' + str(left_script))}",
" [[[terminal_right]]]",
" type = Terminal",
" parent = split0",
" order = 1",
f" command = {_config_quote('bash ' + str(right_script))}",
"[plugins]",
"",
])
config_file = tempfile.NamedTemporaryFile(
mode="w",
prefix="xr_rm_topic_monitor_",
suffix=".conf",
delete=False,
encoding="utf-8",
)
with config_file:
config_file.write(config_text)
return Path(config_file.name)
def write_ros_graph_monitor_config(self) -> Path:
width, height, left, top = self.topic_monitor_rect()
topic_script, node_script = [
self.write_ros_graph_monitor_script(title, command)
for title, command in ROS_GRAPH_MONITORS
]
layout_name = "xr_rm_ros_graph_monitor"
config_text = "\n".join([
"[global_config]",
" suppress_multiple_term_dialog = True",
"[keybindings]",
"[profiles]",
" [[default]]",
" scrollback_infinite = True",
"[layouts]",
f" [[{layout_name}]]",
" [[[window0]]]",
" type = Window",
" parent = \"\"",
f" title = {_config_quote(ROS_GRAPH_MONITOR_TITLE)}",
f" position = {_config_quote(f'{left}:{top}')}",
f" size = {width}, {height}",
" maximised = False",
" fullscreen = False",
" [[[split0]]]",
" type = HPaned",
" parent = window0",
" order = 0",
" ratio = 0.5",
" [[[terminal_topic_list]]]",
" type = Terminal",
" parent = split0",
" order = 0",
f" command = {_config_quote('bash ' + str(topic_script))}",
" [[[terminal_node_list]]]",
" type = Terminal",
" parent = split0",
" order = 1",
f" command = {_config_quote('bash ' + str(node_script))}",
"[plugins]",
"",
])
config_file = tempfile.NamedTemporaryFile(
mode="w",
prefix="xr_rm_ros_graph_monitor_",
suffix=".conf",
delete=False,
encoding="utf-8",
)
with config_file:
config_file.write(config_text)
return Path(config_file.name)
def topic_monitor_geometry(self) -> str:
width, height, left, top = self.topic_monitor_rect()
return f"{width}x{height}+{left}+{top}"
def launch_topic_monitor(self) -> None:
terminal = shutil.which("x-terminal-emulator")
if terminal is None:
messagebox.showerror(
"Terminal Not Found",
"x-terminal-emulator was not found. Install a terminal emulator to use the topic monitor.",
)
return
if self.x_terminal_target() != "terminator":
messagebox.showwarning(
"Topic Monitor",
"The controller topic monitor uses Terminator's split-layout support. "
"Please set x-terminal-emulator to Terminator.",
)
return
config_path = self.write_topic_monitor_config()
command = [
terminal,
"--no-dbus",
"--title",
TOPIC_MONITOR_TITLE,
"--config",
str(config_path),
"--layout",
"xr_rm_topic_monitor",
]
try:
subprocess.Popen(command, cwd=str(self.workspace_root))
self.status.config(text="Opened Terminator controller topic monitor.")
except Exception as exc:
messagebox.showerror("Topic Monitor Failed", f"Failed to launch topic monitor:\n{exc}")
self.status.config(text="Topic monitor launch failed")
def launch_ros_graph_monitor(self) -> None:
terminal = shutil.which("x-terminal-emulator")
if terminal is None:
messagebox.showerror(
"Terminal Not Found",
"x-terminal-emulator was not found. Install a terminal emulator to use the ROS graph monitor.",
)
return
if self.x_terminal_target() != "terminator":
messagebox.showwarning(
"ROS Graph Monitor",
"The ROS graph monitor uses Terminator's split-layout support. "
"Please set x-terminal-emulator to Terminator.",
)
return
config_path = self.write_ros_graph_monitor_config()
command = [
terminal,
"--no-dbus",
"--title",
ROS_GRAPH_MONITOR_TITLE,
"--config",
str(config_path),
"--layout",
"xr_rm_ros_graph_monitor",
]
try:
subprocess.Popen(command, cwd=str(self.workspace_root))
self.status.config(text="Opened Terminator ROS graph monitor.")
except Exception as exc:
messagebox.showerror("ROS Graph Monitor Failed", f"Failed to launch ROS graph monitor:\n{exc}")
self.status.config(text="ROS graph monitor launch failed")
def close_related_terminal_windows(self) -> int:
title_patterns = (TERMINAL_TITLE_PREFIX, TOPIC_MONITOR_TITLE, ROS_GRAPH_MONITOR_TITLE)
closed = 0
if shutil.which("wmctrl"):
try:
output = subprocess.check_output(["wmctrl", "-l"], text=True)
except Exception:
output = ""
for line in output.splitlines():
parts = line.split(None, 3)
if len(parts) < 4:
continue
window_id, title = parts[0], parts[3]
if any(pattern in title for pattern in title_patterns):
subprocess.run(["wmctrl", "-ic", window_id], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
closed += 1
if closed == 0 and shutil.which("xdotool"):
for pattern in title_patterns:
try:
output = subprocess.check_output(["xdotool", "search", "--name", pattern], text=True)
except subprocess.CalledProcessError:
continue
except Exception:
continue
for window_id in output.splitlines():
subprocess.run(["xdotool", "windowclose", window_id], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
closed += 1
return closed
def kill_launched_processes(self) -> None:
confirm = messagebox.askyesno(
"Confirm Stop",
"Stop XR-RM launcher terminals, topic monitors, and ROS nodes started from this workspace?",
)
if not confirm:
return
patterns = [
"ros2 launch xr_rm_bringup",
"ros2 run xr_rm_input",
"ros2 run xr_rm_teleop",
"XR_RM_LAUNCHER_SESSION=1",
TERMINAL_TITLE_PREFIX,
TOPIC_MONITOR_TITLE,
ROS_GRAPH_MONITOR_TITLE,
"xr_rm_topic_monitor_",
"xr_rm_ros_graph_monitor_",
"udp_controller_receiver",
"sample_udp_sender",
"single_arm_velocity_teleop",
"ros2 topic echo /xr/left_controller",
"ros2 topic echo /xr/right_controller",
"ros2 topic list",
"ros2 node list",
]
protected = {os.getpid(), os.getppid()}
killed: set[int] = set()
for pattern in patterns:
try:
output = subprocess.check_output(["pgrep", "-f", pattern], text=True)
except subprocess.CalledProcessError:
continue
except Exception as exc:
print(f"Failed to search process pattern {pattern!r}: {exc}")
continue
for pid_text in output.splitlines():
try:
pid = int(pid_text)
except ValueError:
continue
if pid in protected or pid in killed:
continue
try:
os.kill(pid, signal.SIGTERM)
killed.add(pid)
except OSError:
pass
time.sleep(0.3)
closed_windows = self.close_related_terminal_windows()
self.status.config(
text=f"Stop requested: {len(killed)} matching processes, {closed_windows} terminal windows."
)
messagebox.showinfo(
"Stop Complete",
"Stop requested for:\n"
f"- {len(killed)} matching processes, including launcher terminal wrappers\n"
f"- {closed_windows} related terminal windows",
)
def main() -> None:
root = tk.Tk()
LauncherApp(root)
root.mainloop()
if __name__ == "__main__":
main()

21
xr_rm_teleop/config/single_arm.yaml
View File

@ -15,8 +15,11 @@ single_arm_velocity_teleop:
max_linear_speed: 0.04 max_linear_speed: 0.04
enable_position_axes: [true, true, true] enable_position_axes: [true, true, true]
workspace_min: [-0.50, -0.60, 0.10] workspace_min: [-0.70, -0.60, 0.10]
workspace_max: [0.50, -0.20, 0.50] workspace_max: [0.50, 0.40, 0.70]
cyl_radius_limit: [0.20, 0.60]
low_z_threshold: 0.20
low_z_min_radius: 0.21
# 右臂 PICO/OpenXR 相对位移到 RM75 base 坐标的初始映射： # 右臂 PICO/OpenXR 相对位移到 RM75 base 坐标的初始映射：
# 机器人位移增量 = [手柄y, -手柄z, -手柄x]。 # 机器人位移增量 = [手柄y, -手柄z, -手柄x]。
@ -25,10 +28,22 @@ single_arm_velocity_teleop:
-1.0, 0.0, 0.0] -1.0, 0.0, 0.0]
use_mock: true use_mock: true
mock_initial_pose: [0.1924, -0.3025, 0.2923, 3.0560, 0.1310, 0.9070] mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
robot_ip: 192.168.192.19 robot_ip: 192.168.192.19
robot_port: 8080 robot_port: 8080
avoid_singularity: 1 avoid_singularity: 1
frame_type: 1 frame_type: 1
follow: false follow: false
configure_safety_limits: true
max_line_speed: 1.0
max_angular_speed: 1.5
max_line_acc: 1.0
max_angular_acc: 2.0
joint_max_speed: 180.0
joint_max_acc: 180.0
move_to_initial_pose_on_connect: false
initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67]
initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
init_move_speed: 20
debug_topic_prefix: /xr_rm

115
xr_rm_teleop/xr_rm_teleop/realman_adapter.py
View File

@ -60,12 +60,36 @@ class RealManAdapter:
dt: float, dt: float,
avoid_singularity: int, avoid_singularity: int,
frame_type: int, frame_type: int,
logger: Any | None = None,
configure_safety_limits: bool = True,
max_line_speed: float = 1.0,
max_angular_speed: float = 1.5,
max_line_acc: float = 1.0,
max_angular_acc: float = 2.0,
joint_max_speed: float = 180.0,
joint_max_acc: float = 180.0,
move_to_initial_pose_on_connect: bool = False,
initial_joint_pose: list[float] | None = None,
initial_tcp_pose: list[float] | None = None,
init_move_speed: int = 20,
) -> None: ) -> None:
self._robot_ip = robot_ip self._robot_ip = robot_ip
self._robot_port = robot_port self._robot_port = robot_port
self._dt_ms = int(round(dt * 1000.0)) self._dt_ms = int(round(dt * 1000.0))
self._avoid_singularity = avoid_singularity self._avoid_singularity = avoid_singularity
self._frame_type = frame_type self._frame_type = frame_type
self._logger = logger
self._configure_safety_limits = configure_safety_limits
self._max_line_speed = max_line_speed
self._max_angular_speed = max_angular_speed
self._max_line_acc = max_line_acc
self._max_angular_acc = max_angular_acc
self._joint_max_speed = joint_max_speed
self._joint_max_acc = joint_max_acc
self._move_to_initial_pose_on_connect = move_to_initial_pose_on_connect
self._initial_joint_pose = initial_joint_pose
self._initial_tcp_pose = initial_tcp_pose
self._init_move_speed = init_move_speed
self._arm: Any | None = None self._arm: Any | None = None
def connect(self) -> None: def connect(self) -> None:
@ -77,8 +101,12 @@ class RealManAdapter:
) from exc ) from exc
self._arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E) self._arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
ret = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port) handle = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port)
self._check_return(ret, "rm_create_robot_arm") self._check_robot_handle(handle)
if self._configure_safety_limits:
self._apply_safety_limits()
if self._move_to_initial_pose_on_connect:
self._move_to_initial_pose()
# 速度透传初始化必须和控制循环周期一致，避免真实机械臂出现周期不稳定。 # 速度透传初始化必须和控制循环周期一致，避免真实机械臂出现周期不稳定。
ret = self._arm.rm_set_movev_canfd_init( ret = self._arm.rm_set_movev_canfd_init(
self._avoid_singularity, self._avoid_singularity,
@ -122,6 +150,47 @@ class RealManAdapter:
if self._arm is None: if self._arm is None:
raise RuntimeError("睿尔曼机械臂尚未连接") raise RuntimeError("睿尔曼机械臂尚未连接")
def _apply_safety_limits(self) -> None:
self._try_call("rm_set_avoid_singularity_mode", True)
self._try_call("rm_set_arm_max_line_speed", self._max_line_speed)
self._try_call("rm_set_arm_max_angular_speed", self._max_angular_speed)
self._try_call("rm_set_arm_max_line_acc", self._max_line_acc)
self._try_call("rm_set_arm_max_angular_acc", self._max_angular_acc)
for joint_index in range(1, 8):
self._try_call("rm_set_joint_max_speed", joint_index, self._joint_max_speed)
self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc)
def _move_to_initial_pose(self) -> None:
if self._initial_joint_pose is None or self._initial_tcp_pose is None:
raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose 和 initial_tcp_pose")
ret = self._arm.rm_movej(self._initial_joint_pose, self._init_move_speed, 0, 0, 1)
self._check_return(ret, "rm_movej(initial_joint_pose)")
ret = self._arm.rm_movel(self._initial_tcp_pose, self._init_move_speed, 0, 0, 1)
self._check_return(ret, "rm_movel(initial_tcp_pose)")
def _try_call(self, name: str, *args: Any) -> None:
func = getattr(self._arm, name, None)
if func is None:
self._log_warn(f"当前睿尔曼 SDK 不支持 {name}，跳过该安全配置。")
return
try:
ret = func(*args)
self._check_return(ret, name)
except Exception as exc:
self._log_warn(f"{name} 安全配置失败，继续使用软件侧限幅：{exc}")
def _log_warn(self, message: str) -> None:
if self._logger is not None:
self._logger.warn(message)
@staticmethod
def _check_robot_handle(handle: Any) -> None:
handle_id = getattr(handle, "id", None)
if handle_id == -1:
raise RuntimeError("rm_create_robot_arm failed: socket error or robot unreachable")
@staticmethod @staticmethod
def _check_return(ret: Any, name: str) -> None: def _check_return(ret: Any, name: str) -> None:
code = ret[0] if isinstance(ret, tuple) and ret else ret code = ret[0] if isinstance(ret, tuple) and ret else ret
@ -148,6 +217,19 @@ class RealManAdapter:
pose = cls._find_pose(value) pose = cls._find_pose(value)
if pose is not None: if pose is not None:
return pose return pose
elif hasattr(obj, "to_dictionary"):
try:
return cls._find_pose(obj.to_dictionary(7))
except TypeError:
return cls._find_pose(obj.to_dictionary())
elif hasattr(obj, "to_dict"):
return cls._find_pose(obj.to_dict())
else:
for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
if hasattr(obj, key):
pose = cls._as_pose(getattr(obj, key))
if pose is not None:
return pose
return None return None
@staticmethod @staticmethod
@ -155,4 +237,33 @@ class RealManAdapter:
if isinstance(value, (list, tuple)) and len(value) >= 6: if isinstance(value, (list, tuple)) and len(value) >= 6:
if all(isinstance(item, Number) for item in value[:6]): if all(isinstance(item, Number) for item in value[:6]):
return [float(item) for item in value[:6]] return [float(item) for item in value[:6]]
if isinstance(value, dict):
position = value.get("position")
euler = value.get("euler")
if isinstance(position, dict) and isinstance(euler, dict):
keys = ("x", "y", "z")
rpy_keys = ("rx", "ry", "rz")
if all(key in position for key in keys) and all(key in euler for key in rpy_keys):
return [
float(position["x"]),
float(position["y"]),
float(position["z"]),
float(euler["rx"]),
float(euler["ry"]),
float(euler["rz"]),
]
if all(hasattr(value, attr) for attr in ("position", "euler")):
position = getattr(value, "position")
euler = getattr(value, "euler")
if all(hasattr(position, key) for key in ("x", "y", "z")) and all(
hasattr(euler, key) for key in ("rx", "ry", "rz")
):
return [
float(position.x),
float(position.y),
float(position.z),
float(euler.rx),
float(euler.ry),
float(euler.rz),
]
return None return None

168
xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
View File

@ -4,11 +4,12 @@ import math
from typing import Iterable from typing import Iterable
import rclpy import rclpy
from geometry_msgs.msg import PoseStamped, TwistStamped
from rclpy.node import Node from rclpy.node import Node
from xr_rm_interfaces.msg import XrController from xr_rm_interfaces.msg import XrController
from .realman_adapter import MockRealManAdapter, RealManAdapter from .realman_adapter import ArmPose, MockRealManAdapter, RealManAdapter
def _norm(values: Iterable[float]) -> float: def _norm(values: Iterable[float]) -> float:
@ -37,6 +38,9 @@ class SingleArmVelocityTeleop(Node):
self.declare_parameter("enable_position_axes", [True, True, True]) self.declare_parameter("enable_position_axes", [True, True, True])
self.declare_parameter("workspace_min", [0.20, -0.35, 0.10]) self.declare_parameter("workspace_min", [0.20, -0.35, 0.10])
self.declare_parameter("workspace_max", [0.65, 0.35, 0.60]) self.declare_parameter("workspace_max", [0.65, 0.35, 0.60])
self.declare_parameter("cyl_radius_limit", [0.20, 0.60])
self.declare_parameter("low_z_threshold", 0.20)
self.declare_parameter("low_z_min_radius", 0.21)
self.declare_parameter("xr_to_robot_matrix", [0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0]) self.declare_parameter("xr_to_robot_matrix", [0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0])
self.declare_parameter("use_mock", True) self.declare_parameter("use_mock", True)
self.declare_parameter("mock_initial_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0]) self.declare_parameter("mock_initial_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
@ -45,10 +49,24 @@ class SingleArmVelocityTeleop(Node):
self.declare_parameter("avoid_singularity", 1) self.declare_parameter("avoid_singularity", 1)
self.declare_parameter("frame_type", 1) self.declare_parameter("frame_type", 1)
self.declare_parameter("follow", False) self.declare_parameter("follow", False)
self.declare_parameter("configure_safety_limits", True)
self.declare_parameter("max_line_speed", 1.0)
self.declare_parameter("max_angular_speed", 1.5)
self.declare_parameter("max_line_acc", 1.0)
self.declare_parameter("max_angular_acc", 2.0)
self.declare_parameter("joint_max_speed", 180.0)
self.declare_parameter("joint_max_acc", 180.0)
self.declare_parameter("move_to_initial_pose_on_connect", False)
self.declare_parameter("initial_joint_pose", [0.0] * 7)
self.declare_parameter("initial_tcp_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
self.declare_parameter("init_move_speed", 20)
self.declare_parameter("debug_topic_prefix", "/xr_rm")
self._arm_name = str(self.get_parameter("arm_name").value) self._arm_name = str(self.get_parameter("arm_name").value)
topic = self.get_parameter("controller_topic").value topic = self.get_parameter("controller_topic").value
control_rate_hz = float(self.get_parameter("control_rate_hz").value) control_rate_hz = float(self.get_parameter("control_rate_hz").value)
if control_rate_hz <= 0.0:
raise ValueError("control_rate_hz must be > 0")
self._dt = 1.0 / control_rate_hz self._dt = 1.0 / control_rate_hz
self._command_timeout_sec = float(self.get_parameter("command_timeout_sec").value) self._command_timeout_sec = float(self.get_parameter("command_timeout_sec").value)
self._scale = float(self.get_parameter("scale").value) self._scale = float(self.get_parameter("scale").value)
@ -59,9 +77,15 @@ class SingleArmVelocityTeleop(Node):
self._enable_position_axes = self._bool_list_parameter("enable_position_axes", 3) self._enable_position_axes = self._bool_list_parameter("enable_position_axes", 3)
self._workspace_min = self._float_list_parameter("workspace_min", 3) self._workspace_min = self._float_list_parameter("workspace_min", 3)
self._workspace_max = self._float_list_parameter("workspace_max", 3) self._workspace_max = self._float_list_parameter("workspace_max", 3)
self._cyl_radius_limit = self._float_list_parameter("cyl_radius_limit", 2)
self._low_z_threshold = float(self.get_parameter("low_z_threshold").value)
self._low_z_min_radius = float(self.get_parameter("low_z_min_radius").value)
self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9) self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9)
self._follow = bool(self.get_parameter("follow").value) self._follow = self._bool_parameter("follow")
self._validate_workspace() self._debug_topic_prefix = str(self.get_parameter("debug_topic_prefix").value).rstrip("/")
if not self._debug_topic_prefix:
self._debug_topic_prefix = "/xr_rm"
self._validate_parameters()
self._last_msg: XrController | None = None self._last_msg: XrController | None = None
self._last_msg_time = None self._last_msg_time = None
@ -73,12 +97,17 @@ class SingleArmVelocityTeleop(Node):
self._adapter = self._make_adapter() self._adapter = self._make_adapter()
self._adapter.connect() self._adapter.connect()
debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
self._current_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/current_pose", 10)
self._target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/target_pose", 10)
self._cmd_vel_pub = self.create_publisher(TwistStamped, f"{debug_ns}/cmd_vel", 10)
self.create_subscription(XrController, topic, self._on_controller, 10) self.create_subscription(XrController, topic, self._on_controller, 10)
self.create_timer(self._dt, self._control_tick) self.create_timer(self._dt, self._control_tick)
self.get_logger().info(f"{self._arm_name} 速度遥操节点已启动，监听话题：{topic}") self.get_logger().info(f"{self._arm_name} 速度遥操节点已启动，监听话题：{topic}")
def _make_adapter(self): def _make_adapter(self):
if bool(self.get_parameter("use_mock").value): if self._bool_parameter("use_mock"):
return MockRealManAdapter( return MockRealManAdapter(
[float(v) for v in self.get_parameter("mock_initial_pose").value], [float(v) for v in self.get_parameter("mock_initial_pose").value],
self._dt, self._dt,
@ -90,6 +119,18 @@ class SingleArmVelocityTeleop(Node):
dt=self._dt, dt=self._dt,
avoid_singularity=int(self.get_parameter("avoid_singularity").value), avoid_singularity=int(self.get_parameter("avoid_singularity").value),
frame_type=int(self.get_parameter("frame_type").value), frame_type=int(self.get_parameter("frame_type").value),
logger=self.get_logger(),
configure_safety_limits=self._bool_parameter("configure_safety_limits"),
max_line_speed=float(self.get_parameter("max_line_speed").value),
max_angular_speed=float(self.get_parameter("max_angular_speed").value),
max_line_acc=float(self.get_parameter("max_line_acc").value),
max_angular_acc=float(self.get_parameter("max_angular_acc").value),
joint_max_speed=float(self.get_parameter("joint_max_speed").value),
joint_max_acc=float(self.get_parameter("joint_max_acc").value),
move_to_initial_pose_on_connect=self._bool_parameter("move_to_initial_pose_on_connect"),
initial_joint_pose=self._float_list_parameter("initial_joint_pose", 7),
initial_tcp_pose=self._float_list_parameter("initial_tcp_pose", 6),
init_move_speed=int(self.get_parameter("init_move_speed").value),
) )
def _on_controller(self, msg: XrController) -> None: def _on_controller(self, msg: XrController) -> None:
@ -119,7 +160,8 @@ class SingleArmVelocityTeleop(Node):
controller_now = self._controller_xyz(self._last_msg) controller_now = self._controller_xyz(self._last_msg)
try: try:
robot_now = self._adapter.get_current_pose().xyz() robot_pose = self._adapter.get_current_pose()
robot_now = robot_pose.xyz()
except Exception as exc: except Exception as exc:
self.get_logger().error( self.get_logger().error(
f"{self._arm_name} 读取 TCP 位姿失败，停止输出：{exc}", f"{self._arm_name} 读取 TCP 位姿失败，停止输出：{exc}",
@ -162,7 +204,9 @@ class SingleArmVelocityTeleop(Node):
alpha * velocity[i] + (1.0 - alpha) * self._filtered_velocity[i] alpha * velocity[i] + (1.0 - alpha) * self._filtered_velocity[i]
for i in range(3) for i in range(3)
] ]
self._send_cartesian_velocity(self._filtered_velocity + [0.0, 0.0, 0.0]) cartesian_velocity = self._filtered_velocity + [0.0, 0.0, 0.0]
self._publish_debug(robot_pose, target, cartesian_velocity)
self._send_cartesian_velocity(cartesian_velocity)
@staticmethod @staticmethod
def _controller_xyz(msg: XrController) -> list[float]: def _controller_xyz(msg: XrController) -> list[float]:
@ -177,10 +221,32 @@ class SingleArmVelocityTeleop(Node):
] ]
def _clamp_workspace(self, target: list[float]) -> list[float]: def _clamp_workspace(self, target: list[float]) -> list[float]:
return [ clamped = [
_clamp(target[i], self._workspace_min[i], self._workspace_max[i]) _clamp(target[i], self._workspace_min[i], self._workspace_max[i])
for i in range(3) for i in range(3)
] ]
return self._clamp_cylinder_radius(clamped)
def _clamp_cylinder_radius(self, target: list[float]) -> list[float]:
min_radius, max_radius = self._cyl_radius_limit
if target[2] < self._low_z_threshold:
min_radius = max(min_radius, self._low_z_min_radius)
radius = math.hypot(target[0], target[1])
if max_radius > 0.0 and radius > max_radius:
scale = max_radius / radius
target[0] *= scale
target[1] *= scale
elif min_radius > 0.0 and radius < min_radius:
if radius > 1e-9:
scale = min_radius / radius
target[0] *= scale
target[1] *= scale
else:
target[0] = min_radius
target[1] = 0.0
return target
@staticmethod @staticmethod
def _clamp_vector_norm(vector: list[float], max_norm: float) -> list[float]: def _clamp_vector_norm(vector: list[float], max_norm: float) -> list[float]:
@ -205,6 +271,64 @@ class SingleArmVelocityTeleop(Node):
) )
self._active = False self._active = False
def _publish_debug(self, current_pose: ArmPose, target_xyz: list[float], velocity: list[float]) -> None:
stamp = self.get_clock().now().to_msg()
current_msg = self._pose_msg(stamp, current_pose)
target_msg = self._pose_msg(
stamp,
ArmPose(
x=target_xyz[0],
y=target_xyz[1],
z=target_xyz[2],
rx=current_pose.rx,
ry=current_pose.ry,
rz=current_pose.rz,
),
)
velocity_msg = TwistStamped()
velocity_msg.header.stamp = stamp
velocity_msg.header.frame_id = "rm_base"
velocity_msg.twist.linear.x = float(velocity[0])
velocity_msg.twist.linear.y = float(velocity[1])
velocity_msg.twist.linear.z = float(velocity[2])
velocity_msg.twist.angular.x = float(velocity[3])
velocity_msg.twist.angular.y = float(velocity[4])
velocity_msg.twist.angular.z = float(velocity[5])
self._current_pose_pub.publish(current_msg)
self._target_pose_pub.publish(target_msg)
self._cmd_vel_pub.publish(velocity_msg)
@staticmethod
def _pose_msg(stamp, pose: ArmPose) -> PoseStamped:
qx, qy, qz, qw = SingleArmVelocityTeleop._euler_to_quaternion(pose.rx, pose.ry, pose.rz)
msg = PoseStamped()
msg.header.stamp = stamp
msg.header.frame_id = "rm_base"
msg.pose.position.x = float(pose.x)
msg.pose.position.y = float(pose.y)
msg.pose.position.z = float(pose.z)
msg.pose.orientation.x = qx
msg.pose.orientation.y = qy
msg.pose.orientation.z = qz
msg.pose.orientation.w = qw
return msg
@staticmethod
def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> tuple[float, float, float, float]:
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
qw = cr * cp * cy + sr * sp * sy
qx = sr * cp * cy - cr * sp * sy
qy = cr * sp * cy + sr * cp * sy
qz = cr * cp * sy - sr * sp * cy
return qx, qy, qz, qw
def _float_list_parameter(self, name: str, expected_length: int) -> list[float]: def _float_list_parameter(self, name: str, expected_length: int) -> list[float]:
values = [float(value) for value in self.get_parameter(name).value] values = [float(value) for value in self.get_parameter(name).value]
if len(values) != expected_length: if len(values) != expected_length:
@ -212,15 +336,41 @@ class SingleArmVelocityTeleop(Node):
return values return values
def _bool_list_parameter(self, name: str, expected_length: int) -> list[bool]: def _bool_list_parameter(self, name: str, expected_length: int) -> list[bool]:
values = [bool(value) for value in self.get_parameter(name).value] values = [self._as_bool(value) for value in self.get_parameter(name).value]
if len(values) != expected_length: if len(values) != expected_length:
raise ValueError(f"{name} must contain {expected_length} values") raise ValueError(f"{name} must contain {expected_length} values")
return values return values
def _validate_workspace(self) -> None: def _bool_parameter(self, name: str) -> bool:
return self._as_bool(self.get_parameter(name).value)
@staticmethod
def _as_bool(value) -> bool:
if isinstance(value, bool):
return value
if isinstance(value, str):
return value.strip().lower() in ("1", "true", "yes", "on")
return bool(value)
def _validate_parameters(self) -> None:
if self._command_timeout_sec <= 0.0:
raise ValueError("command_timeout_sec must be > 0")
if self._deadband_m < 0.0:
raise ValueError("deadband_m must be >= 0")
if not 0.0 <= self._low_pass_alpha <= 1.0:
raise ValueError("low_pass_alpha must be in [0, 1]")
if self._max_linear_speed < 0.0:
raise ValueError("max_linear_speed must be >= 0")
for axis, (low, high) in enumerate(zip(self._workspace_min, self._workspace_max)): for axis, (low, high) in enumerate(zip(self._workspace_min, self._workspace_max)):
if low >= high: if low >= high:
raise ValueError(f"workspace_min[{axis}] must be smaller than workspace_max[{axis}]") raise ValueError(f"workspace_min[{axis}] must be smaller than workspace_max[{axis}]")
min_radius, max_radius = self._cyl_radius_limit
if min_radius < 0.0:
raise ValueError("cyl_radius_limit[0] must be >= 0")
if max_radius <= min_radius:
raise ValueError("cyl_radius_limit[1] must be > cyl_radius_limit[0]")
if self._low_z_min_radius < 0.0:
raise ValueError("low_z_min_radius must be >= 0")
def destroy_node(self) -> bool: def destroy_node(self) -> bool:
self._adapter.close() self._adapter.close()