UR3 + Kinect2 + aruco_ros + easy_handeye 手眼标定

参考链接
参考链接
之前做过一段时间easy_handeye手眼标定，具体看下面文章
ros使用相机usb_cam标定内参
ros使用usb摄像头追踪ArUco markers
【手眼标定】ROS + usb_cam + aruco_ros + easy_handeye_demo
之前用的是仿真进行手眼标定，这次用真实机械臂进行 easy_handeye手眼标定

配置环境

安装UR3驱动等

mkdir -p ~/ur_ws/src
cd ~/ur_ws/src
# clone the driver, ur_modern_driver已经废弃
git clone https://github.com/UniversalRobots/Universal_Robots_ROS_Driver.git 

# clone the description. Currently, it is necessary to use the melodic-devel branch.
git clone -b melodic-devel https://github.com/ros-industrial/universal_robot.git src/universal_robot

# install dependencies
sudo apt update -qq
rosdep update
cd ~/ur_ws
rosdep install --from-paths src --ignore-src -y

# build the workspace
catkin_make

# activate the workspace (ie: source it)
source devel/setup.bash

更多安装信息请到官方仓库查看Universal_Robots_ROS_Driver

安装iai_kinect2 （kinect v2的启动节点）

在opencv4下用官方的iai_kinect2库标定有问题，所有改为安装IAI Kinect2 for OpenCV 4，将该仓库直接克隆在ur_ws/src进行编译，教程链接
然后标定相机，教程链接

安装ros-noetic-visp

因为我用的是ubuntu20.04，ros是noetic版本，故安装命令为

sudo apt-get install ros-noetic-visp

安装vision_visp

cd ~/ur_ws/src
git clone -b noetic-devel https://github.com/lagadic/vision_visp.git
catkin_make --pkg visp_hand2eye_calibration

安装aruco_ros

cd ~/ur_ws/src
git clone -b noetic-devel https://github.com/pal-robotics/aruco_ros
catkin_make

安装easy_handeye

cd ~/ur_ws/src
git clone https://github.com/IFL-CAMP/easy_handeye
cd ..  # now we are inside ~/ur_ws
rosdep install -iyr --from-paths src
catkin_make

验证环境

验证aruco_ros

• 修改aruco_ros/aruco_ros/launch/single.launch文件

将图像和相机信息重新映射到kinect节点发布的主题：
（如果使用的是realsense相机或者其他相机，可以先查看相机对应的话题修改以下参数）

<remap from="/camera_info" to="/kinect2/hd/camera_info" />
<remap from="/image" to="/kinect2/hd/image_color_rect" />

适当修改相应的camera_frame，marker_frame，reference_frame，修改结果如下：

<launch>

  <arg name="markerId"        default="26"/>
  <arg name="markerSize"      default="0.05"/>    <!-- in m -->
  <arg name="eye"             default="left"/>
  <arg name="marker_frame"    default="aruco_marker_frame"/>
  <arg name="ref_frame"       default="kinect2_rgb_optical_frame"/>  <!-- leave empty and the pose will be published wrt param parent_name -->
  <arg name="corner_refinement" default="LINES" /> <!-- NONE, HARRIS, LINES, SUBPIX -->


  <node pkg="aruco_ros" type="single" name="aruco_single">
    <remap from="/camera_info" to="/kinect2/hd/camera_info" />
    <remap from="/image" to="/kinect2/hd/image_color_rect" />
    <param name="image_is_rectified" value="True"/>
    <param name="marker_size"        value="$(arg markerSize)"/>
    <param name="marker_id"          value="$(arg markerId)"/>
    <param name="reference_frame"    value="$(arg ref_frame)"/>   <!-- frame in which the marker pose will be refered -->
    <!-- <param name="camera_frame"       value="stereo_gazebo_$(arg eye)_camera_optical_frame"/> -->
    <param name="camera_frame"       value="kinect2_rgb_optical_frame"/>
    <param name="marker_frame"       value="$(arg marker_frame)" />
    <param name="corner_refinement"  value="$(arg corner_refinement)" />
  </node>

</launch>

/camera_info: 填入相机的内参 topic
/image: 图像 topic
reference_frame: marker 坐标系的参考坐标系，我们要获得的是marker 和相机的相对位姿，所以这里设置为相机坐标系，即和 camera_frame 一样。
camera_frame: 相机坐标系，随便写个名字
marker_frame：marker 坐标系，随便写个名字

