在機器人領(lǐng)域里闹司，多個坐標系統(tǒng)應(yīng)該是最常見的事之一了。
一個機械臂沐飘，每一個關(guān)節(jié)都需要一個坐標系游桩，一個無人小車不同的傳感器一般都需要自己的坐標系。必不可少的耐朴，他們都需要世界坐標系借卧。
有些坐標系是移動的，比如在一個車子上的傳感器的自己的坐標系筛峭，隨著車子的移動而移動铐刘；有的坐標系是靜止的，比如世界坐標系影晓。那些固定在機器人身上的傳感器镰吵，他們之間的位置是固定的，有了一者的位置挂签，我們應(yīng)該就能知道另一者的位置疤祭。
追蹤不同的坐標系，看起來是個很簡單的活饵婆，但是實現(xiàn)起來挺復(fù)雜的勺馆，各個坐標系之間需要良好的及時的通訊，而ROS最牛叉的就是用來通訊嘛侨核。
在這一講谓传，我們會使用ROS的tf包，去解決不同坐標系追蹤的問題芹关。我們首先使用官網(wǎng)的第一個例子入門，然后再用我自己編寫的例子更深入的了解紧卒。我自己的例子要實現(xiàn)的功能是：假設(shè)有一個在移動的機器人侥衬，它上面有兩個能定位的傳感器(比如GPS和相機，相機定位通過SLAM實現(xiàn))跑芳。相機和GPS都有自己的坐標系轴总，這兩坐標系要一直附著在我們的機器人上移動即位置相對機器人固定，但是相對世界坐標系移動博个。結(jié)果會在rviz上顯示出來怀樟。你們將會看到如下內(nèi)容:

moving_robot.gif

從動圖中我們可以看到在機器人身上有兩個坐標系，一個叫camera一個叫g(shù)ps盆佣，他們的位置相對固定往堡，然后整個機器人相對世界坐標系移動械荷。(說好的機器人怎么是個綠方塊啊喂!!恩...你就假裝它是個機器人好了...)。下面我們就來看一下怎么實現(xiàn)上面描述的功能虑灰。
之前我們講坐標系變換是通過tf這個package實現(xiàn)的吨瞎，不過我才發(fā)現(xiàn)tf的更新版tf2都出來不少時間了，他們之間大同小異穆咐。我們就使用最新的tf2吧颤诀。由于上一講我們通過

catkin_create_pkg learn_rviz_tf roscpp rospy std_msgs geometry_msgs visualization_msgs tf

創(chuàng)建了learn_rviz_tf這個package，但是現(xiàn)在我們想添加新的依賴項tf2進去对湃，需要修改CMakeLists.txt和package.xml崖叫，具體如果和修改我們在機器人操作系統(tǒng)ROS:從入門到放棄(三)　發(fā)布接收不同消息2
里講過，這里不贅述了拍柒。你如果從我的github上直接下載這個package當然就不用修改什么了心傀，我已經(jīng)改好了。

tf2基礎(chǔ)例子

ROS官網(wǎng)例子鏈接如下
http://wiki.ros.org/tf2/Tutorials
我們使用第二個 Writing a tf2 broadcaster (C++)來講解斤儿。
進入鏈接后我們直接從2.1剧包，the code部分開始，在我們的learn_rviz_tf/src中創(chuàng)建turtle_tf2_broadcaster.cpp并把代碼復(fù)制進去往果。這兒為了方便講解我也復(fù)制過來了

#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg){
  static tf2_ros::TransformBroadcaster br;
  geometry_msgs::TransformStamped transformStamped;
  
  transformStamped.header.stamp = ros::Time::now();
  transformStamped.header.frame_id = "world";
  transformStamped.child_frame_id = turtle_name;
  transformStamped.transform.translation.x = msg->x;
  transformStamped.transform.translation.y = msg->y;
  transformStamped.transform.translation.z = 0.0;
  tf2::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

  br.sendTransform(transformStamped);
}

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf2_broadcaster");

  ros::NodeHandle private_node("~");
  if (! private_node.hasParam("turtle"))
  {
    if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
    turtle_name = argv[1];
  }
  else
  {
    private_node.getParam("turtle", turtle_name);
  }
    
  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

  ros::spin();
  return 0;
};

