RDK X5 + Gemini 2 + 机械臂抓取工程说明

• 使用 RDK X5 + ROS 2 Humble
• 使用 Orbbec Gemini 2 深度相机
• 使用一套 普通 USB 相机 + 深度相机 + 底盘 + 机械臂 的比赛/演示系统
• 需要做 桌面平面抓取标定、深度点抓取、整车任务联动

---

1. 工程整体结构

1.1 目录说明

├── arm_plane_calib/
│   ├── biaodi.png                       # 标定点示意图/参考图
│   ├── collect_plane_calib.py           # 平面采样脚本
│   ├── fit_plane_calib.py               # 拟合脚本
│   ├── predict_plane_cmd.py             # 点击像素预测舵机命令脚本（只预测、不直接发串口）
│   ├── depth_click_to_learm_cmd_node.py # 点击像素后直接预测并下发串口命令
│   ├── model.yaml                       # 拟合得到的模型参数
│   ├── samples.yaml                     # 采样得到的样本数据
│   └── readme.md                        # 原始说明
│
└── dev_ws/
    ├── README_RDK_X5.md                # 原始说明
    ├── scripts/
    │   └── setup_orbbec_udev.sh        # 安装 Orbbec udev 规则
    └── src/
        ├── depth_target_detector       # 深度目标检测
        ├── lerobot_arm_controller      # 机械臂控制
        ├── mission_manager             # 任务状态机
        ├── orbbec_camera               # Gemini 2 驱动
        ├── orbbec_camera_msgs          # Orbbec 消息定义
        ├── origintank_base             # 底盘驱动
        ├── origintank_bringup          # 总启动入口
        └── signal_board_detector       # 普通相机识别信号牌

1.2 功能分层

整套工程可以分成两大部分：

1. 整车/整系统运行部分：位于 dev_ws

   • 底盘运动
   • 普通 USB 相机识别信号牌
   • Gemini 2 深度点检测
   • 机械臂串口抓取控制
   • mission_manager 统一调度

2. 桌面抓取平面标定部分：位于 arm_plane_calib

   • 鼠标点击图像中的桌面点
   • 利用深度图和相机内参反算 x_cam/y_cam/z_cam
   • 手动记录该点对应的机械臂命令向量
   • 用二次多项式建立“相机平面点 -> 舵机命令”的映射
   • 运行时点击任意目标点，即可预测机械臂舵机命令

---

2. ROS 2 功能包

2.1 origintank_base

底盘驱动包，负责底盘控制和基础传感器数据发布：

• /odom
• /imu
• /ultrasonic/front/range

2.2 origintank_bringup

总启动入口，负责把底盘、普通 USB 相机、Orbbec 相机、深度检测、机械臂控制和任务状态机串起来。

2.3 signal_board_detector

普通 USB 相机识别信号牌，输入通常为：

• /image

输出识别结果给任务状态机。

2.4 depth_target_detector

基于深度图提取最近目标，发布：

• /depth_target/point
• /depth_target/point3d
• /depth_target/debug_image

其中：

• /depth_target/point 表示目标距离和归一化偏移
• /depth_target/point3d 表示目标在相机坐标系下的三维点 (x_cam, y_cam, z_cam)

2.5 lerobot_arm_controller

机械臂串口控制与抓取流程执行节点。

主要订阅：

• /arm_command
• /depth_target/point3d

主要发布：

• /arm/status
• /arm/debug_target

2.6 mission_manager

任务状态机，统一调度底盘、识别、抓取等流程。

默认配置中 autostart: false，因此一般需要手动发送启动命令。

2.7 orbbec_camera 与 orbbec_camera_msgs

从 Orbbec SDK ROS2 工程中裁剪出来的 Gemini 2 驱动与消息定义，不再依赖系统额外安装的 ros-humble-orbbec-camera。

• gemini2.launch.py
• Gemini 2 配置文件
• 必要 SDK 源码
• 消息与服务定义
• udev 规则 99-obsensor-libusb.rules

---

3. 硬件与控制链路

3.1 当前机器已确认的设备映射

按你现场提供的枚举结果：

• lsusb
  • Orbbec Gemini 2：2bc5:0670
  • 普通 USB 摄像头：0c45:6341
  • 两个 CH340 串口：1a86:7523
• ls /dev/ttyUSB*
  • /dev/ttyUSB0
  • /dev/ttyUSB1

结合你的实际接线，当前设备映射建议固定写成：