• 打印ArUco markers，

在ArUco markers生成网站中下载aruco marker打印
Dictionary:选择Original ArUco，不然可能识别不到
Marker ID:上面single.launch文件中的markerId，根据上面id为26
Marker size, mm:上面single.launch文件中的markerSize，注意单位，根据上面size为50mm

• 查看检测结果

cd ~/ur_ws
source devel/setup.bash
roslaunch kinect2_bridge kinect2_bridge.launch
# 新窗口
source devel/setup.bash
roslaunch aruco_ros single.launch
# 新窗口
rqt_image_view

rqt_image_view左边订阅的节点为 /aruco_single/result下方为/aruco_single/result_mouse_left

现在已经可以检测到aruco标签，下面就正式开始进行手眼标定操作（注意aruco marker要贴在末端位置）

测试机械臂

• 测试真实机械臂

cd ~/ur_ws
source devel/setup.bash
# 启动硬件驱动节点
roslaunch ur_robot_driver ur3_bringup.launch robot_ip:=192.168.56.101
# 新开窗口
source devel/setup.bash
# 启动Moveit节点
roslaunch ur3_moveit_config moveit_planning_execution.launch
# 新开窗口
source devel/setup.bash
# 启动rviz节点
roslaunch ur3_moveit_config moveit_rviz.launch

此时rviz与真实机械臂位姿一致

• 测试gazebo仿真机械臂

cd ~/ur_ws
source devel/setup.bash
# gazebo中启动机械臂
roslaunch ur_gazebo ur3_bringup.launch
# 新开窗口
source devel/setup.bash
# 启动Moveit节点
roslaunch ur3_moveit_config moveit_planning_execution.launch sim:=true
# 新开窗口
source devel/setup.bash
# 启动rviz节点
roslaunch ur3_moveit_config moveit_rviz.launch

此时rviz与gazebo仿真机械臂位姿一致

手眼标定准备

复制easy_handeye/docs/example_launch/ur5_kinect_calibration.launch到easy_handeye/easy_handeye/launch中，并命名为ur3_kinect_calibration.launch，修改内容为

<launch>
    <arg name="namespace_prefix" default="ur3_kinect_handeyecalibration" />

    <arg name="robot_ip" doc="The IP address of the UR3 robot" />

    <arg name="marker_size" doc="Size of the ArUco marker used, in meters" default="0.05"/>
    <arg name="marker_id" doc="The ID of the ArUco marker used" default="26"/>

    <!-- start the Kinect -->
    <include file="$(find kinect2_bridge)/launch/kinect2_bridge.launch" >
    </include>

    <!-- start ArUco -->
    <node name="aruco_tracker" pkg="aruco_ros" type="single">
        <remap from="/camera_info" to="/kinect2/hd/camera_info" />
        <remap from="/image" to="/kinect2/hd/image_color_rect" />
        <param name="image_is_rectified" value="true"/>
        <param name="marker_size"        value="$(arg marker_size)"/>
        <param name="marker_id"          value="$(arg marker_id)"/>
        <param name="reference_frame"    value="kinect2_rgb_optical_frame"/>
        <param name="camera_frame"       value="kinect2_rgb_optical_frame"/>
        <param name="marker_frame"       value="camera_marker" />
    </node>

    <!-- start the robot -->
    <include file="$(find ur_robot_driver)/launch/ur3_bringup.launch">
        <!-- <arg name="limited" value="true" /> -->
        <arg name="robot_ip" value="192.168.101.222" />
    </include>
    <include file="$(find ur3_moveit_config)/launch/moveit_planning_execution.launch">
        <!-- <arg name="limited" value="true" /> -->
    </include>

    <!-- start easy_handeye -->
    <include file="$(find easy_handeye)/launch/calibrate.launch" >
        <arg name="namespace_prefix" value="$(arg namespace_prefix)" />
        <arg name="eye_on_hand" value="false" />

        <arg name="tracking_base_frame" value="kinect2_rgb_optical_frame" />
        <arg name="tracking_marker_frame" value="camera_marker" />
        <arg name="robot_base_frame" value="base_link" />
        <arg name="robot_effector_frame" value="wrist_3_link" />

        <arg name="freehand_robot_movement" value="false" />
        <arg name="robot_velocity_scaling" value="0.5" />
        <arg name="robot_acceleration_scaling" value="0.2" />
    </include>

