UR10 RG2机械臂手臂+RealsenseZR300 机器人手眼标定系列第四篇

参考文章了链接：

https://blog.csdn.net/zhang970187013/article/details/81098175
<https://blog.csdn.net/zhang970187013/article/details/81098175>

https://github.com/IFL-CAMP/easy_handeye/tree/master
<https://github.com/IFL-CAMP/easy_handeye/tree/master>

https://blog.csdn.net/sinat_23853639/article/details/80276848
<https://blog.csdn.net/sinat_23853639/article/details/80276848>

跑了一个星期参考文章，一点点推进但是一直有问题，各种乱七八糟节点冲突没有发布话题
，坐标系也老是跳，页面出现闪退的情况，干脆把文章写的launch前面的几步单独拎出来跑，没想到问题竟然就这样解决了...

IntelRealsense ZR300相机外参标定

首先，标定的原理是：基坐标系（base_tree）和相机（camera_tree）两个坐标系属于不同的tree，通过将标签贴到手上，相机识别出标签的position
和
orention,并通过hand_eye标定包得到marker_frame(机械手)，进一步得到相对于base的位置关系。即子坐标系（camera_rgb_optical_frame）到父坐标系（base_link）之间的关系。在之后如果摄像头识别到物体的位置（在camera坐标系下），即可通过transform(这种转换关系)，转化为base(也就是机器人知道的自己的位置坐标系)坐标系下的位置，这样机器人就通过转化关系得到相机识别到的位置实际在空间中的位置。

总体步骤为：

1.清除无关节点
#rosnode cleanup
2.启动相机驱动程序
#roslaunch realsense_camera zr300_nodelet_rgbd.launch
3.连接机器人
#roslaunch ur_control ur10_control.launch robot_ip:=192.168.2.24
4.启动moveit和rviz
#roslaunch ur10_rg2_moveit_config demo.launch
5.启动标定程序 launch节点
#roslaunch ur10_realsense_handeyecalibration.launch
launch文件经过修改后：（单独终端跑就解决了rviz老是闪退、坐标系跳动、采集错误等问题）

其中：start robot/robot222 以及前面的start realsense单独拎出来在节点里面跑，这样就解决了问题。
<launch> <arg name="namespace_prefix"
default="ur10_realsense_handeyecalibration" /> <arg name="robot_ip" doc="The IP
address of the UR10 robot" />  <arg name="marker_size" doc="Size of the
ArUco marker used, in meters" default="0.1" /> <arg name="marker_id" doc="The
ID of the ArUco marker used" default="100" />         <node name="aruco_tracker" pkg="aruco_ros"
type="single">  <remap from="/camera_info" to="/camera/rgb/camera_info" />  <remap from="/image"
to="/camera/rgb/image_rect_color" /> <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="camera_rgb_optical_frame"/> <param name="camera_frame"
value="camera_rgb_optical_frame"/> <param name="marker_frame"
value="camera_marker" /> </node>  
         <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="camera_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="rg2_eef_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>
另外要修改标签内容，在ur10_realsense_handeyecalibration.launch文件中，有

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

这里的default值分别为
具体打印出来的标签（在~/catkin_ws/src/aruco_ros/aruco_ros/etc文件夹下面就有对应的各种可以识别出来的标签，用ps改成pdf并缩小打印就可以识别，当然aruco官网也提供了下载
对应的新标签要改配置文件才能使用）的大小（注意精度是m），第二个default值是标签号，可以跑了程序以后在让rqt_image_view里面查看识别到的标签，我的标签ID=582。

最终便可以实现正确的camera和base的标定。