设备	当前接口
底盘控制板	/dev/ttyUSB0
机械臂控制板	/dev/ttyUSB1
普通 USB 相机	/dev/video0
深度相机	/dev/video1

其中：

• 机械臂默认波特率：9600
• 底盘串口仍按底盘驱动配置处理（当前 launch 默认 B115200）

整体链路如下：

• 底盘运动、编码器、IMU、超声波 -> origintank_base
• 普通 USB 相机 -> signal_board_detector
• Gemini 2 深度相机 -> orbbec_camera
• 深度目标点 /depth_target/point3d -> lerobot_arm_controller
• mission_manager 统一调度底盘、识别和抓取

---

4. 环境与依赖

4.1 系统环境

建议环境：

• Ubuntu 22.04
• ROS 2 Humble
• Python 3

4.2 Python/ROS 依赖

从当前代码和 package.xml 看，至少涉及以下依赖：

• rclpy
• rclcpp
• sensor_msgs
• geometry_msgs
• nav_msgs
• std_msgs
• tf2_ros
• cv_bridge
• python3-opencv
• python3-numpy
• python3-serial
• yaml

如果是单独运行 arm_plane_calib 目录下脚本，重点要保证：

• cv_bridge
• opencv-python 或系统 OpenCV
• numpy
• PyYAML
• pyserial（仅 depth_click_to_learm_cmd_node.py 需要）

---

5. 编译前准备

5.1 安装 Orbbec 的 udev 规则

在 dev_ws 根目录下执行：

cd ~/dev_ws
chmod +x scripts/setup_orbbec_udev.sh
./scripts/setup_orbbec_udev.sh

如果你的工作目录不是 ~/dev_ws，请切换到实际的 dev_ws 根目录再执行。

脚本本质上会把：

• src/orbbec_camera/scripts/99-obsensor-libusb.rules

复制到：

• /etc/udev/rules.d/99-obsensor-libusb.rules

然后刷新 udev 规则。

如果 Gemini 2 已插上，建议重新插拔一次。

5.2 编译工作空间

cd ~/dev_ws
source /opt/ros/humble/setup.bash
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
source install/setup.bash

5.3 内存不足时增加临时 swap

Orbbec 相关包体积较大，内存不足时可以临时加 8G swap：

free -h
swapon --show

fallocate -l 8G /swapfile || dd if=/dev/zero of=/swapfile bs=1M count=8192
chmod 600 /swapfile
mkswap /swapfile
swapon /swapfile

free -h
swapon --show

编译完成后可关闭：

swapoff /swapfile
rm -f /swapfile

---

6. 整套系统启动方式

6.1 启动整车任务系统

ros2 launch origintank_bringup rdk_x5_auto_mission.launch.py \
  cmd_port:=/dev/ttyUSB0 \
  arm_port:=/dev/ttyUSB1 \
  usb_video_device:=/dev/video0

这里需要把“代码默认值”和“当前机器实际接线”分开看：

• 当前机器实际接线/枚举（按你提供的 ls /dev/ttyUSB* 与 lsusb）
  • 底盘控制口：/dev/ttyUSB0
  • 机械臂控制口：/dev/ttyUSB1
  • 普通 USB 相机：/dev/video0
  • 深度相机：/dev/video1
• 当前代码默认值
  • rdk_x5_auto_mission.launch.py 里 arm_port 默认值还是 /dev/ttyUSB2
  • lerobot_arm_controller/config/lerobot_arm_controller.yaml 里 port 默认值也是 /dev/ttyUSB2

6.2 默认会启动哪些内容

默认参数下会启动：

• 底盘驱动
• 普通 USB 相机
• Gemini 2
• 深度目标检测
• 机械臂控制
• 任务状态机

对应开关参数：

• use_usb_camera
• use_orbbec_camera
• use_depth_detector
• use_arm_controller
• use_hobot_usb_cam

如果只想局部调试，可以在 launch 参数里关闭对应模块。

---

7. 单独测试 Gemini 2

7.1 启动 Gemini 2

ros2 launch origintank_bringup orbbec_gemini2.launch.py

这个 launch 本质上是对 orbbec_camera/launch/gemini2.launch.py 的封装。

默认传递的典型参数包括：

• camera_name:=camera
• enable_point_cloud:=false
• enable_colored_point_cloud:=false
• depth_registration:=true

7.2 查看设备

ros2 run orbbec_camera list_devices_node

7.3 查看深度工作模式

ros2 run orbbec_camera list_depth_work_mode_node

---

8. 任务控制与关键话题