</launch>

tracking_base_frame就是相机的frame， tracking_marker_frame是标志物的frame， 这两者之间的关系由aruco_ros发布， 所以要对上
robot_base_frame就是机械臂底座，robot_effector_frame是末端， 这两个跟UR3发布的要对上
这里我运行上面的标定launch文件

cd ~/ur_ws
source devel/setup.bash
roslaunch easy_handeye ur3_kinect_calibration.launch

此时打开三个窗口，一个是rviz窗口，其他两个小窗口是标定工具

发现rviz中的Global State是Error，说明easy_handeye自带的rviz配置文件是错误的。由于UR官方仓库中对应UR3的rviz配置文件是正确的，故将其复制过来代替easy_handeye自带的rviz配置文件。
复制universal_robot/ur3_moveit_config/launch/moveit.rviz文件到easy_handeye/easy_handeye/launch/下，修改该目录下的calibrate.launch文件
将文件中的rviz_config_file变量值从

<arg name="rviz_config_file" default="$(find easy_handeye)/launch/rviz_easy_handeye.config" doc="the path to the rviz config file to be opened" />

改为刚刚复制过来的moveit.rviz文件

<arg name="rviz_config_file" default="$(find easy_handeye)/launch/moveit.rviz" doc="the path to the rviz config file to be opened" />

重新运行命令，可以看到Global State正常了。下面根据下图中的步骤将TF关系添加进来

添加TF进来后可以看到

发现没有aruco marker的TF，这是因为没有通过相机订阅相应的节点，本来标定窗口rqt_easy_handeye_Hand-eye Calibration顶部应该出现选项目录，但是这里没有出现（参考报错汇总2），这里通过新开一个窗口，输入rqt，选择plugins->visualization->image view，话题选择/aruco_tracker/result，然后就能看到camera_marker了

手眼标定

下面正式开始手眼标定
为了安全流畅地操作，我们先在rviz的Motion Planning界面中将机械臂的速度和加速度都设为0.1。

看下图红框是否显示0/17，如果不是就按check starting pose，如果点击之后出现Cannot calibrate from current position，就换个姿势再check starting pose一次，直到显示0/17

下面就成功显示0/17，并准备记录下一个点了

点击take sample，采样完后在另一个标定界面点击next pose，再点击plan（ good plan 才执行，否则删除那个采样点重新采集），最后点击excute后机械臂会自动运动到新的位置，再点击take sample，这样反复进行17次。

采集17个点后，点击compute后计算出结果 ，再点击save

便可以在.ros隐藏文件下（在home目录下按ctrl+h便可以显示隐藏文件）看到easy_handeye的文件下的ur3_kinect_handeyecalibration_eye_on_base.yaml文件，文件内容如下：

parameters:
  eye_on_hand: false
  freehand_robot_movement: false
  move_group: manipulator
  move_group_namespace: /
  namespace: /ur3_kinect_handeyecalibration_eye_on_base/
  robot_base_frame: base_link
  robot_effector_frame: wrist_3_link
  tracking_base_frame: kinect2_rgb_optical_frame
  tracking_marker_frame: camera_marker
transformation:
  qw: 0.2260234455377024
  qx: -0.3533878684973005
  qy: -0.9077331251900548
  qz: 0.007140721174777452
  x: -0.2503005495999835
  y: -0.2698175019991061
  z: 1.1409909444449822

到这里，我们便完成了base_link到kinect2_rgb_optical_frame的变换矩阵
相关手眼标定原理参考链接

报错汇总

substitution args not supported: No module named ‘rospkg’

解决方法：退出conda虚拟环境重新执行

conda deactivate

启动easy_handeye后窗口顶部没有出现选项目录

解决办法：新开一个窗口，输入rqt，选择plugins->visualization->image view，话题选择/aruco_tracker/result

此时也能在rviz中看到maker的tf

Moveit!运动规划执行的时候报错

解决办法：在UR3机械臂控制面板中运行程序->加载程序->选一个urp文件加载->点击三角运行

The connection to the remote PC at IP:port could not be established.

解决办法：可能是有其他PC链接过机械臂编程运行过还没断开，关闭其他PC的连接（当时其实命令行窗口已经关了，但是没用，重启才解决）