官網(wǎng)的代碼解讀不甚詳細疆液，我贅述一遍并加點東西。
首先坐標系之間轉(zhuǎn)換的發(fā)布不再是定義一個publisher然后用publisher.publish()函數(shù)來發(fā)布了陕贮，而是在tf2_ros::TransformBroadcaster這個類下定義一個專門的發(fā)布坐標轉(zhuǎn)換的對象堕油，使用對象.sendTransform(msg_type)來發(fā)布坐標之間的關(guān)系。發(fā)布的msg_type是特定的消息類型geometry_msgs/TransformStamped.h肮之〉羧保基于這一點我們來看代碼。
1:
頭文件部分包含了#include <tf2/LinearMath/Quaternion.h>戈擒，代碼有一行定義了tf::quaternion的對象tf::Quaternion q需要通過包含這個頭文件來實現(xiàn)眶明。
回想我們在使用geometry_msgs/PoseStamped時曾需要定義消息的pose，而pose包含position和orientation筐高，其中orientation的消息類型是geometry/Quaternion搜囱，也就是說我們也可以通過geometry::Quaternion q定義一個四元數(shù)對象q，這也是四元數(shù)柑土。這兩種四元數(shù)的定義的對象是有很大的區(qū)別的蜀肘。后者只能通過q.w, q.x, q.y, q.z調(diào)用四元數(shù)包含的數(shù)據(jù)。而前者擁有更廣泛的作用稽屏。具體可以參考
tf: tf::Quaternion Class Reference
點進去, public member function顯示了tf::Quaternion對象可以調(diào)用的一切函數(shù)扮宠。其中就有

tf_setRPY.png

我們上面的代碼中使用了這個函數(shù)，q.setRPY(0, 0, msg->theta)狐榔，這個函數(shù)完成了從roll pitch yaw到四元數(shù)的轉(zhuǎn)換功能坛增，通過設(shè)定roll pitch yaw來設(shè)定四元數(shù)的具體數(shù)據(jù)获雕。tf::Quaternion定義的對象q獲取四元數(shù)的具體方法是q.x(),q.y()...有別于geometry::Quaternion定義的q通過q.x,q.y...的獲取方式。x(),y()..這幾個函數(shù)我們并沒有在上面的鏈接看到轿偎，原因是tf::Quaternion繼承了QuadWord這個類的典鸡，在后者的定義中我們能看到能通過x()，y()這類型的函數(shù)來獲取quaternion的具體數(shù)據(jù)坏晦。這需要讀者去仔細尋找資料了萝玷，在多使用ROS的過程中你會獲取到更多的經(jīng)驗。
另外很重要的是昆婿，在鏈接中我們可以看到下面的內(nèi)容

quaternion_operator.png

tf::Quaternion q_AB //A坐標到B坐標的的旋轉(zhuǎn)變換
tf::Quaternion q_BC //B坐標到C坐標的旋轉(zhuǎn)變換

那么你可以通過

q_AB * q_BC

來獲取到A坐標到C坐標的旋轉(zhuǎn)變換仓蛆。geometry_msgs定義的四元數(shù)是不具備上面的任何功能的睁冬，僅僅是儲存數(shù)據(jù)并用來發(fā)布或者接收。
這里啰嗦了這么多看疙，主要是希望大家通過例子能學會自己找到相關(guān)資料豆拨，這是最重要的。比如以后使用了tf::Transform定義了對象tf1能庆，那么tf1是什么東西施禾，能包含哪些功能，有哪些函數(shù)可以調(diào)用搁胆？你在google上搜尋tf::Transform進入 tf: tf::Transform Class Reference就自然可以查到弥搞。
2：
頭文件包含了<tf2_ros/transform_broadcaster.h>。包含了這個頭文件渠旁，就可以用代碼中的tf2_ros::TransformBroadcaster br來定義一個發(fā)布坐標轉(zhuǎn)換的對象了攀例。
3：
頭文件包含了<geometry_msgs/TransformStamped.h>，即我們之前說的br發(fā)布的內(nèi)容需要是geometry_msgs命名空間下顾腊，TransformStamped類型的消息粤铭。可以看到要發(fā)布的消息類型在代碼中這么定義的
geometry_msgs::TransformStamped transformStamped;
另外在代碼中有一行br.sendTransform(transformStamped)就是用來發(fā)布坐標轉(zhuǎn)換的消息的代碼杂靶。
總的來說承耿，這和我們一般定義publisher并發(fā)布消息的過程很像

