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

refactor launch_ui.py and update README.md

Browse Source
This commit is contained in:
YikaiFu-cart
2026-05-25 18:22:59 +08:00
parent 7551f1e8ea
commit e57890cb11
6 changed files with 13 additions and 322 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
PROMPT.md
View File

@ -22,14 +22,14 @@ PICO 4 Ultra 双手柄位姿/按键
- PICO/OpenXR 手柄 UDP 输入解析。
- 左右手柄话题分发。
- 左右 RM75 单臂速度遥操作节点。
- 双臂 mock、双臂真机、单臂调试 launch。
- 通过统一的 `arm_debug.launch.py` 支持双臂 mock、双臂真机和单臂调试。
- acRealman 来源的 RM75 IP、工作空间、圆柱半径约束、初始化点位和部分安全配置。
- Tkinter 启动面板，用于打开 mock、真机、诊断和话题监控命令。
- 可选的 trigger 到夹爪力控比例桥接节点。
当前阶段暂不实现：
- XR 侧 Unity 应用。
- 夹爪 trigger 桥接和末端执行器 launch 集成。
- 论文中的低延迟立体视频流。
- QP IK、灵巧手 retarget、全身追踪、运动捕捉 tracker。
- VLA 数据集记录和训练流程。
@ -73,7 +73,6 @@ xr_rm_teleop/
xr_rm_teleop/__init__.py
xr_rm_teleop/realman_adapter.py
xr_rm_teleop/single_arm_velocity_teleop.py
xr_rm_teleop/gripper_trigger_bridge.py
xr_rm_bringup/
CMakeLists.txt
@ -81,8 +80,6 @@ xr_rm_bringup/
config/dual_arm_rm75.yaml
config/left_arm_rm75.yaml
config/right_arm_rm75.yaml
launch/dual_arm_mock.launch.py
launch/dual_arm_realman.launch.py
launch/arm_debug.launch.py
tools/launcher_ui.py
```
@ -251,15 +248,6 @@ geometry_msgs/Pose pose
注意：当前阶段只控制 TCP 平移，不控制姿态。
### gripper_trigger_bridge.py
实现可选节点 `GripperTriggerBridge`：
- 订阅某个 `/xr/*_controller`
- 发布 `std_msgs/Float32` 到夹爪力控比例话题，如 `/omnipicker/right/force_ratio`
- 参数包括 `require_grip`、`trigger_deadband`、`open_command`、`min_close_command`、`max_close_command`、`low_pass_alpha`
- stale 或未按 grip 时发布开爪命令。
## bringup 配置需求
### RM75 默认参数
@ -329,19 +317,6 @@ initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
### launch 文件
`dual_arm_mock.launch.py`：
- 启动 `udp_controller_receiver`
- 启动 `left_arm_teleop` 和 `right_arm_teleop`
- 共用 `dual_arm_rm75.yaml`
- 覆盖 `use_mock=true`
- launch 参数：`udp_host`、`udp_port`、`robot_config`、`run_udp`、`run_left_arm`、`run_right_arm`
`dual_arm_realman.launch.py`：
- 和 mock 类似，但 `use_mock=false`
- launch 参数包含 `left_robot_ip`、`right_robot_ip`、`robot_port`、`move_to_initial_pose_on_connect`
`arm_debug.launch.py`：
- 参数 `arm:=left|right|both`
@ -349,6 +324,7 @@ initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
- 单臂时读取 `left_arm_rm75.yaml` 或 `right_arm_rm75.yaml`
- 双臂时读取 `dual_arm_rm75.yaml`
- 总是启动 `udp_controller_receiver`
- 这是唯一的遥操作 launch 主入口，UI 和命令行调试都优先使用它。
### 启动面板
@ -377,7 +353,7 @@ source install/setup.bash
mock 验证：
```bash
ros2 launch xr_rm_bringup dual_arm_mock.launch.py
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000
```
@ -401,7 +377,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
真机启动：
```bash
ros2 launch xr_rm_bringup dual_arm_realman.launch.py \
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
left_robot_ip:=192.168.192.18 \
right_robot_ip:=192.168.192.19
```
@ -428,7 +404,7 @@ ros2 launch xr_rm_bringup dual_arm_realman.launch.py \
验收标准：
- `colcon build --symlink-install` 能通过。
- `ros2 launch xr_rm_bringup dual_arm_mock.launch.py` 能启动三个节点。
- `ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true` 能启动三个节点。
- `sample_udp_sender --hand both` 后能看到左右 `/xr/*_controller` 和 `/xr_rm/*/cmd_vel`。
- 松开 grip、UDP 超时、节点退出时都会发送零速度。
- 修改 YAML 中 `xr_to_robot_matrix` 能改变对应臂方向映射。

17
README.md
View File

@ -18,7 +18,7 @@ PICO/XR 双手柄 UDP 数据
已完成：
- PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
- 左臂、右臂、双臂的 mock 调试和真机调试 launch。
- 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
- RM75 真机连接适配，包含速度透传初始化、安全速度/加速度配置、可选初始化点位移动。
- Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
@ -40,9 +40,7 @@ src/
│ │ ├── left_arm_rm75.yaml # 左臂单独调试配置
│ │ └── right_arm_rm75.yaml # 右臂单独调试配置
│ ├── launch/
│ │ ├── arm_debug.launch.py # 推荐主入口：arm:=left/right/both, use_mock:=true/false
│ │ ├── dual_arm_mock.launch.py # 双臂 mock 快速入口
│ │ └── dual_arm_realman.launch.py
│ │ └── arm_debug.launch.py # 统一主入口：arm:=left/right/both, use_mock:=true/false
│ └── tools/
│ └── launcher_ui.py # 图形化调试启动面板
├── xr_rm_input/
@ -57,8 +55,7 @@ src/
└── xr_rm_teleop/
└── xr_rm_teleop/
├── single_arm_velocity_teleop.py
├── realman_adapter.py
└── gripper_trigger_bridge.py
└── realman_adapter.py
```
`single_arm_velocity_teleop` 这个名字保留是有意的：双臂模式不是一个大节点直接控制两台机械臂，而是启动两个相同的单臂控制节点，分别命名为 `left_arm_teleop` 和 `right_arm_teleop`。
@ -177,7 +174,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
## Launch 入口说明
`arm_debug.launch.py` 是当前推荐主入口。
`arm_debug.launch.py` 是当前唯一的遥操作 launch 主入口，`launcher_ui.py` 中的 mock、单臂真机和双臂真机按钮都调用它。
常用参数：
@ -190,8 +187,6 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
- `robot_port`：RM75 TCP 端口，默认 `8080`。
- `move_to_initial_pose_on_connect`：连接后是否执行 `movej`/`movel` 初始化，默认 `false`。
`dual_arm_mock.launch.py` 和 `dual_arm_realman.launch.py` 是双臂快速入口，适合脚本化启动。它们也支持 `run_udp`、`run_left_arm`、`run_right_arm` 等开关，用于只启动部分节点。
## 配置文件说明
`xr_rm_bringup/config/dual_arm_rm75.yaml` 是双臂配置主文件，包含两个 ROS 节点命名空间：
@ -260,7 +255,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
- `hand`：`left` 或 `right`。
- `grip`：运动使能。`true` 时进入相对位移控制，`false` 时停止。
- `trigger`：扳机值，范围 `0.0-1.0`。当前控制主链路不使用它，`gripper_trigger_bridge.py` 可将其转换为夹爪力控比例。
- `trigger`：扳机值，范围 `0.0-1.0`。当前控制主链路不使用它，预留给后续夹爪集成。
- `pos`：手柄位置，长度 3。
- `quat`：手柄姿态四元数，默认按 `xyzw` 解析。
@ -298,7 +293,7 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
### 3. 集成末端执行器
- 将 `gripper_trigger_bridge.py` 纳入 bringup launch。
- 新增夹爪控制节点或桥接节点，并纳入 `arm_debug.launch.py` 与 `launcher_ui.py`。
- 明确夹爪 topic、力控比例、开合方向和安全上限。
- 在 `launcher_ui.py` 中加入夹爪测试按钮和夹爪状态监控。
- 将 `trigger` 从“预留字段”变成稳定的夹爪控制输入。

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

