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 位姿/按键
-> /xr/left_controller 与 /xr/right_controller
-> 左右 RM75 相对位移速度遥操
-> 右手 trigger 输出 OmniPicker 归一化力控指令
-> PC 端查看 D405/D405/D435 视频，后续再接记录节点
```
@ -14,8 +13,8 @@ PICO 4 Ultra 双手柄 UDP 位姿/按键
## 当前范围
- 已纳入：PICO 双手柄输入、左右 RM75 相对位移控制、右手 OmniPicker trigger 指令出口。
- 暂不纳入：AUBO 移动底盘、升降柱、剪刀手、XR 视频流、自动采摘算法。
- 已纳入：PICO 双手柄输入、左右 RM75 相对位移控制、左右臂独立/双臂调试 launch。
- 暂不纳入：末端执行器、AUBO 移动底盘、升降柱、XR 视频流、自动采摘算法。
## 关键配置来源
@ -24,8 +23,10 @@ PICO 4 Ultra 双手柄 UDP 位姿/按键
- 左臂 IP：`192.168.192.18`
- 右臂 IP：`192.168.192.19`
- 左右臂 TCP 端口：`8080`
- 左右臂工作空间：`x[-0.50, 0.50] y[-0.60, -0.20] z[0.10, 0.50]`
- 左右臂工作空间和 IP 沿用最新 acDual-arm 配置。
- 左臂工作空间：`x[-0.50, 0.50] y[-0.60, -0.20] z[0.10, 0.50]`
- 右臂工作空间：`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 坐标图设置：
@ -47,9 +48,25 @@ colcon build --symlink-install
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 模式不会连接真实 RM75，适合先验证 UDP 输入、左右手柄路由、相对位移控制和 trigger 夹爪指令。
mock 模式不会连接真实 RM75，适合先验证 UDP 输入、左右手柄路由和相对位移控制。
终端 1：
@ -72,7 +89,16 @@ ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000
```bash
ros2 topic echo /xr/left_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
@ -87,11 +113,30 @@ ros2 launch xr_rm_bringup dual_arm_realman.launch.py \
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
ros2 launch xr_rm_bringup single_arm_mock.launch.py
ros2 launch xr_rm_bringup single_arm_realman.launch.py robot_ip:=192.168.192.19
ros2 launch xr_rm_bringup single_arm_mock.launch.py arm:=right
ros2 launch xr_rm_bringup single_arm_realman.launch.py arm:=right robot_ip:=192.168.192.19
```
## UDP JSON 格式
@ -131,16 +176,6 @@ ros2 launch xr_rm_bringup single_arm_realman.launch.py robot_ip:=192.168.192.19
`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`，先只上右臂：
@ -150,6 +185,12 @@ ros2 launch xr_rm_bringup single_arm_realman.launch.py robot_ip:=192.168.192.19
3. 手柄缓慢左移/右移，确认 TCP 方向。
4. 若任一方向相反，只改 `dual_arm_rm75.yaml` 中对应臂的 `xr_to_robot_matrix` 符号。
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}
)
install(PROGRAMS
tools/launcher_ui.py
DESTINATION lib/${PROJECT_NAME}
RENAME launcher_ui
)
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
# - 左臂 IP：192.168.192.18
# - 右臂 IP：192.168.192.19
# - 左右臂工作空间和 IP 沿用最新 acDual-arm 配置。
# - 左右臂工作空间、圆柱半径约束、初始化点位和 IP 沿用最新 acRealman 配置。
# - 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]。
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
# PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
# 左臂 base 坐标：+X 向下，+Z 向右，前方工作区对应 -Y。
@ -39,12 +42,25 @@ left_arm_teleop:
1.0, 0.0, 0.0]
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_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
# 真机 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:
ros__parameters:
@ -60,9 +76,13 @@ right_arm_teleop:
max_linear_speed: 0.04
enable_position_axes: [true, true, true]
# 来自 acDual-arm 的 bounds_p[right]。
workspace_min: [-0.50, -0.60, 0.10]
workspace_max: [0.50, -0.20, 0.50]
# 来自 acDual-arm 的 bounds_p[right]：
# x[-0.70, 0.50]，y[-0.60, 0.40]，z[0.10, 0.70]。
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 向后。
# 右臂 base 坐标：+X 向上，+Z 向左，前方工作区对应 -Y。
@ -72,23 +92,21 @@ right_arm_teleop:
-1.0, 0.0, 0.0]
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_port: 8080
avoid_singularity: 1
frame_type: 1
follow: false
right_omnipicker_trigger_bridge:
ros__parameters:
# 阶段一只使用右手两指夹爪；右手扳机键输出归一化力控指令。
controller_topic: /xr/right_controller
command_topic: /omnipicker/right/force_ratio
publish_rate_hz: 30.0
stale_timeout_sec: 0.25
require_grip: true
trigger_deadband: 0.05
open_command: 0.0
min_close_command: 0.15
max_close_command: 0.45
low_pass_alpha: 0.5
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

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
enable_position_axes: [true, true, true]
workspace_min: [-0.50, -0.60, 0.10]
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 相对位移到 RM75 base 坐标的初始映射：
# 机器人位移增量 = [手柄y, -手柄z, -手柄x]。
@ -25,9 +28,21 @@ single_arm_velocity_teleop:
-1.0, 0.0, 0.0]
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_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

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.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
@ -9,18 +10,23 @@ def generate_launch_description() -> LaunchDescription:
config_file = PathJoinSubstitution([
FindPackageShare("xr_rm_bringup"),
"config",
"dual_arm_rm75.yaml",
LaunchConfiguration("robot_config"),
])
return LaunchDescription([
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
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 数据，并按左右手字段分发到对应手柄话题。
Node(
package="xr_rm_input",
executable="udp_controller_receiver",
name="udp_controller_receiver",
output="screen",
condition=IfCondition(LaunchConfiguration("run_udp")),
parameters=[{
"udp_host": LaunchConfiguration("udp_host"),
"udp_port": LaunchConfiguration("udp_port"),
@ -34,6 +40,7 @@ def generate_launch_description() -> LaunchDescription:
executable="single_arm_velocity_teleop",
name="left_arm_teleop",
output="screen",
condition=IfCondition(LaunchConfiguration("run_left_arm")),
parameters=[config_file, {"use_mock": True}],
),
# 右臂模拟节点：与左臂共用同一配置文件，但读取右手柄话题。
@ -42,14 +49,7 @@ def generate_launch_description() -> LaunchDescription:
executable="single_arm_velocity_teleop",
name="right_arm_teleop",
output="screen",
condition=IfCondition(LaunchConfiguration("run_right_arm")),
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.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
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("robot_port", default_value="8080"),
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(
package="xr_rm_input",
executable="udp_controller_receiver",
name="udp_controller_receiver",
output="screen",
condition=IfCondition(LaunchConfiguration("run_udp")),
parameters=[{
"udp_host": LaunchConfiguration("udp_host"),
"udp_port": LaunchConfiguration("udp_port"),
@ -36,12 +43,17 @@ def generate_launch_description() -> LaunchDescription:
executable="single_arm_velocity_teleop",
name="left_arm_teleop",
output="screen",
condition=IfCondition(LaunchConfiguration("run_left_arm")),
parameters=[
config_file,
{
"use_mock": False,
"robot_ip": LaunchConfiguration("left_robot_ip"),
"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",
name="right_arm_teleop",
output="screen",
condition=IfCondition(LaunchConfiguration("run_right_arm")),
parameters=[
config_file,
{
"use_mock": False,
"robot_ip": LaunchConfiguration("right_robot_ip"),
"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.actions import DeclareLaunchArgument
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description() -> LaunchDescription:
config_file = PathJoinSubstitution([
FindPackageShare("xr_rm_teleop"),
"config",
"single_arm.yaml",
arm_debug_launch = PathJoinSubstitution([
FindPackageShare("xr_rm_bringup"),
"launch",
"arm_debug.launch.py",
])
return LaunchDescription([
DeclareLaunchArgument("arm", default_value="right"),
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
DeclareLaunchArgument("udp_port", default_value="15000"),
Node(
package="xr_rm_input",
executable="udp_controller_receiver",
name="udp_controller_receiver",
output="screen",
parameters=[{
IncludeLaunchDescription(
PythonLaunchDescriptionSource(arm_debug_launch),
launch_arguments={
"arm": LaunchConfiguration("arm"),
"use_mock": "true",
"udp_host": LaunchConfiguration("udp_host"),
"udp_port": LaunchConfiguration("udp_port"),
"topic": "/xr/right_controller",
}],
),
Node(
package="xr_rm_teleop",
executable="single_arm_velocity_teleop",
name="single_arm_velocity_teleop",
output="screen",
parameters=[config_file, {"use_mock": True}],
}.items(),
),
])

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

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

1
xr_rm_bringup/package.xml
View File

@ -11,6 +11,7 @@
<exec_depend>xr_rm_input</exec_depend>
<exec_depend>xr_rm_teleop</exec_depend>
<exec_depend>python3-tk</exec_depend>
<export>
<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
enable_position_axes: [true, true, true]
workspace_min: [-0.50, -0.60, 0.10]
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 相对位移到 RM75 base 坐标的初始映射：
# 机器人位移增量 = [手柄y, -手柄z, -手柄x]。
@ -25,10 +28,22 @@ single_arm_velocity_teleop:
-1.0, 0.0, 0.0]
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_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

115
xr_rm_teleop/xr_rm_teleop/realman_adapter.py
View File

@ -60,12 +60,36 @@ class RealManAdapter:
dt: float,
avoid_singularity: 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:
self._robot_ip = robot_ip
self._robot_port = robot_port
self._dt_ms = int(round(dt * 1000.0))
self._avoid_singularity = avoid_singularity
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
def connect(self) -> None:
@ -77,8 +101,12 @@ class RealManAdapter:
) from exc
self._arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
ret = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port)
self._check_return(ret, "rm_create_robot_arm")
handle = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port)
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(
self._avoid_singularity,
@ -122,6 +150,47 @@ class RealManAdapter:
if self._arm is None:
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
def _check_return(ret: Any, name: str) -> None:
code = ret[0] if isinstance(ret, tuple) and ret else ret
@ -148,6 +217,19 @@ class RealManAdapter:
pose = cls._find_pose(value)
if pose is not None:
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
@staticmethod
@ -155,4 +237,33 @@ class RealManAdapter:
if isinstance(value, (list, tuple)) and len(value) >= 6:
if all(isinstance(item, Number) 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

168
xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
View File

@ -4,11 +4,12 @@ import math
from typing import Iterable
import rclpy
from geometry_msgs.msg import PoseStamped, TwistStamped
from rclpy.node import Node
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:
@ -37,6 +38,9 @@ class SingleArmVelocityTeleop(Node):
self.declare_parameter("enable_position_axes", [True, True, True])
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("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("use_mock", True)
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("frame_type", 1)
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)
topic = self.get_parameter("controller_topic").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._command_timeout_sec = float(self.get_parameter("command_timeout_sec").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._workspace_min = self._float_list_parameter("workspace_min", 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._follow = bool(self.get_parameter("follow").value)
self._validate_workspace()
self._follow = self._bool_parameter("follow")
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_time = None
@ -73,12 +97,17 @@ class SingleArmVelocityTeleop(Node):
self._adapter = self._make_adapter()
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_timer(self._dt, self._control_tick)
self.get_logger().info(f"{self._arm_name} 速度遥操节点已启动，监听话题：{topic}")
def _make_adapter(self):
if bool(self.get_parameter("use_mock").value):
if self._bool_parameter("use_mock"):
return MockRealManAdapter(
[float(v) for v in self.get_parameter("mock_initial_pose").value],
self._dt,
@ -90,6 +119,18 @@ class SingleArmVelocityTeleop(Node):
dt=self._dt,
avoid_singularity=int(self.get_parameter("avoid_singularity").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:
@ -119,7 +160,8 @@ class SingleArmVelocityTeleop(Node):
controller_now = self._controller_xyz(self._last_msg)
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:
self.get_logger().error(
f"{self._arm_name} 读取 TCP 位姿失败，停止输出：{exc}",
@ -162,7 +204,9 @@ class SingleArmVelocityTeleop(Node):
alpha * velocity[i] + (1.0 - alpha) * self._filtered_velocity[i]
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
def _controller_xyz(msg: XrController) -> list[float]:
@ -177,10 +221,32 @@ class SingleArmVelocityTeleop(Node):
]
def _clamp_workspace(self, target: list[float]) -> list[float]:
return [
clamped = [
_clamp(target[i], self._workspace_min[i], self._workspace_max[i])
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
def _clamp_vector_norm(vector: list[float], max_norm: float) -> list[float]:
@ -205,6 +271,64 @@ class SingleArmVelocityTeleop(Node):
)
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]:
values = [float(value) for value in self.get_parameter(name).value]
if len(values) != expected_length:
@ -212,15 +336,41 @@ class SingleArmVelocityTeleop(Node):
return values
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:
raise ValueError(f"{name} must contain {expected_length} 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)):
if low >= high:
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:
self._adapter.close()