# XR-RM75 双臂遥操作工作空间

本仓库是面向 **Ubuntu 22.04 + ROS2 Humble + PICO 4 Ultra + 睿尔曼 RM75** 的阶段一 XR 双臂遥操作项目。当前目标是先跑通一条低速、安全、可调试的闭环：

```text
PICO/XR 双手柄 UDP JSON
  -> xr_rm_input/udp_controller_receiver
  -> /xr/left_controller 与 /xr/right_controller
  -> xr_rm_teleop/single_arm_velocity_teleop
  -> 左右 RM75 笛卡尔相对位移速度控制
  -> /xr_rm/<arm_name>/current_pose、target_pose、cmd_vel 调试话题
```

当前控制方式是“手柄相对位移”遥操作：按住 `grip` 时锁定当前手柄位置和机械臂 TCP 位置，之后根据手柄相对位移生成目标 TCP，再用速度命令跟随。松开 `grip`、UDP 超时或节点退出时会发送零速度。

## 当前范围

已完成：

- PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
- 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
- RM75 真机连接适配，包含速度透传初始化、安全速度/加速度配置、可选初始化点位移动。
- Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
- 自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、`grip` 和 `trigger`。

暂未完成：

- D405/D435 视频流、数据记录、相机标定和目标检测链路。
- 双臂碰撞模型、任务级状态机、自动采摘策略。
- PICO 端与 ROS 端的完整时间同步和状态回传。

## 项目结构

```text
src/
├── README.md                       # 项目主文档
├── CODEX.md                        # Codex/Claude Code 项目工作流和安全规则
├── docs/
│   └── pico_udp_sender_ubuntu22_setup.md
├── unity/
│   ├── XR_RM_PICO_UDP_Sender/      # PICO 4 Ultra UDP Sender Unity 工程
│   └── PICO-Unity-Integration-SDK-release_3.4.0/
├── xr_rm_bringup/
│   ├── config/
│   │   ├── dual_arm_rm75.yaml      # 双臂配置：left_arm_teleop 与 right_arm_teleop
│   │   ├── left_arm_rm75.yaml      # 左臂单独调试配置
│   │   ├── right_arm_rm75.yaml     # 右臂单独调试配置
│   │   └── peripherals_rm75.yaml   # 左右臂末端外设配置
│   ├── launch/
│   │   └── arm_debug.launch.py     # 统一入口：arm:=left/right/both, use_mock:=true/false
│   └── tools/
│       ├── launcher_ui.py          # 图形化调试启动面板
│       └── realman_dual_arm_state_monitor.py
├── xr_rm_input/
│   ├── launch/
│   │   └── udp_receiver.launch.py  # 低层 UDP 接收测试入口
│   └── xr_rm_input/
│       ├── udp_controller_receiver.py
│       └── sample_udp_sender.py    # 本机模拟手柄 UDP 数据
├── xr_rm_interfaces/
│   └── msg/
│       └── XrController.msg        # hand/grip/trigger/pose
└── xr_rm_teleop/
    └── xr_rm_teleop/
        ├── single_arm_velocity_teleop.py
        ├── realman_adapter.py
        └── fun_peripheral.py
```

`single_arm_velocity_teleop` 这个名字保留是有意的：双臂模式不是一个大节点直接控制两台机械臂，而是启动两个相同的单臂控制节点，分别命名为 `left_arm_teleop` 和 `right_arm_teleop`。

## 环境准备