所以修改后的ur10_realsense_handeyecalibration.launch标定程序文件为：
<launch> <arg name="namespace_prefix"
default="ur10_realsense_handeyecalibration" /> <arg name="robot_ip" doc="The IP
address of the UR10 robot" />  <arg name="marker_size" doc="Size of the
ArUco marker used, in meters" default="0.0765" /> <arg name="marker_id"
doc="The ID of the ArUco marker used" default="582" />         <node name="aruco_tracker" pkg="aruco_ros"
type="single">  <remap from="/camera_info" to="/camera/rgb/camera_info" />  <remap from="/image"
to="/camera/rgb/image_rect_color" /> <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="camera_link"/> <param name="camera_frame"
value="camera_rgb_optical_frame"/> <param name="marker_frame"
value="camera_marker" /> </node>  
         <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="camera_link" /> <arg name="tracking_marker_frame" value="camera_marker"
/> <arg name="robot_base_frame" value="base_link" />  <arg
name="robot_effector_frame" value="rg2_eef_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>
6.启动rqt
#rqt_image_view
7.打印标签准备手眼标定

出现各种问题：value值不能成功转换，然后就是-nan -nan -nan -nan。
[ERROR] [1515404258.018207679]: Ignoring transform for child_frame_id
"tool0_controller" from authority "unknown_publisher" because of a nan value in
the transform (0.000000
26815622274503241629349378993175553962225540122973894056168551426931042827562994218784229639762337459109057520690886188494020583679371347120178163255083008.000000
26815622274206272770731577112262473438093890782345820683584369763133472687942565341004009969841415624372630858128332080466218773763999513725165301565227008.000000)
(-nan -nan -nan -nan)
参考博文进行修改：

https://github.com/ThomasTimm/ur_modern_driver/issues/128
<https://github.com/ThomasTimm/ur_modern_driver/issues/128>

在下面的这篇博文也给出了方法，关于tool0_controller报错的问题：

https://blog.csdn.net/coldplayplay/article/details/79106134
<https://blog.csdn.net/coldplayplay/article/details/79106134>

但是我改了程序以后发现并没有解决问题，而且在跑原来的程序时候（参考自己UR10
RG2那篇博文），也出现了这个问题。在哭求无果之际，改变思路。把ur10_rg2_ros(之前关于所有UR10 RG2 以及抓取的定义
控制程序)全部删掉，重新导入之前备份的原件进行编译，没有问题。然后重新按步骤修改（走完全部流程）这个问题就解决了。。。

（注意：修改.cpp文件保存以后一定要重新cmake，不然并不会生成可以用的文件。这点和py文件在ros不同。）

最新的解决办法是：在修改了communication.cpp和realtime_communication.cpp的文件后，如果仍然有上述错误可能是由于网络的原因导致ur10断开与机器的连接，这样的后果不仅会导致上述的bug
还可能导致机械臂抖动、错误急停、连接过程经常reconnect。解决措施是修改两文件，然后用有线的方式或者在较好的无线的情况下进行测试，避免因网络问题在roslaunch时候造成编译错误。

重新进行标定，总的执行步骤运行相关文件为：
#roslaunch realsense_camera zr300_nodelet_rgbd.launch #roslaunch ur_control
ur10_control.launch robot_ip:=192.168.2.24 #roslaunch easy_handeye
static_transform_publisher.launch #roslaunch ur10_rg2_moveit_config demo.launch
#roslaunch easy_handeye ur10_realsense_handeyecalibration.launch
注意要打开rqt #rqt_image_view，不然采集会失败

采集步骤为：1（如果不是17（我的是8）的话）--2--3--4--5--2345（重复直至全部采集完）--6--7（保存结果）

接下来就是将标定的结果实时显示出来：

自己改了一个实时（100hz）发布位置转换关系的launch文件，放在~/catkin_ws/src/easy_handeye/easy_handeye/launch(和之前的ur10_realsense_handeyecalibration.launch在一个包里)，这里的args后面就是采集到的转换关系position(x
y z)和orention(x y z
w)的值，注意后面的是子坐标系（camera_rgb_optical_frame）指向父坐标系(base_link),频率通常设为100Hz。
<launch> <node pkg="tf" type="static_transform_publisher"
name="link1_broadcaster" args="0.244332058703 -0.0155924006203 -0.0434545849503
0.0144077350003 -0.0295559697343 -0.0183787903043 0.999290289101 base_link
camera_link 100" /> </launch>