8.1 手动启动/停止任务

启动：

ros2 topic pub --once /mission/start std_msgs/msg/Bool "{data: true}"

停止：

ros2 topic pub --once /mission/start std_msgs/msg/Bool "{data: false}"

8.2 关键话题总表

底盘与基础传感器：

• /odom
• /imu
• /ultrasonic/front/range

普通相机与识别：

• /image_raw
• /signal_board/result

Gemini 2 深度相关：

• /camera/color/image_raw
• /camera/depth/image_raw
• /camera/depth/camera_info
• /depth_target/debug_image
• /depth_target/point
• /depth_target/point3d

机械臂相关：

• /arm_command
• /arm/status
• /arm/debug_target

任务状态机：

• /mission/start
• /mission_manager/state

8.3 可先检查的话题

ros2 topic list
ros2 topic echo /camera/depth/camera_info
ros2 topic echo /depth_target/point3d
ros2 topic echo /arm/status

---

9. 需要现场标定/修改的关键配置

9.1 深度目标检测参数

文件：

• src/depth_target_detector/config/depth_target_detector.yaml

当前主要参数：

• min_depth_m: 0.10
• max_depth_m: 1.20
• roi_x_ratio: 0.25
• roi_y_ratio: 0.25
• roi_w_ratio: 0.50
• roi_h_ratio: 0.50
• nearest_band_m: 0.03
• min_cluster_pixels: 80

含义补充：

• roi_*：只在图像中央某个区域寻找目标，减少边缘干扰
• nearest_band_m：以最近深度为基准，取一个厚度带做连通域提取
• min_cluster_pixels：过滤太小的噪声区域

9.2 机械臂控制参数

文件：

• src/lerobot_arm_controller/config/lerobot_arm_controller.yaml

当前重点参数：

• 串口：port、baud_rate
• 抓取动作：move_time_ms、settle_sec
• 安全位与 home 位：safe_z_cm、home_x_cm、home_y_cm、home_z_cm
• 抓取前/最终高度偏移：grasp_pre_z_offset_cm、grasp_final_z_offset_cm
• 夹爪开合角：gripper_open_angle_deg、gripper_close_angle_deg
• 相机到机械臂外参：
  • camera_to_arm_x_m
  • camera_to_arm_y_m
  • camera_to_arm_z_m
• 抓取补偿：
  • grasp_forward_extra_m
  • grasp_lateral_extra_m
  • grasp_height_extra_m
• 平台和色块放置点：
  • platform_left_*
  • platform_right_*
  • blue_zone_*
  • green_zone_*

9.3 mission_manager 参数

文件：

• src/mission_manager/config/mission_manager.yaml

当前重点参数：

• autostart: false
• approach_speed_mps
• slope_speed_mps
• lateral_speed_mps
• turn_speed_radps
• yaw_tolerance_deg
• ultrasonic_stop_distance_m
• signboard_stop_distance_m
• board_hold_sec
• alignment_tolerance
• max_wait_signal_board_sec

---

10. arm_plane_calib 的作用与适用场景

这个目录不是整车运行必须的 ROS 包，而是一组用于桌面平面抓取快速标定与验证的独立脚本。 后续可根据arm_plane_calib来创建一个可正常使用的ROS2功能包

11. arm_plane_calib 各脚本说明

11.1 collect_plane_calib.py

作用：

• 订阅彩色图、深度图、相机内参
• 鼠标点击图像中的标定点
• 自动计算点击点的相机坐标 X_cam, Y_cam, Z_cam
• 终端手动输入该点对应的机械臂命令向量
• 自动保存到 samples.yaml

默认参数：

• color_topic: /camera/color/image_raw
• depth_topic: /camera/depth/image_raw
• camera_info_topic: /camera/depth/camera_info
• save_path: samples.yaml
• depth_scale: 0.001
• min_depth_m: 0.05
• max_depth_m: 1.50
• median_kernel: 5
• target_count: 9

11.2 fit_plane_calib.py

作用：

• 读取 samples.yaml
• 用二次多项式最小二乘拟合
• 生成 model.yaml

特征向量固定为：

[1, x, y, x², x*y, y²]

输出模型包含：

• feature_order
• weights
• cmd_dim
• sample_count
• mae_per_dim
• rmse_per_dim
• z_ref_median

补充说明：

• 脚本要求至少 6 个样本 才能拟合
• 实际建议 9~16 个样本
• mae_per_dim 和 rmse_per_dim 可以直接用来判断每个舵机维度拟合得好不好
• z_ref_median 是采样点深度中位数，可作为参考平面高度

