1 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 ar300_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節點

 rosluanch 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不同。）

重新進行標定，總的執行步驟執行相關檔案為：

#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下的不用座標系轉換到同一個。接下來就是繼續這個工作。

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