easy_handeye2_ws 项目说明

本工作区聚合了机器人末端姿态采集、棋盘格检测、相机驱动以及手眼标定的完整链路。以下文档帮助你快速了解各个节点/脚本的作用、启动方式与常见问题排查方法。

环境准备

• 操作系统：Ubuntu 22.04 + ROS 2 Humble（其它发行版请据此调整依赖包名）。
• 基础依赖：

  sudo apt update
  sudo apt install ros-humble-rclpy ros-humble-tf2-ros ros-humble-cv-bridge \
       ros-humble-image-geometry ros-humble-rclcpp-components python3-serial
  pip3 install --user opencv-python numpy

• 如果使用大恒相机，请安装官方 SDK 并确认 daheng_camera.xml 参数文件已根据实际型号配置。

编译与安装

cd ~/easy_handeye2_ws
colcon build
source install/setup.bash

如只需编译单个包，可追加 --packages-select <package_name>。

数据流概览

1. rmos_cam 提供工业相机或 USB 相机的采集组件，发布 /image_raw 图像。
2. my_robot_driver/chessboard_detector 在图像中检测棋盘格，并在 TF 中发布 camera_color_optical_frame → chessboard 变换。
3. my_robot_driver/robot_tf_publisher 从串口 IMU 读取末端姿态，发布 robot_base_frame → robot_effector_frame 变换。
4. handeye_sampler.py 监听两路 TF，人工输入 save 指令采集稳定姿态。
5. handeye_solver 读取采集结果，使用 OpenCV 提供的多种手眼标定算法求解 effector → camera 变换。

---

my_robot_driver 包

robot_tf_publisher

• 作用：从 /dev/ttyACM0 读取 IMU 自定义协议帧，解析四元数并发布 TF。
• 主要特性：
  • 支持 SOF/EOF 帧同步与缓存剔除；
  • 内置欧拉角镜像+常量补偿（当前对 pitch/yaw 取反，并叠加 135° yaw 偏置及 180° roll 翻转）；
  • 10 Hz 定时发布 robot_base_frame → robot_effector_frame，平移默认为零。
• 默认串口参数：115200 波特率、超时 100 ms，自定义帧长 25 字节（详见结构体说明）。
• 运行：

  ros2 run my_robot_driver robot_tf_publisher

• 常用参数调整：若设备路径或 SOF/EOF 标记不同，请编辑 my_robot_driver/robot_tf_publisher.py 中的 self.serial_port、SOF_MARKER、EOF_MARKER。
• 调试建议：
  • 使用 scripts/imu_frame_inspector.py 观测原始四元数、增量角度：

    ros2 run my_robot_driver robot_tf_publisher
    python3 src/my_robot_driver/scripts/imu_frame_inspector.py --port /dev/ttyACM0

  • 若 TF 始终未更新，请检查串口权限 (sudo usermod -a -G dialout $USER) 以及 IMU 是否持续输出。

chessboard_detector

• 作用：在输入图像流中检测棋盘格角点，求解位姿后发布 TF、Pose 与调试图像。

• 话题：

  • 订阅：/image_raw（可通过参数修改），可选订阅 CameraInfo；
  • 发布：/chessboard/pose、/chessboard/debug_image、/tf。

• 关键参数：

  参数	默认值	说明
  chessboard_width	8	内角点列数
  chessboard_height	6	内角点行数
  square_size	0.025	棋盘格边长 (m)
  image_topic	/image_raw	输入图像话题
  camera_info_yaml	空	指向 camera_info.yaml 直接加载内参
  use_camera_info_topic	False	若为 True，则等待 CameraInfo 话题
  enable_clahe / median_blur_kernel	True / 5	提升图像清晰度
  use_findchessboard_sb	True	常规检测失败时使用 findChessboardCornersSB 兜底
  camera_frame / chessboard_frame	camera_color_optical_frame / chessboard	TF 帧名

• 运行示例：

  ros2 run my_robot_driver chessboard_detector \
    --ros-args \
    -p image_topic:=/camera/color/image_raw \
    -p camera_info_yaml:=/path/to/camera_info.yaml \
    -p chessboard_width:=8 -p chessboard_height:=6 -p square_size:=0.03

  ros2 run my_robot_driver chessboard_detector   --ros-args   -p camera_info_yaml:=/home/hejie/easy_handeye2_ws/src/my_robot_driver/config/camera_info.yaml

• 调试技巧：

  • 使用 rqt_image_view /chessboard/debug_image 检查角点质量；
  • 若提示 "PnP 求解失败"，请确认相机内参是否加载正确；
  • 出现 tf2::ExtrapolationException 时，可强制使用节点时间戳，或检查上游图像时间是否回退。
  • ros2 run rqt_image_view rqt_image_view /chessboard/debug_image

---