@ -1,65 +0,0 @@
"""双臂 mock 遥操作启动入口。
该 launch 文件用于不连接真实 RM75 的离线闭环验证：启动 UDP 手柄接收节点，
并分别启动左、右两个 `single_arm_velocity_teleop` 节点。两个遥操作节点使用
同一份双臂配置文件，但通过节点名读取各自的参数命名空间。
"""
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
def generate_launch_description() -> LaunchDescription:
# 通过 robot_config 参数选择 xr_rm_bringup/config 下的双臂配置文件。
config_file = PathJoinSubstitution([
FindPackageShare("xr_rm_bringup"),
"config",
LaunchConfiguration("robot_config"),
])
return LaunchDescription([
# UDP 监听参数，需要和 PICO 端或 sample_udp_sender 的目标地址保持一致。
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
DeclareLaunchArgument("udp_port", default_value="15000"),
DeclareLaunchArgument("robot_config", default_value="dual_arm_rm75.yaml"),
# 这些开关便于单独验证 UDP、左臂或右臂链路。
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"),
"left_topic": "/xr/left_controller",
"right_topic": "/xr/right_controller",
}],
),
# 左臂模拟节点：不连接真实 RM75，只积分速度命令用于验证坐标方向。
Node(
package="xr_rm_teleop",
executable="single_arm_velocity_teleop",
name="left_arm_teleop",
output="screen",
condition=IfCondition(LaunchConfiguration("run_left_arm")),
parameters=[config_file, {"use_mock": True}],
),
# 右臂模拟节点：与左臂共用同一配置文件，但读取右手柄话题。
Node(
package="xr_rm_teleop",
executable="single_arm_velocity_teleop",
name="right_arm_teleop",
output="screen",
condition=IfCondition(LaunchConfiguration("run_right_arm")),
parameters=[config_file, {"use_mock": True}],
),
])

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