ros::Publisher pub_abc = .... //定義publisher
std_msgs::Float64 abc; //定義要發(fā)布的消息類型
abc.data = .....; //給要發(fā)布的消息賦值 
pub_abc.publish(abc);//發(fā)布消息

只不過到發(fā)布坐標轉(zhuǎn)換這兒變成了

tf2_ros::TransformBroadcaster br; //定義坐標轉(zhuǎn)換的publisher
geometry_msgs::TransformStamped transformStamped; //定義坐標轉(zhuǎn)換要發(fā)布的消息類型(只能是此類型)
...//賦值，賦值部分是最上面代碼中位于poseCallback()函數(shù)中transformedStamed.header.stamp = ...以及后面的是來行的內(nèi)容伪煤，稍后再講
br.sendTransform(transformStamped);//發(fā)布坐標轉(zhuǎn)換

可以看到發(fā)布坐標變換的整體過程和發(fā)布普通消息是類似的，只不過使用的函數(shù)凛辣，消息名字有些變化而已抱既。
4：
頭文件包含了#include <turtlesim/Pose.h>. ROS真是很喜歡用turtlesim這個package的內(nèi)容來講解了，具體里面包含的內(nèi)容我也沒去看過扁誓。不過相信大家在自己學習ROS官網(wǎng)的教程時都使用過這個package防泵，打開過一個窗口看到兩個可愛的小烏龜了蚀之。
我們的poseCallback函數(shù)的形參是這樣的(const turtlesim::PoseConstPtr& msg)，對比我們寫過很多的callback函數(shù)的形參比如接收flaot類型消息的(const std_msgs::Float64::ConstPtr& msg)
我們就能猜到#include <turtlesim/Pose.h>是包含了消息類型Pose捷泞，這個Pose專門用來定義小烏龜?shù)奈恢玫摹?定義ConstPtr時前面居然沒有作用域符號::足删，我也吃了一驚，不過試了一下確實有沒有都可以Em...)锁右。
5: std::string turtle_name 用來接收后面要寫的launch文件傳遞進來烏龜?shù)拿质堋Ｈ绾问褂胠aunch文件傳遞參數(shù)參考使用roslaunch那一節(jié)。
6: void poseCallback(const turtlesim::PoseConstPtr& msg), 這個回調(diào)函數(shù)對應(yīng)的subscriber我們在main函數(shù)中可以看到咏瑟。ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);也就是說它subscribe的topic是turtle_name+"/pose".一會兒我們運行程序我們就能看到小烏龜拂到，小烏龜?shù)奈恢脮r刻發(fā)布出來，這個subscriber就會接收
回調(diào)函數(shù)的內(nèi)容码泞，一二行我們已經(jīng)講過兄旬，第一行用了static定義br，主要是避免重復(fù)定義余寥。就像定義publisher我們肯定也只定義一次领铐。遇到這種情況其實我們最好把回調(diào)函數(shù)寫在類中，把br定義為類成員(見在類中publish和subscribr那一講)宋舷。
回調(diào)函數(shù)的第三行開始給我們要發(fā)布的坐標轉(zhuǎn)換賦值绪撵。關(guān)于transformStamped包含哪些成員，怎么使用肥缔，賦值等莲兢，和我們之前在第三講過的poseStamped類似。
transformStamped.header.stamp = ros::Time::now();坐標之間的關(guān)系可能變化续膳，那么自然我們在定義坐標之間的關(guān)系時改艇，自然要給它賦值在什么時刻，坐標之間的關(guān)系如何
transformStamped.header.frame_id = "world" 和 transformStamped.child_frame_id = turtle_name;兩行坟岔，既然涉及到兩個坐標之間的關(guān)系谒兄，我們肯定需要知道兩個坐標系的名字，所以world那一行定義了parent坐標系的名字社付，turtle_name那一行定義了child坐標系的名字承疲。
那么具體兩個坐標之間的關(guān)系是怎么樣的呢？自然就涉及到給坐標系之間的rotation和translation賦值鸥咖。我們說回調(diào)函數(shù)接收到的是turtle的位置和方向燕鸽，那么msg中的位置和方向我們就需要賦值transformStamped。即下面幾行啼辣。

  transformStamped.transform.translation.x = msg->x;
  transformStamped.transform.translation.y = msg->y;
  transformStamped.transform.translation.z = 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();

