ROS之tf座標變換
1、什麼是tf變換
ROS中的很多軟體包都需要機器人釋出tf變換樹，那麼什麼是tf變換樹呢？抽象的來講，一棵tf變換樹定義了不同座標系之間的平移與旋轉變換關係。具體來說，我們假設有一個機器人，包括一個機器人移動平臺和一個安裝在平臺之上的鐳射雷達，以這個機器人為例，定義兩個座標系，一個座標系以機器人移動平臺的中心為原點，稱為base_link參考系，另一個座標系以鐳射雷達的中心為原點，稱為base_laser參考系。
假設在機器人執行過程中，鐳射雷達可以採集到距離前方障礙物的資料，這些資料當然是以鐳射雷達為原點的測量值，換句話說，也就是base_laser參考系下的測量值。現在，如果我們想使用這些資料幫助機器人完成避障功能，當然，由於鐳射雷達在機器人之上，直接使用這些資料不會產生太大的問題，但是鐳射雷達並不在機器人的中心之上，在極度要求較高的系統中，會始終存在一個雷達與機器人中心的偏差值。這個時候，如果我們採用一種座標變換，將及鐳射資料從
為了定義這個變換關係，假設我們已知鐳射雷達安裝的位置在機器人的中心點上方20cm，前方10cm處。這就根據這些資料，就足以定義這兩個參考系之間的變換關係：當我們獲取鐳射資料後，採用(x: 0.1m, y: 0.0m, z: 0.2m)的座標變換，就可以將資料從base_laser參考系變換到base_link參考系了。當然，如果要方向變化，採用(x: -0.1m, y: 0.0m, z: -0.20m)的變換即可。
從上邊的示例看來，參考系之間的座標變換好像並不複雜，但是在複雜的系統中，存在的參考系可能遠遠大於兩個，如果我們都使用這種手動的方式進行變換，估計很快你就會被繁雜的座標關係搞蒙了。
還是以上邊的示例為基礎，為了定義和儲存base_link和base_laser兩個參考系之間的關係，我們需要將他們新增到tf樹中。從樹的概念上來講，tf樹中的每個節點都對應一個參考系，而節點之間的邊對應於參考系之間的變換關係。tf就是使用這樣的樹結構，保證每兩個參考系之間只有一種遍歷方式，而且所有變換關係，都是母節點到子節點的變換。
為了定義上邊示例中的參考系，我們需要定義兩個節點，一個對應於
2、程式碼流程
通過上邊的分析，應該可以從理論上幫助你理解什麼是tf，以及tf的功能了。在實際應用中，我們需要使用程式碼來完成這些理論，下邊我們就來看看如何使用程式碼來呼叫tf的變換。
程式碼的總體思路分為兩個部分：
（1）編寫一個節點，廣播兩個參考系之間的tf變換關係
（2）編寫另外一個節點，訂閱tf樹，然後從tf樹中遍歷到兩個參考系之間的變換公式，然後通過公式計算資料的變換。
我們先來完成第一步。首先我們來建立一個功能包，用於後邊程式的放置，這裡需要依賴roscpp、tf、geometry_msgs。
-
$ cd %TOP_DIR_YOUR_CATKIN_WS%/src
-
$ catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs
3、編寫廣播節點
進入功能包，建立src/tf_broadcaster.cpp檔案，來完成廣播節點的程式碼：
-
#include <ros/ros.h> -
#include <tf/transform_broadcaster.h> -
int main(int argc, char** argv){
-
ros::init(argc, argv, "robot_tf_publisher");
-
ros::NodeHandle n;
-
ros::Rate r(100);
-
tf::TransformBroadcaster broadcaster;
-
while(n.ok()){
-
broadcaster.sendTransform(
-
tf::StampedTransform(
-
tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
-
ros::Time::now(),"base_link", "base_laser"));
-
r.sleep();
-
} -
}
逐行分析如上程式碼：
-
#include <tf/transform_broadcaster.h>
因為後邊會使用到tf::TransformBroadcaster類的例項，來完成tf樹的廣播，所以需要先包含相關的標頭檔案。
-
tf::TransformBroadcaster broadcaster;
建立一個tf::TransformBroadcaster類的例項，用來廣播base_link → base_laser的變換關係。
-
broadcaster.sendTransform(
-
tf::StampedTransform(
-
tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
-
ros::Time::now(),"base_link", "base_laser"));
這部分是程式碼的關鍵所在。通過TransformBroadcaster類來發布變換關係的介面，需要五個引數。首先是兩個參考系之間的旋轉變換，通過btQuaternion四元數來儲存旋轉變換的引數，因為我們用到的兩個參考系沒有發生旋轉變換，所以傾斜角、滾動角、偏航角都是0。第二個引數是座標的位移變換，我們用到的兩個參考系在X軸和Z軸發生了位置，根據位移值填入到btVector3向量中。第三個引數是時間戳，直接太難過ROS的API完成。第四個引數是母節點儲存的參考系，即base_link，最後一個引數是子節點儲存的參考系，即base_laser。
4、編寫訂閱節點
上一節講解了如何使用程式碼編寫一個廣播tf變換的節點，這一節，我們編寫一個訂閱tf廣播的節點，並且使用tf樹中base_link到base_laser的變換關係，完成資料的座標變換。在robot_setup_tf功能包中建立src/tf_listener.cpp檔案，程式碼如下：
-
#include <ros/ros.h> -
#include <geometry_msgs/PointStamped.h> -
#include <tf/transform_listener.h> -
void transformPoint(const tf::TransformListener& listener){
-
//we'll create a point in the base_laser frame that we'd like to transform to the base_link frame -
geometry_msgs::PointStamped laser_point;
-
laser_point.header.frame_id = "base_laser";
-
//we'll just use the most recent transform available for our simple example -
laser_point.header.stamp = ros::Time();
-
//just an arbitrary point in space -
laser_point.point.x = 1.0;
-
laser_point.point.y = 0.2;
-
laser_point.point.z = 0.0;
-
try{
-
geometry_msgs::PointStamped base_point;
-
listener.transformPoint("base_link", laser_point, base_point);
-
ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
-
laser_point.point.x, laser_point.point.y, laser_point.point.z,
-
base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
-
} -
catch(tf::TransformException& ex){
-
ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
-
} -
} -
int main(int argc, char** argv){
-
ros::init(argc, argv, "robot_tf_listener");
-
ros::NodeHandle n;
-
tf::TransformListener listener(ros::Duration(10));
-
//we'll transform a point once every second -
ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));
-
ros::spin();
-
}
進行詳細的分析：
-
#include <tf/transform_listener.h>
在後邊的程式碼中，我們會使用到tf::TransformListener物件，該物件會自動訂閱ROS中的tf訊息，並且管理所有的變換關係資料。所以需要先包含相關的標頭檔案。
-
void transformPoint(const tf::TransformListener& listener){
我們建立一個回撥函式，每次收到tf訊息時，都會自動呼叫該函式，上一節我們設定了釋出tf訊息的頻率是1Hz，所以回撥函式執行的頻率也是1Hz。在回撥函式中，我們需要完成資料從base_laser到base_link參考系的座標變換。
-
//we'll create a point in the base_laser frame that we'd like to transform to the base_link frame -
geometry_msgs::PointStamped laser_point;
-
laser_point.header.frame_id = "base_laser";
-
//we'll just use the most recent transform available for our simple example -
laser_point.header.stamp = ros::Time();
-
//just an arbitrary point in space -
laser_point.point.x = 1.0;
-
laser_point.point.y = 0.2;
-
laser_point.point.z = 0.0;
我們建立了一個geometry_msgs::PointStamped型別的虛擬點，該點的座標為（1.0，0.2，0.0）。該型別包含標準的header訊息結構，這樣，我們可以就可以在訊息中加入釋出資料的時間戳和參考系的id。
-
try{
-
geometry_msgs::PointStamped base_point;
-
listener.transformPoint("base_link", laser_point, base_point);
-
ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
-
laser_point.point.x, laser_point.point.y, laser_point.point.z,
-
base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
-
}
這裡是程式碼的關鍵位置。我們已經在base_laser參考系下虛擬了一個數據點，那麼怎樣將該點的資料轉換到base_base參考系下呢？使用TransformListener物件中的transformPoint（）函式即可，該函式包含三個引數：第一個引數是需要轉換到的參考系id，當然是base_link了；第二個引數是需要轉換的原始資料；第三個引數用來儲存轉換完成的資料。該函式執行完畢後，base_point就是我們轉換完成的點座標了！
-
catch(tf::TransformException& ex){
-
ROS_ERROR("Received an exception trying to transform a point from \"base_laser\"