11.3 predict_plane_cmd.py

作用：

• 运行时点击图像中的任意目标点
• 自动取深度、反算相机坐标
• 用 model.yaml 预测机械臂命令向量
• 在终端打印结果

补充说明：

• 这个脚本只预测、不直接发串口
• 当前代码中，模型输出顺序固定按：

6, 5, 4, 3

• 但打印时会按你更容易读的顺序输出：

3, 4, 5, 6

• 当前只对 3/4/5/6 号舵机做处理，且统一限幅：

servo_min = 125
servo_max = 875

所以如果你的机械臂舵机顺序或限位不是这样，需要同步改代码。

11.4 depth_click_to_learm_cmd_node.py

作用：

• 点击图像中的目标点
• 自动取深度、预测命令向量
• 直接通过串口下发机械臂多舵机运动命令

它相当于把“预测”和“发命令”合成一步，适合快速联调。

补充说明：

• 当前代码默认串口参数：
  • serial_port: /dev/ttyUSB0
  • baud_rate: 9600
• 当前机器实际建议参数（按你现场接线）：
  • serial_port: /dev/ttyUSB1
  • baud_rate: 9600
• 默认运动时间：
  • move_time_ms: 800
• 默认 auto_send: true，点击后会立刻发送
• 默认舵机顺序：

servo_ids = [6, 5, 4, 3]

• 默认舵机限位：

servo_min = [125, 125, 125, 125]
servo_max = [875, 875, 875, 875]

• 按键行为：
  • q：退出
  • s：对上次结果再次发送一次
  • c：清除上次点击显示

这里还要特别补一个容易混淆的点：

• 从 arm_plane_calib 目录脚本看，机械臂串口控制默认按 9600 处理
• 但 dev_ws/src/lerobot_arm_controller 当前代码参数里写的是 115200
• 你现场确认的机械臂默认波特率是 9600

因此，当前文档以你的实际硬件为准：机械臂默认波特率按 9600 使用。若继续使用 lerobot_arm_controller，建议把它的 baud_rate 参数同步改成 9600，避免“标定脚本能通、任务系统不通”的情况。

---

12. 平面标定推荐流程

12.1 准备标定点

建议在桌面贴 9 个点，组成 3x3：

1   2   3
4   5   6
7   8   9

要求：

• 尽量铺满机械臂实际抓取区域
• 点位不要太挤
• 尽量位于同一平面

12.2 启动 Gemini 2

source /opt/ros/humble/setup.bash
source ~/dev_ws/install/setup.bash
ros2 launch origintank_bringup orbbec_gemini2.launch.py

12.3 运行采样脚本

cd ~/arm_plane_calib
python3 collect_plane_calib.py

12.4 逐点采样

每个点执行一次：

1. 鼠标点击图像里的桌面点
2. 终端会打印该点的 X/Y/Z
3. 手动把机械臂末端移动到该点正上方
4. 输入该点对应的命令向量

例如输入：

520,610,430,500

当前样例语义是：

按模型顺序：6轴, 5轴, 4轴, 3轴

夹爪开合通常不参与平面拟合，抓取时再额外处理。

12.5 自动保存样本

脚本每记录一个样本都会自动写入 samples.yaml。

采满 9 个点后会自动退出。

12.6 执行拟合

python3 fit_plane_calib.py samples.yaml model.yaml

成功后会输出每维的 MAE / RMSE。

12.7 测试预测

python3 predict_plane_cmd.py

点击图像任意位置，会打印类似：

点击像素: (694, 230)
相机坐标: X=0.0267, Y=-0.0681, Z=0.3600
预测机械臂命令向量(按模型顺序 6,5,4,3): 501, 874, 385, 393
舵机列表: 3:393,4:385,5:874,6:501

12.8 测试“点击即发命令”

python3 depth_click_to_learm_cmd_node.py

如果串口、波特率、舵机顺序都正确，点击后就会直接下发多舵机运动命令。

---

13. 样本文件与模型文件格式说明

13.1 samples.yaml

每个样本包含：

• u, v：点击像素坐标
• x_cam, y_cam, z_cam：相机坐标系下三维点
• arm_cmd：该点对应的机械臂命令向量

顶层还包含：

• cmd_dim

13.2 model.yaml

当前模型文件中记录：

• 特征顺序 ['1', 'x', 'y', 'x2', 'xy', 'y2']
• 权重矩阵 weights
• 输出维度 cmd_dim
• 样本数 sample_count
• mae_per_dim
• rmse_per_dim
• z_ref_median

