ROS 機(jī)器人技術(shù) - 添加一個(gè) TF 幀
登龍
這是我的第 140 篇原創(chuàng)
在實(shí)際的機(jī)器人中往往有很多個(gè)傳感器，比如我們組目前用的小車(chē)上就有相機(jī)，雷達(dá)，IMU 等，為了能夠在 TF 系統(tǒng)中找到傳感器之間的轉(zhuǎn)換，就需要把每個(gè)傳感器的坐標(biāo)系加到系統(tǒng)的 TF 樹(shù)中，方法很簡(jiǎn)單，下面一起來(lái)學(xué)習(xí)下。
一、TF 樹(shù)的注意事項(xiàng)
在實(shí)際使用和調(diào)試 TF 的時(shí)候一定要時(shí)刻記?。?strong style="color:#000000;">TF 樹(shù)中的一個(gè)節(jié)點(diǎn)可以有多個(gè)子節(jié)點(diǎn)，但是只能有一個(gè)父節(jié)點(diǎn)，并且 TF 樹(shù)中不能出現(xiàn)回環(huán)！
一個(gè)典型的 TF 樹(shù)如下：
這個(gè) TF 樹(shù)中有 3 個(gè)坐標(biāo)系：
- world：世界坐標(biāo)系
- turtle1：烏龜 1 的坐標(biāo)系，父節(jié)點(diǎn)是 world
- turtle2：烏龜 2 的坐標(biāo)系，父節(jié)點(diǎn)時(shí) world
如果你再發(fā)布一個(gè)「xxx -> turtle1」的變換，那 turtle1 就有 2 個(gè)父節(jié)點(diǎn)，這樣是不可行的，違反了 TF 樹(shù)的構(gòu)建規(guī)則，實(shí)際使用是一定要注意了，如果你想查看系統(tǒng)當(dāng)前的 TF 樹(shù)，使用下面的命令：
rosrun rqt_tf_tree rqt_tf_tree
下面來(lái)學(xué)習(xí)如何為一個(gè)節(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)時(shí) turtle1
// 即添加一個(gè)新的 carrot1 子坐標(biāo)系到 turtle1
transformStamped.header.frame_id = "turtle1";
transformStamped.child_frame_id = "carrot1";
// carrot1 相對(duì)于 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 參考系隨著時(shí)間移動(dòng)
transformStamped.transform.translation.x = 2.0 * sin(ros::Time::now().toSec());
transformStamped.transform.translation.y = 2.0 * cos(ros::Time::now().toSec());
// 將 carrot1 相對(duì)于 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 中添加啟動(dòng)：
<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>
開(kāi)始啟動(dòng)節(jié)點(diǎn)：
roslaunch learning_tf2 start_demo.launch
不過(guò)你應(yīng)該發(fā)現(xiàn)小烏龜?shù)母S運(yùn)動(dòng)跟上一個(gè)實(shí)驗(yàn)一模一樣，我們自己添加的坐標(biāo)系沒(méi)有產(chǎn)生作用，這是為何呢？這是因?yàn)殡m然我們發(fā)布了新的變換，但是我們并沒(méi)有使用它，來(lái)修改下 listener 的代碼：
transformStamped = listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0));
把 /turtle1 坐標(biāo)系改為 /carrot1，因?yàn)槲覀円褂眯绿砑拥淖鴺?biāo)系，所以尋找變換的坐標(biāo)系參數(shù)就要填寫(xiě)新添加的坐標(biāo)系名稱，這樣系統(tǒng)才能正確找到新添加的變換，再次編譯重新運(yùn)行：
catkin_make
roslaunch learning_tf2 start_demo.launch
你應(yīng)該會(huì)發(fā)現(xiàn)現(xiàn)在的小烏龜產(chǎn)生的跟隨運(yùn)動(dòng)與之前不一定了，兩者之間的 y 方向有一定的距離，這個(gè)距離就是我們發(fā)布變換時(shí)指定的坐標(biāo)系的相對(duì)位置：
OK！TF 系統(tǒng)常用的基礎(chǔ)就學(xué)完了，目前因?yàn)轫?xiàng)目有用到 TF，所以寫(xiě)了幾篇基礎(chǔ)的文章，之前沒(méi)有看過(guò)的可以再回過(guò)頭看下：
- 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)擊閱讀原文，查看更多精彩文章！