1.增加一個座標系

前面我們的效果是讓turtle2跟著turtle1走，那麼做到有一定佈局地跟隨呢？事實上原理比較簡單，我們只需再構造一個turtle1的子座標系，然後讓turtle2跟著這個子座標系走即可。

需要注意的是，每個子座標系都只能有一個父座標系，而一個座標系是可以有多個子座標系的。

 $ roscd learning_tf

src/frame_tf_broadcaster.cpp

   1 #include <ros/ros.h>
   2 #include <tf/transform_broadcaster.h>
   3 
   4 int main(int
 
 argc, char** argv){
   5   ros::init(argc, argv, "my_tf_broadcaster");
   6   ros::NodeHandle node;
   7 
   8   tf::TransformBroadcaster br;
   9   tf::Transform transform;
  10 
  11   ros::Rate rate(10.0);
  12   while (node.ok()){
  13     transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
  14     transform
 
.setRotation( tf::Quaternion(0, 0, 0, 1) );
  15     br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
  16     rate.sleep();
  17   }
  18   return 0;
  19 };

核心程式碼解釋

  13     transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
  14     transform.setRotation( tf::Quaternion
 
(0, 0, 0, 1) );
  15     br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));

setOrigin是設定這個子座標系與父座標系的初始位置關係，三個引數分別問x，y，z，z一般設定為0.

setRotation是設定這個子座標系與父座標系的角度關係，前三個引數為pitch,row,yaw，最後一個引數為角速度。

最後一行就是繼承的父座標系和子座標系名稱和時間起點。

修改src/turtle_tf_listener.cpp

   1   listener.lookupTransform("/turtle2", "/carrot1",
   2                            ros::Time(0), transform);

修改CMakeLists.txt

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

在launch檔案中新增

    <node pkg="learning_tf" type="frame_tf_broadcaster"
          name="broadcaster_frame" />

編譯執行

 $ catkin_make

 $ roslaunch learning_tf start_demo.launch

事實上，我們對引數進行一些簡單的修改就能看到一些有趣的效果

   1     transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) );
   2     transform.setRotation( tf::Quaternion(0, 0, 0, 1) );

例如改變初始位置為一個時間的正餘弦函式，那麼很自然的，turtle2便會繞turtle1做一個圓周運動。

2.tf和時間漫遊

在之前的例子裡，我們的lookupTransform()函式的時間引數都是ros::Time(0)，這裡的0就意味著是最近時刻取樣的座標系。這個引數是可以變化的，比如我們將其改為ros::Time::now()，now的意思就是當前的座標系。但是這時候如果直接執行就睡出現如下錯誤

這是因為一個座標系從一個節點廣播到被另一個節點接收，是需要一定時間的，所以直接訂閱當前時間節點的座標系是找不到的，我們可以做如下修改，等待一個時間確保廣播的資料已經被收到。

  try{
    ros::Time now = ros::Time::now();
    listener.waitForTransform("/turtle2", "/turtle1",
                              now, ros::Duration(3.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             now, transform);

之後再執行就會如我們預期一樣。

我們可以再做一些有趣的嘗試，比如turtle2不再到達turtle1目前的位置，而是到達turtle2 5秒前所在的位置。

  try{
    ros::Time past = ros::Time::now() - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", "/turtle1",
                              past, ros::Duration(1.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             past, transform);

但是結果貌似和我們的預期有很大差距

開始的詭異軌跡可以理解為我們並沒有turtle1在0時刻以前的資料，那之後呢？所以，顯然這裡面有一個隱藏的錯誤。

我們回過頭來看上面那段程式碼，你會發現，我們輸入量是5秒之前的turtle1，但是，我們忽略了我們控制的也是5秒之前的turtle2。

很顯然，我們希望達到的目的是利用5秒之前的turtle1的資訊來控制當前的turtle2的位置，因此我們需要對程式碼做一些修改

  try{
    ros::Time now = ros::Time::now();
    ros::Time past = now - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", now,
                              "/turtle1", past,
                              "/world", ros::Duration(1.0));
    listener.lookupTransform("/turtle2", now,
                             "/turtle1", past,
                             "/world", transform);

這樣結果就和我們預期的一樣了。