ROS[aruco_ros+easy_handeye]手眼标定(眼在手外+UR10e+realsense-d435i)

参考链接：

https://zhuanlan.zhihu.com/p/576861119

https://blog.csdn.net/qq_32618327/article/details/120730198

本次在Docker中使用

1. 打印Aruco码：https://chev.me/arucogen/

   选择Dictionary为 Original ArUco(aruco_ros默认这个，如果选其他的可能需要修改)，其他的选完要记住，Aruco码打印下来测量一下以防被缩放导致实际尺寸不对。

2. 安装realsense的驱动和ros包：

   #安装
   sudo apt install ros-noetic-realsense2-camera
   #使用
   roslaunch realsense2_camera rs_camera.launch
   #点云
   roslaunch realsense2_camera rs_camera.launch filters:=pointcloud
   

   如果出现bug：failed to open usb interface: 0, error: RS2_USB_STATUS_ACCESS

   宿主机和容器都设置一下：

   检查目录/etc/udev/rules.d/下是否有99-realsense-libusb.rules
   如果有，需要修改内容。
   如果没有，则需要创建规则文件99-realsense-libusb.rules，内容如下：

   ##Version=1.1##
   # Device rules for Intel RealSense devices (R200, F200, SR300 LR200, ZR300, D400, L500, T200)
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0a80", MODE:="0666", GROUP:="plugdev", RUN+="/usr/local/bin/usb-R200-in_udev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0a66", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0aa5", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0abf", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0acb", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad0", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="04b4", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad1", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad2", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad3", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad4", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad5", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad6", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0af2", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0af6", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0afe", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0aff", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b00", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b01", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b03", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b07", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b0c", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b0d", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3d", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b48", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b49", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b4b", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b4d", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b52", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b5b", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b5c", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b64", MODE:="0666", GROUP:="plugdev"# Intel RealSense recovery devices (DFU)
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ab3", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0adb", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0adc", MODE:="0666", GROUP:="plugdev"
   SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b55", MODE:="0666", GROUP:="plugdev"# Intel RealSense devices (Movidius, T265)
   SUBSYSTEMS=="usb", ENV{DEVTYPE}=="usb_device", ATTRS{idVendor}=="8087", ATTRS{idProduct}=="0af3", MODE="0666", GROUP="plugdev"
   SUBSYSTEMS=="usb", ENV{DEVTYPE}=="usb_device", ATTRS{idVendor}=="8087", ATTRS{idProduct}=="0b37", MODE="0666", GROUP="plugdev"
   SUBSYSTEMS=="usb", ENV{DEVTYPE}=="usb_device", ATTRS{idVendor}=="03e7", ATTRS{idProduct}=="2150", MODE="0666", GROUP="plugdev"KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad5", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'"
   DRIVER=="hid_sensor_custom", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0ad5", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'"
   KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0af2", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'"
   DRIVER=="hid_sensor*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0af2", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'"
   KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0afe", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'"
   DRIVER=="hid_sensor_custom", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0afe", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'"
   KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0aff", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'"
   DRIVER=="hid_sensor_custom", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0aff", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'"
   KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b00", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'"
   DRIVER=="hid_sensor_custom", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b00", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'"
   KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b01", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'"
   DRIVER=="hid_sensor_custom", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b01", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'"
   KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'"
   DRIVER=="hid_sensor*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", RUN+="/bin/sh -c ' chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'"
   KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3d", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'"
   DRIVER=="hid_sensor*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3d", RUN+="/bin/sh -c ' chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'"
   KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b4b", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'"
   DRIVER=="hid_sensor*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b4b", RUN+="/bin/sh -c ' chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'"
   KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b4d", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'"
   DRIVER=="hid_sensor*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b4d", RUN+="/bin/sh -c ' chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'"
   KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b5b", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'"
   DRIVER=="hid_sensor*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b5b", RUN+="/bin/sh -c ' chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'"
   KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b5c", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'"
   DRIVER=="hid_sensor*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b5c", RUN+="/bin/sh -c ' chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'"
   KERNEL=="iio*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b64", MODE:="0777", GROUP:="plugdev", RUN+="/bin/sh -c 'chmod -R 0777 /sys/%p'"
   DRIVER=="hid_sensor*", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b64", RUN+="/bin/sh -c ' chmod -R 0777 /sys/%p && chmod 0777 /dev/%k'"

   设置完后执行sudo /etc/init.d/udev restart->USB拔掉->容器关闭->插入USB->启动容器。

