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

參考文章了鏈接：

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

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

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_frame" default="aruco_marker_frame"/>-->
    <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" />

    <!-- start the realsense -->
    <!-- <include file="$(find kinect2_bridge)/launch/kinect2_bridge.launch" /> -->
   <!--  <group ns="namespace1"> -->
    <!-- <include file="$(find realsense_camera)/launch/zr300_nodelet_rgbd.launch" /> -->
        <!-- <arg name="depth_registration" value="true" /> -->
    <!-- </group> -->
    <!-- </include> -->
 
    <!-- start ArUco -->
    <node name="aruco_tracker" pkg="aruco_ros" type="single">
        <!-- <remap from="/camera_info" to="/kinect2/hd/camera_info" /> -->
        <remap from="/camera_info" to="/camera/rgb/camera_info" />
        <!-- <remap from="/image" to="/kinect2/hd/image_color_rect" /> -->
        <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="kinect2_rgb_optical_frame"/>
        <param name="camera_frame"       value="kinect2_rgb_optical_frame"/> -->
        <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>

<!-- start the robot -->
  <!--   <include file="$(find ur_modern_driver)/launch/ur10_bringup.launch">
        <arg name="limited" value="true" />
        <arg name="robot_ip" value="192.168.2.24" />
    </include> -->

    <!-- <include file="$(find ur10_rg2_moveit_config)/launch/planning_context.launch">
        <arg name="load_robot_description" value="false"/>
        <arg name="load_robot_description" value="true"/> -->
        <!-- <include file="$(find ur5_moveit_config)/launch/ur5_moveit_planning_execution.launch"> -->
   <!--  </include>  -->

   <!--  <include file="$(find ur10_rg2_moveit_config)/launch/ur10_moveit_planning_execution.launch">
        <arg name="limited" value="true" />
    </include> -->

<!-- start the robot222 -->
<!--     <group ns="namespace2">
    <include file="$(find ur_control)/launch/ur10_control.launch">
        <arg name="limited" value="true" />
        <arg name="robot_ip" value="192.168.2.24" />
    </include>
    </group>
    <group ns="namespace3">
    <include file="$(find ur10_rg2_moveit_config)/launch/demo_norviz.launch"> -->
    <!-- <include file="$(find ur10_rg2_moveit_config)/launch/demo.launch"> -->
        <!-- <arg name="limited" value="true" />
    </include>
    </group>
 -->
<!-- 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_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="wrist_3_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_frame" default="aruco_marker_frame"/>-->
    <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" />

    <!-- start the realsense -->
    <!-- <include file="$(find kinect2_bridge)/launch/kinect2_bridge.launch" /> -->
   <!--  <group ns="namespace1"> -->
    <!-- <include file="$(find realsense_camera)/launch/zr300_nodelet_rgbd.launch" /> -->
        <!-- <arg name="depth_registration" value="true" /> -->
    <!-- </group> -->
    <!-- </include> -->
 
    <!-- start ArUco -->
    <node name="aruco_tracker" pkg="aruco_ros" type="single">
        <!-- <remap from="/camera_info" to="/kinect2/hd/camera_info" /> -->
        <remap from="/camera_info" to="/camera/rgb/camera_info" />
        <!-- <remap from="/image" to="/kinect2/hd/image_color_rect" /> -->
        <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="kinect2_rgb_optical_frame"/>
        <param name="camera_frame"       value="kinect2_rgb_optical_frame"/> -->

        <!-- <param name="reference_frame"    value="camera_rgb_frame"/>
        <param name="camera_frame"       value="camera_rgb_frame"/> -->

        <param name="reference_frame"    value="camera_link"/>
        <param name="camera_frame"       value="camera_rgb_optical_frame"/>

        <param name="marker_frame"       value="camera_marker" />
    </node>

<!-- start the robot -->
  <!--   <include file="$(find ur_modern_driver)/launch/ur10_bringup.launch">
        <arg name="limited" value="true" />
        <arg name="robot_ip" value="192.168.2.24" />
    </include> -->

    <!-- <include file="$(find ur10_rg2_moveit_config)/launch/planning_context.launch">
        <arg name="load_robot_description" value="false"/>
        <arg name="load_robot_description" value="true"/> -->
        <!-- <include file="$(find ur5_moveit_config)/launch/ur5_moveit_planning_execution.launch"> -->
   <!--  </include>  -->

   <!--  <include file="$(find ur10_rg2_moveit_config)/launch/ur10_moveit_planning_execution.launch">
        <arg name="limited" value="true" />
    </include> -->

<!-- start the robot222 -->
<!--     <group ns="namespace2">
    <include file="$(find ur_control)/launch/ur10_control.launch">
        <arg name="limited" value="true" />
        <arg name="robot_ip" value="192.168.2.24" />
    </include>
    </group>
    <group ns="namespace3">
    <include file="$(find ur10_rg2_moveit_config)/launch/demo_norviz.launch"> -->
    <!-- <include file="$(find ur10_rg2_moveit_config)/launch/demo.launch"> -->
        <!-- <arg name="limited" value="true" />
    </include>
    </group>
 -->
<!-- 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_base_frame" value="camera_rgb_frame" /> -->
        <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="wrist_3_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

在下面的這篇博文也給出了方法，關於tool0_controller報錯的問題：

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啓動的節點中包含了<!-- Dummy calibration to have a fully connected TF tree and see all frames -->這個下面的定義中便有相關的固定座標系位置標註，所以要去掉就不會顯示了。（下圖就是錯誤的座標系，真實的只能閃爍出現）。

 

修改後：

---

初步的標定工作完成，接着進行程序的整合，和之前的深度相機以及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下的不用座標系轉換到同一個。接下來就是繼續這個工作。

當然這都是工程實踐，開始看一些深度學習模型，最終發論文創新點也是基於這個平臺，在圖像識別這塊進行創新。任重道遠，繼續努力(#^.^#)~

--------------------------------------------------更新2019.7.19--------------------------------------------------------------

因爲最近半年都在準備發論文以及準備雅思，所以都沒有登csdn回覆。希望8.24雅思順利，繼續努力把。之後準備好雅思及申請好後，會總結一下今年前兩個半月paper做的3D 物體識別、6D位姿估計以及重疊物體的檢測等內容。

另外，有很多人通過郵箱聯繫到了我我也給出了我的解答。如有問題請發郵件到： [email protected] 最近依然不會回覆csdn。

如果有人感興趣想加入的話，自動化所招實習生啦！！！

【自動化所類腦中心-類腦機器人實習生招聘】
類腦認知機器人算法及應用實習生：主要負責基於ROS的認知機器人開發，要求自動控制及相關專業（或個人十分感興趣），具有機器人ROS開發經驗/python編程基礎者優先，最好能實習6個月以上。我們將提供Baxter機器人、Robotnik機器底盤、NAO機器人等作爲硬件支撐實習生的研究。
有意者請聯繫[email protected]

--------------------------------------------------------7.19--------------------------------------------------------------------------------------

-------------------------------------------------------------------------------2020.3.2更新-------------------------------------------------------------

最近在準備面試，有問題可以聯繫上面的郵箱。最終的文章代碼鏈接以及原文、實現步驟已經在系列終結篇給出。

另外本人畫的場景設計畫在站酷上，歡迎交流www.zcool.com.cn/u/20178144