节卡MinoCobo机器人单目视觉抓取的ROS实现

最近做了一个机器人视觉抓取的项目，目标是让机器人准确地拿到桌子上摆放的咖啡，递给旁边的人。目前的实现结果是能够抓取到一定区域内任意摆放的纸杯，水平方向定位精度在20mm以内，竖直方向定位精度较差，会有20~35mm的误差，原因是竖直方向对应着相机的深度方向。

• 硬件：MiniCobo协作机器人、单目相机、大寰夹爪PGC-50、电脑
• 技术方案：ROS下的AprilTag视觉定位、机器人API接口使用、手眼标定

下面总结一下项目中一些代码使用和bug解决，大部分代码都是已经开源的方案，不过用的时候总会出现各式各样的问题，都是靠着其他人的博客和代码库的issue才解决的。

1. ApirlTag定位

1.1 参数设置

AprilTag有专门写了一个ROS版本的代码(https://github.com/AprilRobotics/apriltag_ros)，跑AprilTag代码之前得先装好相机驱动，并且做好相机标定。
启动节点前要先设置一下apirltag_ros/apriltag_ros/config目录下的tags.yaml和settings.yaml。
apirltag_ros/apriltag_ros/config/settings.yaml

tag_family:        'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12
tag_threads:       2          # default: 2
tag_decimate:      1.0        # default: 1.0

settings.yaml得注意tag_family和你打印的tag一致，一般好像都用36h11。
apirltag_ros/apriltag_ros/config/tags.yaml

standalone_tags:
  [
    # {id: 1, size: 0.058},   #size对应标签的大小
    # {id: 0, size: 0.048},
    # {id: 0, size: 0.04},
    # {id: 1, size: 0.04}, 
    # {id: 2, size: 0.04},
    # {id: 3, size: 0.04}, 
    # {id: 4, size: 0.04}
  ]
  tag_bundles:
  [
    {
      name: 'my_bundle',
      layout:
        [
          {id: 0, size: 0.044, x: 0.0000, y: 0.0000, z: 0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000},
          {id: 4, size: 0.044, x: 0.0300, y: -0.0830, z:  0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000},
          {id: 3, size: 0.044, x: -0.0300, y: -0.0830, z: 0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000},
          {id: 2, size: 0.044, x: 0.0300, y: 0.0830, z:  0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000},
          {id: 1, size: 0.044, x: -0.0300, y: 0.0830, z:  0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000}
        ]
    }
  ]

tags.yaml的话就得注意standalone_tags和tag_bundles的区别。

• 如果你只用一个tag来定位，就设置前面的standalone_tags，只需要将id改为你打印的tag的id，size和实际大小一致，单位m；
• 如果你用多个tag同时定位，目的应该是以为可以提高精度（不知道能不能，我没做实验），就设置后面的tag_bundles，我这里用了5个tag同时定位。第一个参数是主定位的二维码，最后识别输出的位置也就是第一个id的位置。它的xyz、四元数都是没有偏差（注意qw=1）。其他tag是用来辅助定位的，需要知道它们与主定位的id0之间的偏差，所以xyz和四元数都得是实际的偏差值。这里我是用直尺测量的tag之间的距离，因为都在一个平面上，所以四元数就是无旋转对应的数值。

1.2 usb相机启动

我源码安装了usb_cam驱动包，不过源码也没什么用，没看也没改，整个项目就只用到了usb_cam-test.launch一个文件。
usb_cam/launch/usb_cam-test.launch

<launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="camera_info_url" type="string" value="file:///home/jeremy/.ros/camera_info/head_camera.yaml"/>
    <param name="video_device" value="/dev/video2" />
    <param name="image_width" value="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="yuyv" />
    <param name="camera_frame_id" value="usb_cam" />
    <param name="io_method" value="mmap"/>
  </node>
  <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
    <remap from="image" to="/usb_cam/image_raw"/>
    <param name="autosize" value="true" />
  </node>
</launch>

第3行代码camera_info_url是加上了标定后相机参数的位置，源码应该没有这一句，我是看到手眼标定里面有个启动文件这样写了，就加上去了。（其实单目相机标定的作用我也没看出来，删掉这句我观察输出图像也没看到啥变化。但为了心理安慰，还是别删，要不然我精度又没了… …）
还有第4行的video_device，如果启动launch文件后发现打开的是笔记本自带相机（突然出现自己的脸，吓一跳），就把video2改成video0，或者video0改成video2，反正就是换一下。不过别改成video1，会说找不到的。

1.3 报错

1. [ WARN] [1639733001.211902775]: Requested description of standalone tag ID [0], but no description was found...
   这个问题是因为tags.yaml中没有正确设置二维码id，要注意id必须和打印出来二维码一致，还有就是单个tag设置standalone_tags，多个就设置后面的tag_bundles。

2. 启动相机后发现图像黑屏
   遇到这个问题我本来上网找半天，按照其他博主的方法改各种.launch文件中的参数，但是都没用，最后哭笑不得地发现是单目相机的光圈调到最小了，调大光圈就出现图像了，亏得我还linux和window下反复测试相机能不能用。

3. 输出位置信息都是0
   未进行相机标定，相机参数没有保存至.ros/camera_info目录下。

2. JAKA机器人API使用

JAKA有官方的API二次开发文档，python的文档写得详细一点，不过我的机器人控制代码用的C++写的，所以开发的时候我python和C++的API都用了一遍。一般用python先试一下，成功了再换成C++。

2.1 python二次开发

Linux平台下使用机器人API的话，需要将JAKA给的libjakaAPI.so, jkrc.so两个编译好的库放到和python代码相同的文件夹，运行代码前还需要代码文件夹路径添加到环境变量中，就是在终端中运行下面这行代码。我的代码就放在grab/script路径下。

export LD_LIBRARY_PATH=~/project/jaka/SendCoffee/src/grab/script

还有一点需要注意的是，节卡机器人的API使用环境是python3.5，如果你的linux默认python环境是2.7的话，可以用python3 ***.py的命令来运行代码。我python3的环境是3.6.9，与官方要求的3.5不一致，但是目前运行也没遇到问题。

2.2 C++二次开发

C++要编译，所以设置会比python麻烦，我是通过参考JAKA给的文件中的linux示例代码才编译成功的。具体步骤是先在grab功能包建立grab/include文件夹，放入JAKAZuRobot.h, jkerr.h, jktypes.h3个头文件。接着建立grab/lib文件夹，放入编译好的C++静态库libjakaAPI.a。最后只需要正确编写Cmake编译文件，就可以用catkin_make命令编译。哦对了，还得将你要运行的C++代码放到grab/src文件夹中。
grab/CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(grab)
find_package(catkin REQUIRED COMPONENTS 
   roscpp  
   rospy  
   std_msgs  
   apriltag_ros 
)
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES test
#  CATKIN_DEPENDS roscpp
#  DEPENDS system_lib
)
include_directories(
  ${catkin_INCLUDE_DIRS}
)
include_directories( "/usr/include/eigen3" )