中間省略的兩行

  tf2::Quaternion q;
  q.setRPY(0, 0, msg->theta);

主要是換個方式表達rotation啊研。后面我們可以看到由于烏龜是二維運動，所以msg涉及到的只有x,y和theta. theta即三維坐標中的yaw角度，這就是為什么利用q.setRPY(0, 0, msg->theta);定義了quaternion具體的內(nèi)容党远。之后把tf的quaternon的值賦值給我們要發(fā)布的消息類型transformStamped所包含的quaternion的值就可以了削解。
當transformed的內(nèi)容都設(shè)置好了后br.sendTransform(transformStamped);發(fā)布坐標轉(zhuǎn)換。

5：主函數(shù)中沟娱，ros::NodeHandle private_node("~")和我們平時定義node的方式不是很一樣氛驮。平時我們定義node都是直接ros::NodeHandle nh。這里多了一個參數(shù)～济似。它表示此時這個nodehandle是讀取局部參數(shù)的矫废。具體可見這篇文章https://blog.csdn.net/shijinqiao/article/details/50450844〖钇ǎ總體作用和使用方式和普通node沒有大的區(qū)別磷脯。:
6：main函數(shù)的if else語句就是要從launch文件中讀取turtle_name那個參數(shù)。
再之后定義獲取烏龜pose的subscriber娩脾。為什么有兩個nodehandle呢赵誓，我們在launch文件中解釋。
把這個文件添加到CMakeLists.txt編譯柿赊。
之后我們在learn_rviz_tf目錄下添加一個launch文件夾俩功，創(chuàng)建一個start_demo.launch。寫入下面的內(nèi)容碰声。

  <launch>
     <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    <!-- Axes -->
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>

    <node pkg="learn_rviz_tf" type="turtle_tf2_broadcaster"
          args="/turtle1" name="turtle1_tf2_broadcaster" />
    <node pkg="learn_rviz_tf" type="turtle_tf2_broadcaster"
          args="/turtle2" name="turtle2_tf2_broadcaster" />

  </launch>

注意這兒和原tutorial的launch文件有些小出入诡蜓。原launch文件第三個和第四個node的后面pkg的名字是learning_tf2，因為他們創(chuàng)建了一個叫l(wèi)earningtf2的package來做tf的tutorial胰挑，而我們的現(xiàn)在代碼是在learning_rviz_tf這個pakcage里蔓罚，所以package名字改一下，不是大問題瞻颂。
寫好roslauch編譯好文件后我們先來運行一下程序看看什么效果豺谈。

roslaunch learn_rviz_tf start_demo.launch

看到下圖一個小烏龜出來了。

turtle.png

重新點擊跑roslaunch的終端贡这，你的小烏龜會移動茬末。打開另一個terminal，在其中輸入

rosrun tf tf_echo /world /turtle1

這時候你會看到類似于下面的內(nèi)容

show_tf.png

每一個

At time ...
- Transolation ..
- Rotation ...