在工作空间根目录，也就是包含 `src/` 的目录执行：

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
rosdep update
rosdep install --from-paths src -y --ignore-src
colcon build --symlink-install
source install/setup.bash
```

真机模式还需要安装睿尔曼 Python API2。若未安装，mock 模式仍可正常使用；真机启动时会提示缺少 `Robotic_Arm` 包。

如果希望 `launcher_ui.py` 从任意目录找到工作空间，可以设置：

```bash
export XR_RM_WS=/home/robot/WS_xr
```

## 使用 launcher_ui.py 调试

推荐现场调试优先使用图形化启动面板。它会自动进入工作空间、source ROS2 与 `install/setup.bash`，并把每个命令放到独立终端中运行。

源码方式启动：

```bash
cd /home/robot/WS_xr
python3 src/xr_rm_bringup/tools/launcher_ui.py
```

构建后也可以通过 ROS2 入口启动：

```bash
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 run xr_rm_bringup launcher_ui
```

面板顶部的 `Mode` 分为五类：

- `Simulation`：左臂 mock、右臂 mock、双臂 mock、sample UDP 发送、one-click mock demo。
- `Left Arm`：左臂网络 ping、左臂真机 launch、左手 sample UDP。
- `Right Arm`：右臂网络 ping、右臂真机 launch、右手 sample UDP。
- `Dual Arm`：左右臂 ping、双臂真机 launch、双手 sample UDP。
- `Diagnostics`：`ros2 doctor --report` 和核心包的 `ros2 pkg prefix` 检查。

常用按钮：

- `Run Selected`：运行当前选中的命令。双击列表项也可以运行。
- `Check Env`：检查 ROS2 Humble、工作空间 build、终端、核心 ROS 包、睿尔曼 API2。
- `Stop All`：结束由本工作空间启动的 launch、sample sender、topic monitor、相关 ROS 节点和终端窗口。

每个模式都会附带三个监控入口：

- `Open Controller Topic Monitor`：同时查看 `/xr/left_controller` 和 `/xr/right_controller`。
- `Open Cmd Vel Topic Monitor`：同时查看 `/xr_rm/left_rm75/cmd_vel` 和 `/xr_rm/right_rm75/cmd_vel`。
- `Open ROS Topic/Node List Monitor`：每秒刷新 `ros2 topic list` 和 `ros2 node list`。

分屏监控依赖 `x-terminal-emulator` 指向 Terminator。若提示不支持，可安装并切换：

```bash
sudo apt install terminator wmctrl xdotool
sudo update-alternatives --config x-terminal-emulator
```

## 推荐调试顺序

第一步：检查环境。

打开 `launcher_ui.py`，点击 `Check Env`。如果 `install/setup.bash` 缺失，先回工作空间根目录重新执行 `colcon build --symlink-install`。

第二步：跑 mock 闭环。

在 `Simulation` 模式运行 `One-Click Dual Mock Demo`，或分开运行：

```bash
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 --seconds 10
```

观察：

```bash
ros2 topic echo /xr/left_controller
ros2 topic echo /xr/right_controller
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:=false
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false
```

第四步：双臂真机。

```bash
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
```

默认不会自动移动到初始化点。只有在确认安全区清空后，才显式打开：

```bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
  move_to_initial_pose_on_connect:=true
```

## Launch 入口说明

`arm_debug.launch.py` 是当前唯一的遥操作 launch 主入口，`launcher_ui.py` 中的 mock、单臂真机和双臂真机按钮都调用它。

常用参数：

- `arm`：`left`、`right`、`both`，默认 `right`。
- `use_mock`：`true` 不连接真机，`false` 连接 RM75。
- `udp_host`：UDP 监听地址，默认 `0.0.0.0`。
- `udp_port`：UDP 监听端口，默认 `15000`。
- `left_robot_ip`：左臂 IP，默认 `192.168.192.18`。
- `right_robot_ip`：右臂 IP，默认 `192.168.192.19`。
- `robot_port`：RM75 TCP 端口，默认 `8080`。
- `enable_tool_control`：是否在遥操作节点内启用末端工具控制 topic，默认 `true`。
- `configure_peripheral_on_connect`：遥操作节点连接真机后是否配置末端外设，默认 `true`；工具控制会复用同一个 RealMan 连接，避免两个进程同时抢占同一机械臂。
- `move_to_initial_pose_on_connect`：连接后是否执行 `movej`/`movel` 初始化，默认 `false`。

## 配置文件说明

`xr_rm_bringup/config/dual_arm_rm75.yaml` 是双臂配置主文件，包含两个 ROS 节点命名空间：

- `left_arm_teleop`
- `right_arm_teleop`

`left_arm_rm75.yaml` 和 `right_arm_rm75.yaml` 用于 `arm_debug.launch.py arm:=left/right` 的单臂调试，因为单臂节点名是 `single_arm_velocity_teleop`。

`xr_rm_bringup/config/peripherals_rm75.yaml` 保存末端工具坐标、负载和左右臂外设选择。当前配置为左臂 `scissorgripper=1`、右臂 `scissorgripper=2`，并且只在真机连接阶段初始化外设，不在主遥操作循环里控制开合。

重点控制参数：

- `controller_topic`：订阅的手柄话题。
- `scale`：手柄位移到 TCP 位移的比例。
- `kp_linear`：位置误差到速度命令的比例增益。
- `max_linear_speed`：软件侧最大线速度。
- `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
- `cyl_radius_limit`：基座圆柱半径限制。
- `xr_to_robot_matrix`：PICO/OpenXR 位移到 RM75 base 坐标的映射矩阵。
- `mock_initial_pose`：mock 模式初始 TCP 位姿。
- `initial_joint_pose` / `initial_tcp_pose`：可选真机初始化点。