3. 创建工作空间，安装aruco_ros和easy_handeye 。

   其中 aruco_ros 需要找到aruco_ros/aruco_ros/src/simple_single.cpp，注释以下框中的内容（我这里已经注释），原因见https://zhuanlan.zhihu.com/p/576861119。

   

   执行的命令顺序：

   #安装aruco_ros
   mkdir -p calibrate_ws/src && cd calibrate_ws/src
   #安装自己系统版本的，这里是noetic，具体版本去github上面查branch
   git clone https://github.com/pal-robotics/aruco_ros.git -b noetic-devel 按照上图手动注释simple_single.cpp中的内容#安装easy_handeye
   git clone https://github.com/IFL-CAMP/easy_handeye#准备编译
   cd ..  #pwd=/path/calibrate_ws
   rosdep install -iyr --from-paths src 
   #这个命令会从src目录中查找所有ROS包的package.xml文件，
   #然后自动安装所有这些文件中指定的依赖项，即使某些依赖项已经安装过，也会重新安装
   #安装完以后编译
   catkin_makeecho "source /path/calibrate_ws/devel/setup.bash" >> ~/.bashrc
   

4. 编写launch文件(我这里为ur10e_realsense_calibrate.launch)：主要修改的参数都在arg中， easy_handeye里对应的robot_base_frame和robot_effector_frame也要改一下。

   <?xml version="1.0" ?>
   <launch><!-- fill in the following parameters according to your tracking system --><arg name="namespace_prefix" default="ur10e_realsense_calibrate" /><arg name="eye_on_hand" default="false" /><!-- <arg name="robot_ip" default="10.8.1.1" /> --><arg name="markerId"        default="6"/>      <!-- set your marker id --><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=""/>  <!-- leave empty and the pose will be published wrt param parent_name --><arg name="corner_refinement" default="LINES" /> <!-- NONE, HARRIS, LINES, SUBPIX -->    <arg name="camera_frame" default="camera_link" /><arg name="camera_image_topic" default="/camera/color/image_raw" /><arg name="camera_info_topic" default="/camera/color/camera_info" /> <!-- start the robot (using https://github.com/UniversalRobots/Universal_Robots_ROS_Driver) --><!-- <include file="$(find ur_robot_driver)/launch/ur10e_bringup.launch"><arg name="robot_ip" value="$(arg robot_ip)" /><arg name="kinematics_config" value="$(find ur_description)/config/ur10e/default_kinematics.yaml" /> --><!-- </include><include file="$(find ur10e_moveit_config)/launch/ur10e_moveit_planning_execution.launch" /><include file="$(find ur10e_moveit_config)/launch/moveit_rviz.launch" ><arg name="rviz_config" value="$(find ur10e_moveit_config)/launch/moveit.rviz" /></include> --><!-- (start your tracking system's ROS driver) --><include file="$(find realsense2_camera)/launch/rs_camera.launch"> </include>     <node pkg="aruco_ros" type="single" name="aruco_single"><remap to="$(arg camera_info_topic)" from="/camera_info" /><remap to="$(arg camera_image_topic)" from="/image" /><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="$(arg camera_frame)"/><param name="marker_frame"       value="$(arg marker_frame)" /><param name="corner_refinement"  value="$(arg corner_refinement)" /></node><!-- 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="$(arg eye_on_hand)" /><arg name="tracking_base_frame" value="$(arg camera_frame)" /><arg name="tracking_marker_frame" value="$(arg marker_frame)" /><arg name="robot_base_frame" default="ra_base_link" /><arg name="robot_effector_frame" default="ra_tool0_controller" /><arg name="freehand_robot_movement" value="true" /><arg name="robot_velocity_scaling" value="0.5" /><arg name="robot_acceleration_scaling" value="0.2" /><arg name="translation_delta_meters" default="0.05"  /><arg name="rotation_delta_degrees" default="25"  /><arg name="rviz_config_file" value="$(find easy_handeye)/launch/self_arm.rviz" /></include><node name="rviz_for_aruco_result" pkg="rviz" type="rviz" respawn="true" output="screen" /></launch>
   

5. 这里没有设置机械臂启动的脚本，所以启动前自己先正常启动机械臂，再启动这个脚本就可以了。

6. 启动后一共三个窗口(两个rviz和一个rqt)，一个rviz控制机械臂移动，一个rviz观察aruco码的识别结果，一个rqt用来采样和保存结果。观察到aruco码的识别结果准确时点击Take Sample，采集结果足够多(15次以上)后点击Compute会出现结果，最后点击Save，保存后需要去终端查看保存的位置，是个yaml文件。

   

7. 根据标定的结果查看结果：

   rosrun tf2_ros static_transform_publisher 0.012 -1.2 0.396 -0.6437 0.023 0.0344 0.7641 /ra_base_link_test /camera_link_test
   # 0.012 -1.2 0.396 -0.6437 0.023 0.0344 0.7641 是我标定的结果
   # 用法：
   static_transform_publisher x y z yaw pitch roll frame_id child_frame_id
   static_transform_publisher x y z qx qy qz qw frame_id child_frame_id
   

8. 打开rviz，选择Fixed Frame为ra_base_link_test，添加TF插件，添加刚刚指定的两个坐标系：ra_base_link_test和camera_link_test，可以看到结果。