是一組tf盖矫，顯示的是在某個時間點,world坐標系和turtle坐標系之間的關(guān)系丽惭。當你移動小烏龜時，translation和rotation會發(fā)生改變辈双。
下面解釋launch文件中的內(nèi)容
1：

<node pkg="turtlesim" type="turtlesim_node" name="sim"/>

這個節(jié)點是顯示烏龜這個GUI的责掏，不用深究。
2：

<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

這是為了使你的的鍵盤能控制烏龜?shù)囊苿印?從key那個名字就可以看出)不用深究湃望。
3：
param兩行的參數(shù)目前在我們程序中沒有使用换衬。
4：

<node ... args = `/turtle1`...>

即運行剛剛寫的程序
5：
下面一個node暫時沒有使用局义。官網(wǎng)關(guān)于tf2的tutorial的另一個例子會使用。
好像并沒有解釋什么...
最后回到程序中關(guān)于praivate_node的問題冗疮，為什么多用一個nodeHandle?其實我們在launch文件中可以發(fā)現(xiàn)

    <node pkg="learn_rviz_tf" type="turtle_tf2_broadcaster"
          args="/turtle1" name="turtle1_tf2_broadcaster" />
    <node pkg="learn_rviz_tf" type="turtle_tf2_broadcaster"
          args="/turtle2" name="turtle2_tf2_broadcaster" />

這代表這兩個node其實使用的是一個可執(zhí)行文件，即turtle_tf2_broadcast檩帐，為了區(qū)分他們术幔，給了他們不同的名字turtle1_tf2_broadcaster，turtle2_tf2_broadcaster湃密，即同樣的執(zhí)行文件不同的**節(jié)點
**名诅挑。同時這兩個節(jié)點的傳入args不同。
當好幾個節(jié)點都來自于一個可執(zhí)行文件時泛源，我們難免遇到同一個問題拔妥。如果他們都需要傳入某個名叫A的參數(shù)，但是A的值需要不一樣达箍，該怎么辦呢没龙？這時候我們通常要把要讀取的參數(shù)分開寫到節(jié)點內(nèi)。如下

<param name="A" value="11" />
  <node name="you" pkg="ABC" type="abc" output="screen">
    <param name="A" value="10" />
  </node>
  <node name="me" pkg="ABC" type="abc" output="screen">
    <param name="A" value="9" />
  </node>

在<node>...</node>內(nèi)部定義的參數(shù)稱為局部參數(shù)缎玫，必須定義一個私有nodehandle, 即ros::NodeHandle abc("~")并使用代碼abc.getParam(...)才能讀取
例如上面為了讓節(jié)點you和me分別讀取值為10的A和值為9的A硬纤，我們需要在程序內(nèi)定義帶參數(shù)的"~"的nodeHandle。表示讀取自己內(nèi)部的參數(shù)赃磨，同時不要讀取值為11的全局參數(shù)A筝家。
但其實上面的launch文件沒有重復(fù)名字的參數(shù)(args不算)，理論上可以不用局部的nodehandle邻辉。我把("~")去掉了也一切正常溪王。不過大家以后要在一個launch文件中使用相同名字但值不同的參數(shù)時，一定要考慮這個問題值骇。
所以總的來說莹菱，我們原來的程序只定義一個node并且不添加參數(shù)("~")也沒有問題，由于沒用args為turtle2的節(jié)點雷客，所以把args = "turtle2"那一個節(jié)點刪掉了也沒有問題芒珠，本來可以大大做簡化，但是考慮到很多同學還是按照官網(wǎng)的例子學習搅裙，所以就冗雜地解釋了一番皱卓。
在程序運行時，我們在使用rviz來可視化tf部逮。打開一個新的terminal娜汁，在其中輸入

rosrun rviz rivz

