464 lines
22 KiB
Markdown
Executable File
464 lines
22 KiB
Markdown
Executable File
# 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、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题
|
||
```
|
||
|
||
当前控制方式是“手柄相对位姿透传”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP，经过工作空间限幅、目标低通、姿态低通和单帧步长限制后，通过 `rm_movep_canfd` 下发目标位姿。松开 `grip`、UDP 超时或节点退出时会请求机械臂慢停。
|
||
|
||
## 当前范围
|
||
|
||
已完成：
|
||
|
||
- PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
|
||
- 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
|
||
- RM75 真机连接适配，包含 `rm_movep_canfd` 位姿透传、安全速度/加速度配置、可选初始化点位移动。
|
||
- 真机模式下，点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。
|
||
- Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
|
||
- 自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、`grip`、`trigger` 和 pose 诊断字段。
|
||
|
||
暂未完成：
|
||
|
||
- D405/D435 视频流、数据记录、相机标定和目标检测链路。
|
||
- 双臂碰撞模型、任务级状态机、自动采摘策略。
|
||
- PICO 端与 ROS 端的完整时间同步和状态回传。
|
||
|
||
## 项目结构
|
||
|
||
```text
|
||
src/
|
||
├── README.md # 项目主文档
|
||
├── CODEX.md # Codex/Claude Code 项目工作流和安全规则
|
||
├── docs/
|
||
│ └── pico_udp_sender_ubuntu22_setup.md # Ubuntu 22.04 下 PICO UDP Sender 配置教程
|
||
├── unity/
|
||
│ ├── XR_RM_PICO_UDP_Sender/ # PICO 4 Ultra UDP Sender Unity 工程
|
||
│ │ ├── Assets/Editor/ # Android/PICO 设置与 APK 构建菜单
|
||
│ │ ├── Assets/Scripts/ # UDP sender、配置面板、KeepAwake
|
||
│ │ ├── Assets/Resources/ # PICO 资源与 Roboto TMP 字体
|
||
│ │ ├── Packages/ # Unity package manifest
|
||
│ │ └── ProjectSettings/
|
||
│ └── 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、controller 位置/频率监控。
|
||
- `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 Target Velocity 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`。
|
||
|
||
`Simulation` 模式还提供 `Open Controller Position Monitor` 和 `Open Controller Hz Monitor`，用于快速看手柄位置字段和接收频率。
|
||
|
||
分屏监控依赖 `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 \
|
||
--pattern axis_sweep --seconds 60 --both-mode staggered
|
||
```
|
||
|
||
`sample_udp_sender` 默认使用 `axis_sweep` 成对扫轴轨迹，并在终端打印 `XR +X/-X/+Y/-Y/+Z/-Z` 标签。`--hand both --both-mode staggered --seconds 60` 会先左后右，适合肉眼确认左右臂方向；如果只想左右同时动，可用 `--both-mode synchronized`。需要检查末端姿态时可增加 `--rotation-pattern rpy_steps --rotation-amplitude-deg 25`。
|
||
|
||
观察：
|
||
|
||
```bash
|
||
ros2 topic echo /xr/left_controller
|
||
ros2 topic echo /xr/right_controller
|
||
ros2 topic echo /xr_rm/left_rm75/target_pose
|
||
ros2 topic echo /xr_rm/right_rm75/target_pose
|
||
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
|
||
```
|
||
|
||
单臂真机默认执行配置文件中的 `movej(initial_joint_pose)`；现场需要跳过时，显式传入
|
||
`move_to_initial_pose_on_connect:=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`。
|
||
- `udp_timer_hz`：UDP receiver 轮询频率，默认 `200.0`。
|
||
- `left_robot_ip`：左臂 IP，默认 `192.168.192.18`。
|
||
- `right_robot_ip`：右臂 IP，默认 `192.168.192.19`。
|
||
- `robot_port`：RM75 TCP 端口，默认 `8080`。
|
||
- `left_avoid_singularity` / `right_avoid_singularity`：左右臂避奇异参数，默认左 `0`、右 `1`。
|
||
- `avoid_singularity`：非空时覆盖左右臂避奇异参数。
|
||
- `frame_type`：`rm_movep_canfd` 坐标系类型，默认 `1`。
|
||
- `control_rate_hz`：`rm_movep_canfd` 目标位姿发送频率，默认 `90.0`。
|
||
- `follow`：传给 `rm_movep_canfd` 的跟随标志，默认 `false`。
|
||
- `configure_safety_limits`：连接真机后是否配置速度/加速度安全参数，默认 `true`。
|
||
- `enable_tool_control`：是否在遥操作节点内启用末端工具控制 topic，默认 `true`。
|
||
- `enable_trigger_gripper_control`：是否允许用 `trigger` 点击切换对应夹爪状态，默认 `true`。
|
||
- `trigger_close_threshold`：trigger 点击判定阈值，默认 `0.95`。
|
||
- `configure_peripheral_on_connect`：遥操作节点连接真机后是否配置末端外设，默认 `true`；工具控制会复用同一个 RealMan 连接，避免两个进程同时抢占同一机械臂。
|
||
- `move_to_initial_pose_on_connect`：连接后是否执行 `movej(initial_joint_pose)`；默认 `auto`，单臂配置启用、双臂配置禁用，也可显式传 `true`/`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=2`、右臂 `scissorgripper=1`，真机连接阶段会初始化外设，后续开合命令复用同一个 RealMan 连接。
|
||
|
||
重点控制参数：
|
||
|
||
- `controller_topic`：订阅的手柄话题。
|
||
- `scale`：手柄位移到 TCP 位移的比例。
|
||
- `target_filter_alpha` / `target_filter_alpha_fast`：目标 TCP 低通滤波系数，快速移动时自动使用更大的系数。
|
||
- `target_filter_fast_threshold_m`：进入快速滤波区间的目标变化阈值。
|
||
- `max_linear_speed`：目标位姿单帧步长限制对应的最大线速度。
|
||
- `enable_orientation_control`：是否把手柄相对旋转映射到 TCP 姿态。
|
||
- `orientation_filter_alpha` / `orientation_deadband_rad`：目标 TCP 姿态低通和死区。
|
||
- `max_orientation_speed`：目标姿态单帧步长限制对应的最大角速度。
|
||
- `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
|
||
- `cyl_radius_limit`：基座圆柱半径限制。
|
||
- `xr_to_robot_matrix`：`/xr/*_controller` Project 位移到 RM75 base 坐标的映射矩阵。
|
||
- `current_pose_poll_hz`：低频读取真机当前 TCP 的频率；控制中不再每帧阻塞读取状态。
|
||
- `mock_initial_pose`：mock 模式初始 TCP 位姿。
|
||
- `initial_joint_pose`：可选真机初始关节角。
|
||
|
||
当前 `/xr/*_controller` 的 Project 坐标约定：
|
||
|
||
- Project：`+X` 向右，`+Y` 向上，`+Z` 向后。
|
||
- Unity APK 的 `Project (+Z back)` 会把 PXR `pxr_predict` 原始坐标转换为 `project.x=native.z`、`project.y=native.y`、`project.z=-native.x`。
|
||
- `Source raw` 模式保留原始 pose source 坐标，只用于现场对照。
|
||
- 左臂映射：机器人位移增量 = `[-手柄y, 手柄z, -手柄x]`。
|
||
- 右臂映射：机器人位移增量 = `[手柄y, 手柄z, 手柄x]`。
|
||
|
||
如果 `/xr/*_controller.pose.position` 已符合 Project 坐标，但某个机械臂方向相反，只改对应臂的 `xr_to_robot_matrix` 符号，不要同时改多个控制参数。
|
||
|
||
## 末端工具开合
|
||
|
||
真机 launch 默认会在遥操作节点内启用工具控制。左/右手柄 `trigger` 从低于阈值按到 `>= 0.95` 时，会切换一次对应夹爪开/关状态，并保持到下一次点击。`grip` 仍只控制机械臂运动，不影响夹爪 trigger 切换。
|
||
|
||
也可以用 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` 模式里也有对应的 Tool Open/Close 命令项；`Dual Arm` 真机模式下可直接通过左右手柄 `trigger` 分别切换夹爪。
|
||
|
||
## UDP 数据格式
|
||
|
||
当前 Unity APK 每个周期发送一个双手柄 JSON 包：
|
||
|
||
```json
|
||
{
|
||
"t": 12.345,
|
||
"source_time": 12.345,
|
||
"seq": 42,
|
||
"frame_id": "xr_world",
|
||
"controllers": {
|
||
"left": {
|
||
"grip": true,
|
||
"trigger": 0.0,
|
||
"pos": [-0.12, 1.05, 0.30],
|
||
"quat": [0.0, 0.0, 0.0, 1.0],
|
||
"pose_valid": true,
|
||
"pose_source": "pxr_predict",
|
||
"tracking_state": 3,
|
||
"controller_status": 2,
|
||
"grip_value": 1.0,
|
||
"axis": [0.0, 0.0],
|
||
"buttons": {
|
||
"grip": true,
|
||
"primary": false,
|
||
"secondary": false,
|
||
"menu": false,
|
||
"axis_click": false
|
||
}
|
||
},
|
||
"right": {
|
||
"grip": true,
|
||
"trigger": 0.4,
|
||
"pos": [0.12, 1.05, 0.30],
|
||
"quat": [0.0, 0.0, 0.0, 1.0],
|
||
"pose_valid": true,
|
||
"pose_source": "unity_xr",
|
||
"tracking_state": 3,
|
||
"controller_status": -1,
|
||
"grip_value": 0.8,
|
||
"axis": [0.0, 0.0],
|
||
"buttons": {
|
||
"grip": true,
|
||
"primary": false,
|
||
"secondary": false,
|
||
"menu": false,
|
||
"axis_click": false
|
||
}
|
||
}
|
||
}
|
||
}
|
||
```
|
||
|
||
字段说明：
|
||
|
||
- `t` / `source_time`：Unity 端 `Time.realtimeSinceStartupAsDouble`，用于后续延迟分析。
|
||
- `seq`：Unity 端递增包序号，用于后续丢包分析。
|
||
- `frame_id`：默认 `xr_world`，会写入 `XrController.header.frame_id`。
|
||
- `grip`：运动使能。`true` 时进入相对位姿控制，`false` 时停止。
|
||
- `trigger`：扳机值，范围 `0.0-1.0`。真机模式下跨过 `0.95` 的上升沿会切换对应夹爪开/关状态。
|
||
- `pos`：手柄位置，长度 3。
|
||
- `quat`：手柄姿态四元数，默认按 `xyzw` 解析；遥操作节点会用 grip 锁定后的相对旋转控制 TCP 姿态。
|
||
- `pose_valid`：姿态是否可信。ROS 接收端看到 `false` 会强制 `grip=false`。
|
||
- `pose_source`：`pxr_predict`、`unity_xr`、`xrobotoolkit` 或 `none`，用于判断姿态来自 PICO 预测接口、Unity XR fallback 还是官方 XRoboToolkit SDK bridge。
|
||
- `tracking_state` / `controller_status`：Unity/PICO 侧追踪诊断值，只用于日志和排查。
|
||
- `grip_value`、`axis`、`buttons`：PICO 端输入诊断字段，当前不会写入 `XrController` 消息。
|
||
|
||
`udp_controller_receiver` 仍兼容调试用的单手柄包：可以直接发送带 `hand`、`pos`、`quat` 的 JSON object，也可以用 `controllers` list、顶层 `left/right`、`pose.position`、`position`、`p`、`q` 等常见字段。四元数默认按 `xyzw` 解析，也可通过 `quat_order:=wxyz` 切换。
|
||
|
||
PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并向 ROS2 主机发送 UDP 的详细步骤见 [docs/pico_udp_sender_ubuntu22_setup.md](docs/pico_udp_sender_ubuntu22_setup.md)。
|
||
|
||
## 官方 XRoboToolkit bridge
|
||
|
||
如果使用官方 XRoboToolkit APK 和 PC-Service，可以用 `xrobotoolkit_to_udp_bridge` 从本机 ROS Python 环境中的 `xrobotoolkit_sdk` 读取左右手柄数据，再转换成当前 `udp_controller_receiver` 支持的 UDP JSON。
|
||
|
||
正式运行时不要同时启动官方 `PXREAClientUnity` / `RobotLinuxDemo` 可视化窗口。`/opt/apps/roboticsservice/run3D.sh` 会启动这个可视化 demo，适合单独确认 PICO 与 PC-Service 已连接；bridge 遥操作链路中只需要 PC-Service。
|
||
|
||
运行前只保留一个 UDP 输入源。先清掉重复 bridge、sample sender 和官方 Unity 可视化 demo，再保留或启动 PC-Service：
|
||
|
||
```bash
|
||
pkill -f '[x]robotoolkit_to_udp_bridge'
|
||
pkill -f '[s]ample_udp_sender'
|
||
pkill -f '[R]obotLinuxDemo.x86_64'
|
||
pkill -f '[P]XREAClientUnity'
|
||
pgrep -af RoboticsServiceProcess || /opt/apps/roboticsservice/runService.sh
|
||
```
|
||
|
||
启动 ROS mock 接收链路：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
source install/setup.bash
|
||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
|
||
```
|
||
|
||
另开终端启动 bridge：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source ~/.bashrc
|
||
source /opt/ros/humble/setup.bash
|
||
source install/setup.bash
|
||
ros2 run xr_rm_input xrobotoolkit_to_udp_bridge \
|
||
--host 127.0.0.1 --port 15000 --hz 90
|
||
```
|
||
|
||
bridge 默认对 grip/trigger 做轻量滞回：`grip` 按下阈值 `0.90`、松开阈值 `0.75`；`trigger` 按下阈值 `0.95`、松开阈值 `0.75`。启动日志会打印 PID、UDP endpoint 和阈值，便于确认当前只运行了一个 bridge。
|
||
|
||
验证手柄数据是否进入 ROS：
|
||
|
||
```bash
|
||
ps -ef | grep -E 'xrobotoolkit_to_udp_bridge|sample_udp_sender|RobotLinuxDemo|PXREAClientUnity' | grep -v grep
|
||
ros2 topic hz /xr/left_controller
|
||
ros2 topic hz /xr/right_controller
|
||
ros2 topic echo /xr/left_controller --field pose.position
|
||
ros2 topic echo /xr/right_controller --field pose.position
|
||
ros2 topic echo /xr/left_controller --field grip
|
||
ros2 topic echo /xr/right_controller --field grip
|
||
ros2 topic echo /xr/right_controller --field trigger
|
||
```
|
||
|
||
`/xr/left_controller` 和 `/xr/right_controller` 持续刷新、位置随手柄移动变化、`grip` 随握持键切换，即表示官方 XRoboToolkit 数据已经进入当前遥操作输入层。
|
||
|
||
## 真机安全验证
|
||
|
||
第一次接真机时按这个顺序走：
|
||
|
||
1. 确认急停、网络、机械臂工作区和人员位置。
|
||
2. `launcher_ui.py` 中先 `Ping Left RM75` 或 `Ping Right RM75`。
|
||
3. 单臂启动，`move_to_initial_pose_on_connect:=false`。
|
||
4. 手握急停，按住 `grip` 后只做小幅单轴移动。
|
||
5. 逐个确认上/下、前/后、左/右方向。
|
||
6. 小角度转动手柄，确认 `/xr_rm/<arm>/target_pose` 姿态和 `/xr_rm/<arm>/cmd_vel.twist.angular` 变化符合预期。
|
||
7. 点击对应 `trigger`，确认每次点击都会切换对应夹爪状态，松开 trigger 后状态保持且左右不串臂。
|
||
8. 确认松开 `grip` 后机械臂慢停，`/xr_rm/<arm>/cmd_vel` 回到零；trigger 仍只影响夹爪，不影响机械臂运动门控。
|
||
9. 左右臂都确认后，再进入双臂模式。
|
||
|
||
当前项目没有双臂碰撞检测。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。
|
||
|
||
## 后续优化路线
|
||
|
||
为了达到“稳定可用的双臂 XR 遥操作/采摘平台”，建议按下面顺序推进：
|
||
|
||
1. 稳定 PICO 数据链路：利用 `seq`、`source_time`、`pose_valid` 做频率、延迟、丢包和追踪状态统计，记录 `/xr/*_controller`、`/xr_rm/*/raw_target_pose`、`/xr_rm/*/target_pose`、`/xr_rm/*/target_clamped`、`/xr_rm/*/current_pose`。
|
||
2. 提升真机安全性：增加启动前安全检查、软件急停 topic、UI Stop 状态提示、双臂中间区域互斥边界和速度/加速度限幅。
|
||
3. 细化末端执行器：增加夹爪状态反馈、力控比例、安全上限和现场可视化提示。
|
||
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` 在本机验证接收链路。
|
||
- 如果 Unity HUD 显示某个手柄 `invalid none`，ROS 侧会把该手柄 `grip` 强制置为 `false`。
|
||
|
||
机械臂不动：
|
||
|
||
- 确认 `grip=true`。
|
||
- 确认 `udp_controller_receiver` 终端没有持续 `pose_valid=false` 日志；该字段不会写入 `XrController` 消息，但会让接收端强制停止。
|
||
- 确认 `/xr_rm/<arm>/raw_target_pose` 与 `/xr_rm/<arm>/target_pose` 是否在变化。
|
||
- 确认 `/xr_rm/<arm>/target_clamped` 是否持续为 `true`，如果是，目标 TCP 可能被工作空间、圆柱半径或单帧步长限制夹住。
|
||
- 确认真机 SDK 连接成功，且 RM75 没有报警或急停。
|