这意味着当前平面拟合实际上是：

cmd = [1, x, y, x², x*y, y²] @ W

---

14. 机械臂串口控制补充说明

14.1 depth_click_to_learm_cmd_node.py 中的串口协议

代码中构造的帧格式为：

55 55 Length CMD Params...

多舵机控制使用：

• CMD = 0x03

参数格式包含：

• 舵机数量
• 运动时间低高字节
• 每个舵机的 id + pulse低字节 + pulse高字节

14.2 为什么需要注意舵机顺序

平面拟合得到的只是“命令向量”，它本身并不知道哪一维对应哪一个舵机。

所以你必须保证以下三者一致：

1. 采样时你输入 arm_cmd 的顺序
2. model.yaml 的输出维度顺序
3. 实际发送串口时的 servo_ids 顺序

只要这三者有一处不一致，就会出现“预测看起来有值，但机械臂动作完全不对”的情况。

---

16. 常见检查项

16.1 相机有画面但点不到有效深度

优先检查：

• /camera/depth/image_raw 是否正常
• /camera/depth/camera_info 是否正常
• 点击位置是否超出有效深度范围
• depth_scale 是否正确
• min_depth_m / max_depth_m 是否设置过窄

16.2 点击后有预测值，但机械臂动作不对

优先检查：

• servo_ids 顺序是否和采样时输入顺序一致
• servo_min / servo_max 是否限幅过死
• 波特率是否匹配
• 串口是否接对
• 发送协议是否和控制板一致

16.3 整车能跑，但机械臂不动

优先检查：

• lerobot_arm_controller 的 port 是否正确
• 当前运行参数是否已经改成 /dev/ttyUSB1（不要沿用代码里的旧默认值 /dev/ttyUSB2）
• /depth_target/point3d 是否真的有数据
• /arm/status 是否一直停在 busy 或没有变化

16.4 编译 Orbbec 包报内存不足

直接先加 swap，再编译。

16.5 标定效果差

可优先从这几个方向优化：

• 采样点从 9 点增加到 12~16 点
• 采样区域尽量铺满工作区
• 点击点尽量准确
• 输入的机械臂命令值保持一致的姿态基准
• 保证目标平面高度基本一致

---

17. 当前工程里需要注意的不一致点

这里把我在合并时发现的几个容易踩坑的地方单独列出来：

17.1 串口口名要按当前机器枚举，不要只看旧文档

你当前机器现场枚举已经确认：

• 底盘：/dev/ttyUSB0
• 机械臂：/dev/ttyUSB1

但 dev_ws 里的部分 launch/config 仍保留着 /dev/ttyUSB2。因此：

• 直接运行时，要么显式传 arm_port:=/dev/ttyUSB1
• 要么把源码默认值统一改成 /dev/ttyUSB1

17.2 机械臂波特率要统一到 9600

• arm_plane_calib/depth_click_to_learm_cmd_node.py：默认 9600
• arm_plane_calib/learm_test.py：默认 9600
• lerobot_arm_controller 当前代码参数：115200
• 你现场确认的机械臂默认波特率：9600

所以现阶段推荐统一按 9600 配置机械臂相关节点。

17.3 相机设备号按当前机器写法

你当前机器现场信息为：

• 普通 USB 相机：/dev/video0
• 深度相机：/dev/video1

不过在 ROS 2 实际运行中，Gemini 2 主要还是通过 orbbec_camera 驱动接管，不一定直接依赖 /dev/video1 做图像采集入口；/dev/video1 更适合作为设备枚举和排障参考。

17.4 learm_test.py 实际存在

之前旧说明里这个文件容易被忽略，但你当前压缩包里实际有：

• arm_plane_calib/learm_test.py

它适合用来做最基础的串口链路与舵机动作联调。

18. 推荐的最小联调顺序

如果你要从零开始快速打通，建议按下面顺序：

1. 单独启动 Gemini 2，确认彩色图/深度图/相机内参正常
2. 跑 collect_plane_calib.py 采 9~16 个点
3. 跑 fit_plane_calib.py 生成 model.yaml
4. 跑 predict_plane_cmd.py 看预测值是否合理
5. 跑 depth_click_to_learm_cmd_node.py 验证点击后是否能直达目标附近
6. 再接入 lerobot_arm_controller
7. 最后再接入 mission_manager 做整车联调

这样排查最快，也最容易把问题切分清楚。