上一章節(jié)我們?yōu)榱丝梢暬痯ose通過ADD按鈕添加了marker，現(xiàn)在類似兄朋，rviz打開后點擊左下方的ADD按鈕
掐禁，在顯示出的對話框中選擇TF，由于我們發(fā)布的TF是關(guān)于world和turtle1的，我們希望world坐標系是固定的傅事，所以在rviz中把Fixed Frame改成world缕允，這時候你剛才roslaunch跑的程序所發(fā)布的坐標系之間的轉(zhuǎn)換就正在被rviz接收了。你會看到類似下圖的東西

tf_world_turtle1.png

rosrun rqt_tf_tree rqt_tf_tree

輸入該命令后會出現(xiàn)類似于下面的圖像

rqt_tf_tree.png

這表示world是母坐標系,turtle1是子坐標系驾霜。

tf2多個坐標系追蹤

講完了基礎(chǔ)例子，我們就可以來實現(xiàn)我第一張動圖moving_robot.gif买置。在那個動圖中粪糙，我們有三個坐標系，camera和gps相對靜止忿项，和我們的機器人(方塊marker)一塊兒相對于世界坐標系world移動蓉冈。官網(wǎng)的例子很不錯，不過turtlesim之類的東西內(nèi)部是咋樣的我們畢竟不是那么了解倦卖，還有局部nodeHandle洒擦，幾個例子里還使用了service之類，雖然都不是復(fù)雜東西怕膛，但和在一起總會有人不清楚其中的部分熟嫩。我還是傾向于大家能懂tutorial的例子的每一行代碼，代碼的內(nèi)容我們在之前都有所接觸褐捻。
我們在learn_rviz_tf/src中創(chuàng)建一個叫moving_coordinate_system.cpp的文件掸茅。寫入下面內(nèi)容

#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
#include <tf2/LinearMath/Quaternion.h>

class MovingObject{
public:
    MovingObject(ros::NodeHandle& nh);
    void PoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
    void set_marker_fixed_property();
private:
    ros::Publisher pub_object_;
    visualization_msgs::Marker mk_;
    tf2_ros::TransformBroadcaster br_;
};

int main(int argc, char** argv){
    ros::init(argc, argv, "tf2_broadcaster");

    ros::NodeHandle nh;

    MovingObject mo(nh);

    ros::Subscriber sub_gps = nh.subscribe("/chatter", 10, &MovingObject::PoseCallback, &mo);
  
    ros::spin();
}

MovingObject::MovingObject(ros::NodeHandle& nh){
    pub_object_ = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);
    set_marker_fixed_property();
}