rmos_cam 包

• 定位：通过组件容器加载大恒或 USB 相机驱动，统一发布 image_raw。
• 启动文件：ros2 launch rmos_cam cam.launch.py camera_type:=<usb|daheng>。
• 参数文件：
  • 大恒相机参数从 rmos_bringup/config/daheng_node.yaml 读取；
  • 额外的 config/daheng_camera.xml 提供 SDK 级别的宽高、曝光、增益等默认设置。
• 输出：
  • /daheng_node/image_raw 或 /usb_node/image_raw（可在 launch 中通过 remappings 适配）。
• 调试提示：
  • 启动前确保 LD_LIBRARY_PATH 已包含大恒 SDK；
  • 若容器未加载组件，可运行 ros2 component types/ros2 component list 检查；
  • USB 摄像头需系统识别为 Video4Linux 设备，可使用 v4l2-ctl --list-devices 排查。

---

手眼标定工具链

handeye_sampler.py

• 入口：在工作区根目录执行 python3 src/handeye_sampler.py（脚本内部自启 ROS2 节点）。

• 依赖帧：

  • 机器人姿态：base_frame → effector_frame（默认 robot_base → robot_effector，可通过参数覆盖）；
  • 目标姿态：camera_frame → target_frame（默认 camera_color_optical_frame → chessboard）。

• 采样逻辑：

  1. 节点以 poll_rate（默认 30 Hz）轮询 TF，并比较平移/旋转变化是否低于阈值；
  2. 连续满足 consecutive_required 次后提示 "Transforms stable"；
  3. 终端输入 save 手动保存当前姿态对，写入 output_file（JSON）；
  4. 若姿态与前一次过于接近，会拒绝保存以避免冗余。

• 常用参数：

  参数	默认值	说明
  translation_threshold	0.0008 m	判断稳定的平移阈值
  rotation_threshold_deg	0.15°	判断稳定的旋转阈值
  min_sample_interval	1.0 s	两次满足条件间隔
  max_samples	30	达到后自动退出
  output_file	handeye_samples.json	结果保存路径
  append	True	是否在已有文件末尾追加

• 建议流程：

  • 启动后等待终端出现 “Transforms stable” 提示，再输入 save；
  • 从不同方位/距离采集 ≥10 组数据以提高解算稳定性；
  • 采集完成后按 Ctrl+C 或输入 quit/exit 结束节点。

handeye_solver

• 入口：python3 handeye_solver.py --input handeye_samples.json --method park --save-yaml effector_to_camera.json。

• 算法选择：支持 tsai（默认）、park、horaud、andreff，底层调用 cv2.calibrateHandEye。

• 输出：

  • 终端打印平移/四元数；
  • 若提供 --save-yaml，会导出可供 TF 静态发布的 JSON/YAML 数据。

• 常见问题：

  • 少于 3 组样本将报错；
  • 姿态差异过小时结果不稳定，需采集更多不同姿态。

• 可选可视化：python3 handeye_samples_viz.py --input handeye_samples.json 可查看采样分布。

---

推荐使用流程

1. 启动相机：ros2 run rmos_cam daheng_camera 。
2. 启动棋盘格检测：ros2 run my_robot_driver chessboard_detector --ros-args ...，确认 /chessboard/debug_image 正常。
3. 启动 IMU TF 发布：ros2 run my_robot_driver robot_tf_publisher，使用 ros2 run tf2_ros tf2_echo robot_base_frame robot_effector_frame 验证输出。
4. 采集姿态样本：python3 src/handeye_sampler.py，在终端根据提示输入 save。
5. 求解标定：python3 handeye_solver.py --method park --save-yaml effector_to_camera.json。
6. 应用结果：将求得的平移/四元数加入机器人 TF 树，或通过 static_transform_publisher 发布。

---

故障排查备忘

• 串口读不到数据：检查串口权限、波特率、线缆连接；若缓冲区爆满，可降低定时器频率或在固件端减少广播频率。
• 棋盘格检测抖动：调整 translation_threshold/rotation_threshold_deg 使采样更严格，或在图像预处理参数中增大 CLAHE 的 clip_limit。
• TF 始终不更新：使用 ros2 run tf2_ros tf2_monitor 查看 TF 更新周期；如时间戳倒退，请确保所有节点使用同一时间基准。
• OpenCV 缺失：ImportError: cv2 时请 pip3 install opencv-python 或安装系统包 sudo apt install python3-opencv。

---

相关脚本补充

• my_robot_driver/scripts/imu_frame_inspector.py：串口原始数据调试助手。
• handeye_samples_viz.py：用于评估采样姿态覆盖范围。

欢迎根据实际项目补充示例配置、相机标定文件及许可证信息。若在使用中遇到新的问题，也建议把解决经验追加到本 README 中，便于后续协作。