@ -1,94 +0,0 @@
"""双 RM75 真机遥操作启动入口。
该 launch 文件用于连接左右两台真实 RM75：启动 UDP 手柄接收节点，并分别启动
左、右两个真机遥操作节点。默认不会自动移动到初始点，只有显式设置
`move_to_initial_pose_on_connect:=true` 后才会执行初始化点位移动。
"""
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
def generate_launch_description() -> LaunchDescription:
# 双臂真机默认读取 xr_rm_bringup/config/dual_arm_rm75.yaml。
config_file = PathJoinSubstitution([
FindPackageShare("xr_rm_bringup"),
"config",
LaunchConfiguration("robot_config"),
])
return LaunchDescription([
# UDP 监听参数，需要与 PICO 端发送地址和端口匹配。
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
DeclareLaunchArgument("udp_port", default_value="15000"),
# 左右 RM75 的默认现场 IP，可在 launch 命令中按实际网络覆盖。
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("robot_config", default_value="dual_arm_rm75.yaml"),
# 调试时可关闭部分节点，只验证 UDP 或单侧机械臂。
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"),
# 接收 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"),
"left_topic": "/xr/left_controller",
"right_topic": "/xr/right_controller",
}],
),
# 左臂真机节点：连接 left_robot_ip，并读取 left_arm_teleop 参数命名空间。
Node(
package="xr_rm_teleop",
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,
),
},
],
),
# 右臂真机节点：连接 right_robot_ip，并读取 right_arm_teleop 参数命名空间。
Node(
package="xr_rm_teleop",
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,
),
},
],
),
])

4
xr_rm_teleop/setup.py
View File