void MovingObject::PoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg){
    ROS_INFO("get gps position, %f, %f, %f", msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
    geometry_msgs::TransformStamped transformStamped;
    transformStamped.header.stamp = ros::Time::now();
    transformStamped.header.frame_id = "world";
    transformStamped.child_frame_id = "gps";
    transformStamped.transform.translation.x = msg->pose.position.x - 0.1;
    transformStamped.transform.translation.y = msg->pose.position.y;
    transformStamped.transform.translation.z = msg->pose.position.z;
    transformStamped.transform.rotation.w = msg->pose.orientation.w;
    transformStamped.transform.rotation.x = msg->pose.orientation.x;
    transformStamped.transform.rotation.y = msg->pose.orientation.y;
    transformStamped.transform.rotation.z = msg->pose.orientation.z;
    br_.sendTransform(transformStamped);

    mk_.header.stamp = ros::Time();
    mk_.pose = msg->pose;
    pub_object_.publish(mk_);
}

void MovingObject::set_marker_fixed_property(){
    mk_.ns = "my_namespace";
    mk_.id = 0;
    mk_.header.frame_id = "world";
    mk_.type = visualization_msgs::Marker::CUBE;

    mk_.scale.x = 0.5;
    mk_.scale.y = 0.5;
    mk_.scale.z = 0.5;

    mk_.color.a = 0.3; 
    mk_.color.r = 0.0;
    mk_.color.g = 1.0;
    mk_.color.b = 0.0;

    mk_.action = visualization_msgs::Marker::ADD;
}

頭文件除了第一個基礎(chǔ)例子的以外，增加了

#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>

我們依次講解程序內(nèi)容
1:我們在上一章可視化marker的時候?qū)懥艘粋€subscriber文件柠逞，用來接收rosbag發(fā)出的機器人pose的信息昧狮，并轉(zhuǎn)換為marker的pose使之能在rviz中顯示出來。而我們現(xiàn)在的例子同樣顯示marker板壮，marker的pose后面可以看到同樣來自于rosbag發(fā)出的消息逗鸣。所以包含了這兩個上一章節(jié)使用過的頭文件，一個用來使用PoseStamped消息绰精，一個用來使用Marker消息撒璧。并不意外。
2:定義了MovingObject類笨使。
成員函數(shù)：
類的構(gòu)造函數(shù)傳入了nodehandle卿樱，用來初始化一些性質(zhì)并定義publisher的內(nèi)容。
PoseCallback函數(shù)用來接收來自于rosbag的機器人位置的消息并轉(zhuǎn)化為marker類型的消息發(fā)布出去硫椰。
set_marker_fixed_property()函數(shù)用來設(shè)置marker一些我們不想改變的性質(zhì)繁调，和上一章類似萨蚕。
成員變量：
數(shù)據(jù)成員pub_object_用來發(fā)布marker的pose使rviz接收
mk_就是maker了
br_即用來發(fā)布坐標系之間的關(guān)系的。
3：主函數(shù)中sub_gps就是用來接收來自rosbag發(fā)布的poseStamped類型消息蹄胰。消息在PoseCallback函數(shù)中處理岳遥。
4：主函數(shù)之后，是MovingObject類的構(gòu)造函數(shù)裕寨，在構(gòu)造函數(shù)中寒随，我們首先給pub_object賦值，負責發(fā)布marker類型的消息懒闷，pub_object發(fā)布的內(nèi)容會用來產(chǎn)生我們動圖1中的小方塊宾濒。構(gòu)造函數(shù)調(diào)用了set_marker_fixed_property()函數(shù)，在上一講我們用這個函數(shù)來設(shè)置一些我們不想要改變的marker性質(zhì)。除了顏色和大小不想改變外泌辫，我們并不想像上次那樣，每接收一個pose就產(chǎn)生一個新的marker峭弟，我們希望當前的marker接收到一個新的pose后就移動到那個位置朗伶，所以marker只能有一個。上一講我們說過marker的id和namespace兩個量共同定義一個marker昨稼，所以在set_marker_fixed_property中把這兩個量設(shè)置為常亮就可以了节视。即

    mk_.ns = "my_namespace";
    mk_.id = 0;

5：最后是PoseCallback函數(shù)。
我們要發(fā)布坐標系之間的tf假栓，和第一個例子一樣寻行，需要定義是哪兩個坐標系，即frame_id和child_frame_id匾荆，分別是world和gps拌蜘。需要定義transform的平移和旋轉(zhuǎn)。pose和transform本就是一個東西牙丽，都需要定義平移和旋轉(zhuǎn)简卧，他們在transformStamped和PoseStamped的中的名字不一樣。所以我們挨個把pose的position賦值給transform的translation烤芦，把pose的orinetation賦值給transform的rotation就可以了举娩。
代碼中有一行

mk_.pose = msg->pose;

我們直接把接收到的消息的pose直接賦值給了marker的pose。如果gps到world的transform也用接收到的消息的pose构罗，那么marker的中心就會和gps坐標系的中心重合铜涉。所以代碼中我把pose.position.x減了0.1，只是人為地把為了gps的坐標系原點和marker中心做點區(qū)別绰播，不是非得加骄噪。
定義好了tranformStamped之后就可以用br_.sendTranform把它發(fā)布出去，定義好了marker的pose之后蠢箩，由于它的其他性質(zhì)我們已經(jīng)在set_marker_fixed_property中定義链蕊，那么就可以用pub_object_把marker發(fā)布出去了事甜。這兩個東西同時發(fā)布出去，用的同一個pose(除了x方向有0.1的固定差別)滔韵，如果我們像第一個例子那樣編譯并跑程序逻谦，設(shè)置好rviz，跑rosbag文件發(fā)布pose陪蜻，理論上就可以在rviz當中看到gps這個坐標系隨著我們的機器人(marker一起移動了)邦马。
等一下！還有一個坐標系呢宴卖？滋将？動圖里還有個camera坐標系，可是仔細看我們的代碼里症昏，沒有任何關(guān)于camera的東西随闽。原來當兩個坐標系相對靜止的時候我們，我們最好直接把他們之間的tf直接設(shè)置到launch文件里肝谭。
首先我們把源文件寫到CMakelists里進行編譯掘宪。

