【手眼标定】ROS + usb_cam + aruco_ros + easy_handeye

【手眼标定】ROS + usb_cam + aruco_ros + easy_handeye_demo

注意：在手眼前先进行相机内参标定以及ArUco markers检测测试，测试成功后再进行手眼标定
ArUco markers检测参考链接

1. 下载运行easy_handeye_demo

这个库是easy_handeye的一个demo，不需要真实的机械臂就可以实现easy_handeye，并且easy_handeye_demo可以作为将easy_handeye与自己的机械臂整合的一个例子

下载easy_handeye_demo及附属包

easy_handeye_demo仓库地址

easy_handeye仓库地址

机械臂仿真iiwa_stack仓库地址

(1)创建工作空间，克隆仓库

mkdir -p easy_handeye_ws/src
cd easy_handeye_ws/src
git clone https://github.com/IFL-CAMP/iiwa_stack
git clone https://github.com/IFL-CAMP/easy_handeye
git clone https://github.com/marcoesposito1988/easy_handeye_demo

(2)安装依赖

cd .. #回到工作空间easy_handeye_ws
rosdep install -yir --ignore-src --from-paths src

(3)编译

catkin build

(4)启动仿真

source devel/setup.bash
# first perform the calibration,默认眼在手外
roslaunch easy_handeye_demo calibrate.launch

可以看到如下界面

拖动小球可以使机械臂移动，点击plan&Execute可以使机械臂运动到拖动到的位置

再开一个命令窗口，可以通过下面命令验证机械臂运动

# then, check the result
roslaunch easy_handeye_demo check_calibration.launch

可以看到第一个窗口执行plan&Execute，第二个窗口机械臂同步移动

到此easy_handeye_demo运行成功

2. 在仿真中手眼标定准备工作

启动相机

roslaunch usb_cam usb_cam-test.launch #根据实际的相机启动文件名进行修改

启动aruco_ros检测Marker,这里有问题看这篇文章

cd ~/aruco_ws
source devel/setup.bash
roslaunch aruco_ros single.launch

修改easy_handeye_demo的laucnh文件

复制 easy_handeye_demo/launch/calibrate.launch文件作修改，并命名为calibrate2.launch,修改以下参数

    <arg name="robot_base_frame" value="iiwa_link_0" /><arg name="robot_effector_frame" value="iiwa_link_ee" /><arg name="tracking_base_frame" value="usb_cam" /><arg name="tracking_marker_frame" value="aruco_marker_frame" />

tracking_base_frame: marker的坐标系的参考坐标系，这里写相机坐标系的参考坐标系，也就是相机坐标系。
tracking_marker_frame: marker 坐标系，即 aruco 设置的 marker_frame

启动标定文件进行标定

cd easy_handeye_ws
source devel/setup.bash
roslaunch easy_handeye_demo calibrate2.launch

正常情况下会出现四个界面，一个是相机画面，一个是同上文的rviz仿真界面，下面的两个标定界面

但是我的一开始没有出现这两个标定界面，查看运行终端的报错信息，发现缺少相应的安装包

安装pyqt5，由于我的ros版本为melodic，运行环境为python2.7，参考该篇博客直接安装PySide2即可

pip install PySide2

然后还缺少transforms3d包，直接安装

pip install transforms3d

然后重新运行calibrate2.launch就可以看到上面的标定界面了

但是rqt_easy_handeye没有菜单栏无法打开aruco码位姿检测图像

解决方法：新开一个命令窗口，打开rqt界面，菜单栏-plugins-topics-topics monitor,查看话题，打开菜单栏-plugins-visualization-image view，订阅/aruco_single/result话题，放入ArUco markers便可以检测到marker的id和坐标轴

2. 开始标定

（1）手动调节机械臂，使 aruco 二维码移动至相机视野中心处附近。点击 check starting pose，若检查成功，界面会出现： 0/17，ready to start
（2）依次点击 Next Pose，Plan，Execute，机械臂会移动至新的位置，若二维码在相机视野范围内，且能检测成功，则进行下一步
（3）界面 2 中点击 Take Sample，若 Samples 对话框中出现有效信息，说明第一个点标定成功
（4）重复执行步骤 2 和步骤 3，直至 17 个点全部标定完毕
（5）界面 2 中点击 Compute，则 Result 对话框中会出现结果
（6）界面 2 中 Save，会将结果保存为一个 YAML 文件，路径为 ~/.ros/easy_handeye

出现的问题：
第一步点击 check starting pose时，提示Can't calibrate from this position!，解决办法：拖动小球找到能够进行标定的位姿（有点麻烦，我也找了很久）

然后继续进行第二步，得到如下图

标定完成界面

3. 发布TF(待研究)

easy_handeye 功能包提供了 publish.launch 文件，可以将标定好的 TF 发布出。注意修改 namespace_prefix 参数与标定的 launch 文件中的保持一致，这样才能找到标定好的 YAML 文件。

roslaunch easy_handeye publish.launch eye_on_hand:=false namespace_prefix:=demo_handeyecalibration# 查看 TF（基坐标系到相机坐标系）
rosrun tf tf_echo /iiwa_link_0 /usb_cam