在rviz里面出现标定结果，但是有之前的一直显示，而且正确的标定结果坐标系只是闪烁出现。这是因为在start
easy_handeye（之前写的ur10_realsense_handeyecalibration.launch包里有调用这个包实现标定功能）时默认发布一个固定的原始坐标系位置，而这个规定的默认一直显示的坐标系在calibration.launch文件里面。在calibration启动的节点中包含了这个下面的定义中便有相关的固定坐标系位置标注，所以要去掉就不会显示了。（下图就是错误的坐标系，真实的只能闪烁出现）。

修改后：

初步的标定工作完成，接着进行程序的整合，和之前的深度相机以及linemod同时跑（当然实时显示电脑很吃力）。
操作步骤相机+识别+listener #roslaunch realsense_camera zr300_nodelet_rgbd.launch
#rosrun object_recognition_core detection -c `rospack find
object_recognition_linemod`/conf/detection.ros.ork #rosrun
ur10_rg2_moveit_config listener.py (#rosrun rqt_reconfigure rqt_reconfigure)
坐标系变换+启动机器人 #roslaunch ur_control ur10_control.launch robot_ip:=192.168.2.24
#roslaunch easy_handeye static_transform_publisher.launch #roslaunch
ur10_rg2_moveit_config demo.launch #roslaunch easy_handeye
ur10_realsense_handeyecalibration.launch 机器人控制 #rosrun ur10_rg2_moveit_config
moveit_controller.py //加限制 #rosrun ur10_rg2_moveit_config
new_moveit_controller.py //控制程序
这里的lisntener.py是自己改的一个挖出camera_frame坐标系下物体的位置，方便以后的调用：
#!/usr/bin/env python #coding=utf-8 import rospy # from std_msgs.msg import
String from object_recognition_msgs.msg import RecognizedObjectArray def
callback(data): if len(data.objects)>0: print "coke
detected~~~~~~~~~~~~~~~~~~~~" rospy.loginfo(rospy.get_caller_id() + 'heard %s',
data.objects[0].pose.pose.pose) else: print "nothing detected................."
def listener(): # In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The # anonymous=True flag
means that rospy will choose a unique # name for our 'listener' node so that
multiple listeners can # run simultaneously. rospy.init_node('listener',
anonymous=True) rospy.Subscriber('recognized_object_array',
RecognizedObjectArray, callback) # spin() simply keeps python from exiting
until this node is stopped rospy.spin() if __name__ == '__main__': listener()
显示结果：

这样，总体的程序跑下来，在rviz里面观察结果：

当然已经很卡了，而且目前存在的问题是camera_rgb_frame和camera_depth_frame之间没有transform,虽然并不影响，因为这是相机内部的坐标系，而IntelRealsense
ZR300好处是不需要进行相机的内参的标定。

展望：

在标定这块浪费了大量的时间，不过还是顺利的完成了外参标定工作。接下来就是利用TF，广播监听TF,利用lookupTransform（）、transformPoint（）等函数，将相机采集到的可乐罐所在的位置信息（在camera_rgb_frame下）转换到基坐标系下（base_link）,这样就可以指示机械手达到相对应的位置。

之前的外参标定只是将base_link和camera_frame两个tree连接起来了（static_transform
100Hz发布），也就是说tf可以用到两棵tree的子坐标系，而tf才可以存储变换关系，并将对应的点在不同tree下的不用坐标系转换到同一个。接下来就是继续这个工作。

当然这都是工程实践，开始看一些深度学习模型，最终发论文创新点也是基于这个平台，在图像识别这块进行创新。任重道远，继续努力(#^.^#)~

热门工具换一换

UR10 RG2机械臂手臂+RealsenseZR300 机器人手眼标定 系列第四篇