add_executable(moving_coordinate_system src/moving_coordinate_system.cpp)
target_link_libraries(moving_coordinate_system ${catkin_LIBRARIES})

然后在launch文件夾里創(chuàng)建一個叫run_mcs.launch的launch文件。并把下面的內(nèi)容寫到launch文件中

<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="link1_broadcaster" args="0.3 0 0 0 0 0 1 gps camera" />
    <node type="moving_coordinate_system" pkg="learn_rviz_tf" name="moving_coordinate_system" output="screen">
    </node>
</launch>

可以看到<node pkg = "tf2_ros"...>那一行攘烛，我們使用了名叫tf2_ros這個pakcage里名叫static_transform_publisher這個可執(zhí)行文件魏滚，節(jié)點名我們命名為link1_broadcaster，args即我們要傳入的transform了坟漱，前三個數(shù)字是transform的平移鼠次，后四個是四元數(shù)x,y,z,w(如果只有三個數(shù)字默認roll pitch yaw)。gps camera表示前面那組平移旋轉(zhuǎn)就是這兩個坐標系的關(guān)系芋齿。接著如果我們運行我們剛剛編譯好的moving_coordinate_system须眷，就可以看到一個新的坐標系出現(xiàn)在rviz里了。
我們先打開rviz

rosrun rviz rviz

如果你保存了之前rviz的信息沟突，現(xiàn)在你rviz可能直接能接收marker和tf的信息花颗，但我們假設(shè)是空白的。現(xiàn)在由于我們的程序既要發(fā)布marker的消息惠拭，又要發(fā)布tf的消息扩劝，自然我們需要加兩個接收器。如上一章一樣职辅，點擊rviz中ADD按鈕棒呛，選擇Marker點擊OK。再次點擊ADD按鈕域携，選擇TF簇秒，點擊OK。把Global Options下面的Fixed_Frame設(shè)置為world秀鞭。這時候你的rviz界面應(yīng)該是這樣的(注意左邊欄包含了TF和Marker)趋观。

moving_coordinate_rviz_set.png

roslaunch learn_tf_rviz run_mcs.launch

這時候我們注意到rviz中有些變化扛禽，如下圖

rviz_error.png

rosbag play robot_pose.bag

這行命令一執(zhí)行，rviz里的錯誤會消失吸祟，你也就可以看到類似于我們本講第一個動圖里的東西了瑟慈。

總結(jié)

關(guān)于靜態(tài)坐標系之間的關(guān)系，當然也可以在程序中設(shè)置屋匕，不過直接寫到launch文件中是優(yōu)先推薦的葛碧。關(guān)于tf還有很多其他內(nèi)容，在官網(wǎng)中有寫过吻。
tf2/Tutorials - ROS Wiki
我們沒有講到的进泼，既然能寫tf的發(fā)布程序，那么我們也能寫tf的接收程序纤虽，這和publisher,subscriber一個道理乳绕。即官網(wǎng)里writing a tf listener那一講。另外我們在設(shè)置transformStamped的時候總是設(shè)置了每一個tf對應(yīng)的時間的逼纸，這意味著我們可以隨時查看之前的tf洋措，即learn about tf and time那一講。我再講一次就太冗余了杰刽，能看到這兒相信那些剩下的內(nèi)容你也自己能看懂了菠发。關(guān)于rviz和tf我們就說這么多王滤，至少有個簡單的認識，當你實際遇到更多的問題時雷酪，google一下淑仆。