# include 头文件目录
include_directories(include)
LINK_DIRECTORIES($(SRC_ROOT_PATH))
LINK_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}/lib)
LINK_LIBRARIES(pthread)
 
# 编译成可执行文件，如果编译动态库可以用：target_link_libraries
add_executable(control_robot src/control_robot.cpp)
target_link_libraries(control_robot jakaAPI)
target_link_libraries(control_robot  ${catkin_LIBRARIES})

find_package(Threads)
target_link_libraries (control_robot ${CMAKE_THREAD_LIBS_INIT})

3. 机器人手眼标定（眼在手外）

眼在手外的手眼标定是为了求解相机相对于机器人底部的位姿，在本项目中，AprilTag定位能够得到tag在相机坐标系下的位姿，而抓取目标与tag的相对位置是固定的。因此，完成手眼标定后，即可实现抓取目标在tag坐标系–>相机坐标系–>机器人坐标系的位姿转换。
求解得到抓取目标在机器人坐标系下的位置后，即可控制机器人运动至目标点，进行抓取。

3.1 标定方法

我用的是鱼香ROS写的handeye-calib包(https://gitee.com/ohhuo/handeye-calib)，代码给出了操作步骤的详细介绍，我是跟着在线标定的流程实现的。handeye-calib其实与easy_handeye功能包的原理一样，都是调用OpenCV的cv2.calibrateHandEye()函数完成手眼标定。easy_handeye有一个更好的可视化界面，但是相应的代码也更难读懂。
很多手眼标定博客都是用moveit驱动机器人，通过tf树来获取机器人位姿的，但是我不知道怎样用moveit，所以我决定直接调用JAKA SDK来获取机器人位姿，并且通过ros的geometry_msgs::PoseStamped消息格式发布话题/robot_pos。这样标定程序就能直接订阅机器人位姿话题，下面是修改后的手眼标定launch文件。
handeye-calib/launch/online/online_hand_to_eye_calib.launch

<launch>
    <!-- The arm marker in camera Pose Topic,Use ros geometry_msgs::Pose-->
    <arg   name="camera_pose_topic"   default="/aruco_single/pose" />
    <!-- The arm tool Pose Topic,Use ros geometry_msgs::Pose-->
    <arg   name="arm_pose_topic"   default="/robot_pos" />

    <node pkg="handeye-calib" type="online_hand_to_eye_calib.py" name="online_hand_to_eye_calib" output="screen" >
         <param name="arm_pose_topic" value="$(arg arm_pose_topic)" />
         <param name="camera_pose_topic" value="$(arg camera_pose_topic)" />
    </node>
</launch>

3.2 问题

1. no moudle named 'rospkg'
   将handeye-calib/src/handeye/online_hand_ro_eye_calib.py的第一句#!/usr/bin/env python3中的python3改为python，原因是ros melodic版本只支持python2，所以python3的代码无法使用ros包。
2. no moudle named 'transforms3d'
   通过pip install transforms3d安装一下该库，如果没有pip，请先安装pip，sudo apt-get install python-pip。如果手眼标定代码出现其他库找不到的错误，也可以先尝试用pip安装，另外网上搜索一下报错信息会找到很多安装库的教程。不过需要注意库需要安装到python2还是python3，python2就用pip，python3用pip3。
3. 'module' object has no attribute 'CALIB_HAND_EYE_TSAI'
   参考博主miss_future的kinova与intel realsense D435手眼标定中的错误2。
4. super().__init() Error: super() takes at least 1 argument(0 given)
   删除super().__init()，这一句代码是用来继承父类的初始化函数的，作者这里可能写错了。
5. 标定结果精度很差
   标定过程不应该大范围移动机器人，不同组记录之间的末端旋转角度要求比较大。提高标定精度的方法可参考博主Yoyo_wym的手眼标定(二): Tsai求解方法，我采用博主提到的措施后，标定结果的精度在10mm范围内。

收藏网址

• 相机标定棋盘格在线生成：https://calib.io/pages/camera-calibration-pattern-generator
• OpenMV生成ApirlTag.png图片 https://blog.csdn.net/u013952812/article/details/106766132
• ArUco markers在线生成：https://chev.me/arucogen/
• 旋转矩阵 欧拉角 四元数的在线转换 https://www.andre-gaschler.com/rotationconverter/