一.廣播tf
其實就是發佈你建立的座標系。步驟如下:
1.定義一個廣播,相當於發佈話題時定義一個發佈器,還是以官方的小烏龜例程爲例:
http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28C++%29
static tf::TransformBroadcaster br;
2.定義存放轉換信息(平動,轉動)的變量
tf::Transform transform;
3.設置座標原點
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
需要注意:
(1)setOrigin()函數的參數類型需要爲tf::Vector3類型
(2)假設是要發佈一個子座標系爲”turtle1”父座標系爲“world”,那麼其中(msg->x,msg->y,0.0)是指“turtle1”的座標原點在“world”座標系下的座標。
4.定義旋轉
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
需要注意:
(1)setRPY()函數的參數爲”turtle1”在“world”座標系下的roll(繞X軸),pitch(繞Y軸),yaw(繞Z軸)
(2)爲了確保轉換正確強烈建議,在轉換完後,運行下程序,打開rviz下使用確認下轉換是否正確,我在實驗中有一次,明明依次填入了三個角度,但是在rviz下發現並不正確,pitch,yaw還存在一個負號的關係(樓主對旋轉角的正負號向來頭暈,所以都是直接打開rviz,直接試,直觀而且準確)。
在設置變換時,也可以直接使用transform的構造函數
tf::Transform::Transform ( const Quaternion & q, const Vector3 & c = Vector3(tfScalar(0), tfScalar(0), tfScalar(0)) )
5.將變換廣播出去
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
其中,
(1)transform:存儲變換關係的變量;
(2)ros::Time::now():廣播tf使的時間戳;
(3)“world”:父座標系的名字;
(4)turtle_name:子座標系的名字,這裏因爲子座標系有兩個,所以它定義了字符串用於存放名字,方便切換,通常我們把名字填到引號裏就可以了。
總結一下,假設你在機器人上應用,如果你知道機器人的位置x,y,z,與三個旋轉角roll,pitch,yaw就可以廣播一個tf了,如果在是平面移動機器人,則只需要知道x,y與yaw即可。
二.監聽tf
通過監聽tf,我們可以避免繁瑣的旋轉矩陣的計算,而直接獲取我們需要的相關信息。
在監聽中我最常用兩個函數:
(1)lookupTransform();
(2)transformPoint();
(一)lookupTransform();函數的使用
1.函數功能:可以過得兩個座標系之間轉換的關係,包括旋轉與平移。
2.使用例程:http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28C++%29
3.主要步驟:
(1)定義監聽器;
tf::TransformListener listener
(2)定義存放變換關係的變量
tf::StampedTransform transform;
(3)監聽兩個座標系之間的變換
try{
listener.lookupTransform("/turtle2", "/turtle1",
ros::Time(0), transform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
注意:
A.由於tf的會把監聽的內容存放到一個緩存中,然後再讀取相關的內容,而這個過程可能會有幾毫秒的延遲,也就是,tf的監聽器並不能監聽到“現在”的變換,所以如果不使用try,catch函數會導致報錯:
“world” passed to lookupTransform argument target_frame does not exist. ”
並且會導致程序掛掉,使用try,catch之後就OK了。
B.lookupTransform()函數參數說明
a.官網的例程說的是實現從“/turtle2”到“/turtle1”的轉換,但實際中使用時,我發現轉換得出的座標是在“/turtle2”座標系下的
b.不可以把ros::Time(0)改成ros::time::now(),因爲監聽做不到實時,會有幾毫秒的延遲。ros::Time(0)指最近時刻存儲的數據,ros::time::now()則指當下,如果非要使用ros::time::now,則需要結合waitForTransform()使用,具體見:
http://wiki.ros.org/tf/Tutorials/tf%20and%20Time%20%28C++%29
實際中ros::Time(0)大多數情況下可以滿足要求。
c.最後轉換關係存入transform中
(4)使用變換
通常我們使用的是點的信息
transform.getOrigin().x()
transform.getOrigin().y(),
可以得到在turtle1座標系的原點,在turtle2座標系下的位置。
transform.getRotation().getW();
transform.getRotation().getX();
transform.getRotation().getY();
transform.getRotation().getZ();
可以得到旋轉的四元數,如何直接得到偏航角因爲沒有用到這個功能所以也就沒有去找相關的函數。
(二)transformPoint()函數使用
在實際應用中我們肯定會需要把在一個座標系下的點轉換到另一個座標系下,這就需要transformPoint()函數。
1.函數功能:將一個座標系下的點的座標轉換到另一個座標系下。
2.函數參數:如
listener_.transformPoint("PTAM_world",m_normal_pose,pose_PTAM_world);
(1)其中m_normal_pose數據類型爲 geometry_msgs::PointStamped,
其中需要定義m_normal_pose.header.frame_id即該點所屬的座標系
(2)而“PTAM_world”則指,我要將m_normal_pose轉換到“PTAM_world”座標系下。
pose_PTAM_world是轉換的結果,數據類型同樣爲geometry_msgs::PointStamped。
3.使用例程:
geometry_msgs::PointStamped turtle1;
turtle1.header.stamp=ros::Time();
turtle1.header.frame_id="turtle1";
turtle1.point.x=1;
turtle1.point.y=2;
turtle1.point.z=3;
geometry_msgs::PointStamped turtle1_world;
try{
listener_.transformPoint("PTAM_world",turtle1,turtle1_world);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
說明:同樣因爲延時,turtle1.header.stamp不能爲ros::Time::now();否則會出類似錯誤
[ERROR] [1456669076.279804500]: Lookup would require extrapolation into the future. Requested time 1456669076.279616253 but the latest data is at time 1456669076.159341977, when looking up transform from frame …