ROS 機(jī)器人技術(shù) - 添加一個 TF 幀
登龍
這是我的第 140 篇原創(chuàng)
在實(shí)際的機(jī)器人中往往有很多個傳感器，比如我們組目前用的小車上就有相機(jī)，雷達(dá)，IMU 等，為了能夠在 TF 系統(tǒng)中找到傳感器之間的轉(zhuǎn)換，就需要把每個傳感器的坐標(biāo)系加到系統(tǒng)的 TF 樹中，方法很簡單，下面一起來學(xué)習(xí)下。
一、TF 樹的注意事項(xiàng)
在實(shí)際使用和調(diào)試 TF 的時候一定要時刻記?。?strong style="color:#000000;">TF 樹中的一個節(jié)點(diǎn)可以有多個子節(jié)點(diǎn)，但是只能有一個父節(jié)點(diǎn)，并且 TF 樹中不能出現(xiàn)回環(huán)！
一個典型的 TF 樹如下：
這個 TF 樹中有 3 個坐標(biāo)系：
- world：世界坐標(biāo)系
- turtle1：烏龜 1 的坐標(biāo)系，父節(jié)點(diǎn)是 world
- turtle2：烏龜 2 的坐標(biāo)系，父節(jié)點(diǎn)時 world
如果你再發(fā)布一個「xxx -> turtle1」的變換，那 turtle1 就有 2 個父節(jié)點(diǎn)，這樣是不可行的，違反了 TF 樹的構(gòu)建規(guī)則，實(shí)際使用是一定要注意了，如果你想查看系統(tǒng)當(dāng)前的 TF 樹，使用下面的命令：
rosrun rqt_tf_tree rqt_tf_tree
下面來學(xué)習(xí)如何為一個節(jié)點(diǎn)添加子坐標(biāo)系。
二、添加子坐標(biāo)系
同樣進(jìn)入 learning_tf2 包中：
roscd learning_tf2
然后在 src 下新建 frame_tf2_broadcaster.cpp 文件，代碼如下：
#include
#include
#include
int main(int argc, char** argv)
{
ros::init(argc, argv, "my_tf2_broadcaster");
ros::NodeHandle node;
tf2_ros::TransformBroadcaster tfb;
geometry_msgs::TransformStamped transformStamped;
// 指定 carrot1 的父節(jié)點(diǎn)時 turtle1
// 即添加一個新的 carrot1 子坐標(biāo)系到 turtle1
transformStamped.header.frame_id = "turtle1";
transformStamped.child_frame_id = "carrot1";
// carrot1 相對于 tutle1 做了 y 軸的偏移
transformStamped.transform.translation.x = 0.0;
transformStamped.transform.translation.y = 2.0;
transformStamped.transform.translation.z = 0.0;
tf2::Quaternion q;
q.setRPY(0, 0, 0);
transformStamped.transform.rotation.x = q.x();
transformStamped.transform.rotation.y = q.y();
transformStamped.transform.rotation.z = q.z();
transformStamped.transform.rotation.w = q.w();
ros::Rate rate(10.0);
while (node.ok()) {
transformStamped.header.stamp = ros::Time::now();
// 這兩行表示讓該 carrot1 參考系隨著時間移動
transformStamped.transform.translation.x = 2.0 * sin(ros::Time::now().toSec());
transformStamped.transform.translation.y = 2.0 * cos(ros::Time::now().toSec());
// 將 carrot1 相對于 tutle1 的坐標(biāo)變換廣播到 TF 系統(tǒng)中
tfb.sendTransform(transformStamped);
rate.sleep();
printf("sending\n");
}
};
代碼中最關(guān)鍵的就是要正確指定發(fā)布轉(zhuǎn)換的 ID：
frame_id：父坐標(biāo)系名稱child_frame_id：子坐標(biāo)系名稱
然后添加編譯規(guī)則：
add_executable(frame_tf2_broadcaster src/frame_tf2_broadcaster.cpp)
target_link_libraries(frame_tf2_broadcaster ${catkin_LIBRARIES})
編譯：
catkin_make
在 launch 中添加啟動：
<launch>
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<param name="scale_linear" value="2" type="double"/>
<param name="scale_angular" value="2" type="double"/>
<node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle1" name="turtle1_tf2_broadcaster" />
<node pkg="learning_tf2" type="turtle_tf2_broadcaster" args="/turtle2" name="turtle2_tf2_broadcaster" />
<node pkg="learning_tf2" type="turtle_tf2_listener" name="listener" />
<node pkg="learning_tf2" type="frame_tf2_broadcaster" name="broadcaster_frame" />
launch>
開始啟動節(jié)點(diǎn)：
roslaunch learning_tf2 start_demo.launch
不過你應(yīng)該發(fā)現(xiàn)小烏龜?shù)母S運(yùn)動跟上一個實(shí)驗(yàn)一模一樣，我們自己添加的坐標(biāo)系沒有產(chǎn)生作用，這是為何呢？這是因?yàn)殡m然我們發(fā)布了新的變換，但是我們并沒有使用它，來修改下 listener 的代碼：
transformStamped = listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0));
把 /turtle1 坐標(biāo)系改為 /carrot1，因?yàn)槲覀円褂眯绿砑拥淖鴺?biāo)系，所以尋找變換的坐標(biāo)系參數(shù)就要填寫新添加的坐標(biāo)系名稱，這樣系統(tǒng)才能正確找到新添加的變換，再次編譯重新運(yùn)行：
catkin_make
roslaunch learning_tf2 start_demo.launch
你應(yīng)該會發(fā)現(xiàn)現(xiàn)在的小烏龜產(chǎn)生的跟隨運(yùn)動與之前不一定了，兩者之間的 y 方向有一定的距離，這個距離就是我們發(fā)布變換時指定的坐標(biāo)系的相對位置：
OK！TF 系統(tǒng)常用的基礎(chǔ)就學(xué)完了，目前因?yàn)轫?xiàng)目有用到 TF，所以寫了幾篇基礎(chǔ)的文章，之前沒有看過的可以再回過頭看下：
- ROS 機(jī)器人技術(shù) - TF 坐標(biāo)系統(tǒng)基本概念
- ROS 機(jī)器人技術(shù) - 靜態(tài) TF 坐標(biāo)幀
- ROS 機(jī)器人技術(shù) - 廣播與接收 TF 變換
廈大同學(xué)，與你分享編程，AI 算法等技術(shù)干貨！精品文章創(chuàng)作不易，謝謝關(guān)注，歡迎在看。
點(diǎn)擊閱讀原文，查看更多精彩文章！