@ -1,7 +1,6 @@
"""xr_rm_teleop 包安装配置。
该包提供基于 XR 相对位移的 RM75 笛卡尔速度遥操作节点，以及把
手柄 trigger 映射到夹爪力控命令的桥接节点。
该包提供基于 XR 相对位移的 RM75 笛卡尔速度遥操作节点。
"""
from setuptools import setup
@ -25,7 +24,6 @@ setup(
tests_require=["pytest"],
entry_points={
"console_scripts": [
"gripper_trigger_bridge = xr_rm_teleop.gripper_trigger_bridge:main",
"single_arm_velocity_teleop = xr_rm_teleop.single_arm_velocity_teleop:main",
],
},

119
xr_rm_teleop/xr_rm_teleop/gripper_trigger_bridge.py
View File

@ -1,119 +0,0 @@
"""手柄 trigger 到夹爪力控指令的桥接节点。
订阅 XR 手柄消息，将 trigger 压力映射为 OmniPicker 可使用的归一化
闭合力比例，并在消息超时或未握持时自动回到开爪指令。
"""
from __future__ import annotations
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32
from xr_rm_interfaces.msg import XrController
def _clamp(value: float, low: float, high: float) -> float:
return min(max(value, low), high)
class GripperTriggerBridge(Node):
"""将 PICO 扳机键映射为 OmniPicker 可订阅的归一化力控指令。"""
def __init__(self) -> None:
super().__init__("gripper_trigger_bridge")
self.declare_parameter("controller_topic", "/xr/right_controller")
self.declare_parameter("command_topic", "/omnipicker/right/force_ratio")
self.declare_parameter("publish_rate_hz", 30.0)
self.declare_parameter("stale_timeout_sec", 0.25)
self.declare_parameter("require_grip", True)
# 扳机键输入小于死区时保持开爪，避免手柄轻微误触导致夹爪闭合。
self.declare_parameter("trigger_deadband", 0.05)
self.declare_parameter("open_command", 0.0)
# 阶段一以保护番茄为先，闭合力控上限应从低值开始实测。
self.declare_parameter("min_close_command", 0.15)
self.declare_parameter("max_close_command", 0.45)
self.declare_parameter("low_pass_alpha", 0.5)
controller_topic = str(self.get_parameter("controller_topic").value)
command_topic = str(self.get_parameter("command_topic").value)
publish_rate_hz = float(self.get_parameter("publish_rate_hz").value)
self._stale_timeout_sec = float(self.get_parameter("stale_timeout_sec").value)
self._require_grip = bool(self.get_parameter("require_grip").value)
self._trigger_deadband = float(self.get_parameter("trigger_deadband").value)
self._open_command = float(self.get_parameter("open_command").value)
self._min_close_command = float(self.get_parameter("min_close_command").value)
self._max_close_command = float(self.get_parameter("max_close_command").value)
self._low_pass_alpha = float(self.get_parameter("low_pass_alpha").value)
self._validate_parameters()
self._last_msg: XrController | None = None
self._last_msg_time = None
self._filtered_command = self._open_command
self._publisher = self.create_publisher(Float32, command_topic, 10)
self.create_subscription(XrController, controller_topic, self._on_controller, 10)
self.create_timer(1.0 / publish_rate_hz, self._publish_command)
self.get_logger().info(
f"夹爪 trigger 桥接已启动：{controller_topic} -> {command_topic}"
)
def _on_controller(self, msg: XrController) -> None:
self._last_msg = msg
self._last_msg_time = self.get_clock().now()
def _publish_command(self) -> None:
command = self._open_command
# 手柄消息超时后立即退回开爪，避免夹爪在通信中断时保持闭合力。
if self._last_msg is not None and self._last_msg_time is not None:
age = (self.get_clock().now() - self._last_msg_time).nanoseconds * 1e-9
if age <= self._stale_timeout_sec:
command = self._command_from_trigger(self._last_msg)
# 对夹爪指令做一阶平滑，避免扳机键抖动直接传到力控命令。
alpha = self._low_pass_alpha
self._filtered_command = alpha * command + (1.0 - alpha) * self._filtered_command
msg = Float32()
msg.data = float(self._filtered_command)
self._publisher.publish(msg)
def _command_from_trigger(self, msg: XrController) -> float:
if self._require_grip and not msg.grip:
return self._open_command
trigger = _clamp(float(msg.trigger), 0.0, 1.0)
if trigger <= self._trigger_deadband:
return self._open_command
ratio = (trigger - self._trigger_deadband) / (1.0 - self._trigger_deadband)
return self._min_close_command + ratio * (self._max_close_command - self._min_close_command)
def _validate_parameters(self) -> None:
if not 0.0 <= self._trigger_deadband < 1.0:
raise ValueError("trigger_deadband must be in [0, 1)")
if self._min_close_command < self._open_command:
raise ValueError("min_close_command must be >= open_command")
if self._max_close_command < self._min_close_command:
raise ValueError("max_close_command must be >= min_close_command")
if not 0.0 <= self._low_pass_alpha <= 1.0:
raise ValueError("low_pass_alpha must be in [0, 1]")
def main(args: list[str] | None = None) -> None:
rclpy.init(args=args)
node = GripperTriggerBridge()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()