当前坐标约定：

- PICO/OpenXR：`+X` 向右，`+Y` 向上，`+Z` 向后。
- 左臂映射：机器人位移增量 = `[-手柄y, 手柄z, -手柄x]`。
- 右臂映射：机器人位移增量 = `[手柄y, 手柄z, 手柄x]`。

如果某个方向相反，只改对应臂的 `xr_to_robot_matrix` 符号，不要同时改多个控制参数。

## 末端工具开合

真机 launch 默认会在遥操作节点内启用工具控制。启动后可以用 Bool 话题控制开合，`true` 表示打开，`false` 表示闭合：

```bash
ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: false}"

ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: false}"
```

桌面 UI 的 `Left Arm`、`Right Arm` 和 `Dual Arm` 模式里也有对应的 Tool Open/Close 命令项。

## UDP 数据格式

单个手柄：

```json
{
  "hand": "right",
  "grip": true,
  "trigger": 0.2,
  "pos": [0.12, 1.05, 0.30],
  "quat": [0.0, 0.0, 0.0, 1.0]
}
```

左右手柄一起发送：

```json
{
  "controllers": {
    "left": {
      "grip": true,
      "trigger": 0.0,
      "pos": [-0.12, 1.05, 0.30],
      "quat": [0.0, 0.0, 0.0, 1.0]
    },
    "right": {
      "grip": true,
      "trigger": 0.4,
      "pos": [0.12, 1.05, 0.30],
      "quat": [0.0, 0.0, 0.0, 1.0]
    }
  }
}
```

字段说明：

- `hand`：`left` 或 `right`。
- `grip`：运动使能。`true` 时进入相对位移控制，`false` 时停止。
- `trigger`：扳机值，范围 `0.0-1.0`。当前控制主链路不使用它，预留给后续夹爪集成。
- `pos`：手柄位置，长度 3。
- `quat`：手柄姿态四元数，默认按 `xyzw` 解析。

PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并向 ROS2 主机发送 UDP 的详细步骤见 [docs/pico_udp_sender_ubuntu22_setup.md](docs/pico_udp_sender_ubuntu22_setup.md)。

## 真机安全验证

第一次接真机时按这个顺序走：

1. 确认急停、网络、机械臂工作区和人员位置。
2. `launcher_ui.py` 中先 `Ping Left RM75` 或 `Ping Right RM75`。
3. 单臂启动，`move_to_initial_pose_on_connect:=false`。
4. 手握急停，按住 `grip` 后只做小幅单轴移动。
5. 逐个确认上/下、前/后、左/右方向。
6. 确认松开 `grip` 后 `/xr_rm/<arm>/cmd_vel` 归零。
7. 左右臂都确认后，再进入双臂模式。

当前项目没有双臂碰撞检测。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。

## 后续优化路线

为了达到“稳定可用的双臂 XR 遥操作/采摘平台”，建议按下面顺序推进：

1. 稳定 PICO 数据链路：固定 UDP JSON 协议和坐标系，增加频率、延迟、丢包统计，记录 `/xr/*_controller`、`/xr_rm/*/cmd_vel`、`/xr_rm/*/current_pose`。
2. 提升真机安全性：增加启动前安全检查、软件急停 topic、UI Stop 状态提示、双臂中间区域互斥边界和速度/加速度限幅。
3. 集成末端执行器：明确夹爪 topic、力控比例、开合方向和安全上限，把 `trigger` 从预留字段变成稳定夹爪输入。
4. 接入视觉和数据记录：加入 D405/D435 相机 launch、TF、内外参和 rosbag2 实验记录。
5. 从遥操作走向半自动：先做目标检测和 3D 定位提示，再做单臂辅助，最后做双臂任务分配和任务级状态机。

## 常见问题

`launcher_ui.py` 提示找不到 `install/setup.bash`：

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
colcon build --symlink-install
source install/setup.bash
```

真机模式提示缺少 `Robotic_Arm`：

```text
未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。
```

Controller topic 没有数据：

- 确认 UDP 发送端目标 IP 是运行 ROS2 的主机 IP。
- 确认端口是 `15000`，或 launch 与发送端端口一致。
- 用 `sample_udp_sender` 在本机验证接收链路。

机械臂不动：

- 确认 `grip=true`。
- 确认 `/xr_rm/<arm>/cmd_vel` 是否有非零速度。
- 确认目标 TCP 没有被工作空间边界或圆柱半径限制夹住。
- 确认真机 SDK 连接成功，且 RM75 没有报警或急停。