Using sensor messages with tf（Using Stamped datatypes with tf::MessageFilter）

官網：http://wiki.ros.org/tf/Tutorials/Using%20Stamped%20datatypes%20with%20tf%3A%3AMessageFilter

本教程介紹瞭如何將傳感器數據與tf一起使用。一些傳感器數據的實際例子是：

    單目攝像頭和雙目攝像頭    激光雷達

假設創建了一隻名爲turtle3的新龜，但它沒有里程信息；

但有一個高架攝像機跟蹤了其位置，並將其位置作爲與世界座標系相關的geometry_msgs / PointStamped消息發佈出來。

Turtle 1如果想知道turtle3與自身的比較呢？

要做到這一點，turtle1必須收聽正在發佈的關於turtle3姿勢話題，等待轉換到所需的座標已準備好，然後執行其操作。

爲了使這更容易，tf :: MessageFilter類非常有用。

tf :: MessageFilter將訂閱任何ros消息並對其進行緩存，直到可以將其轉換爲目標座標（turtle3相對於turtle1的座標）。

重點：前面幾節都是發佈和接收的各個節點相對於其他節點的tf樹的變換transform，這次直接收發的是某個節點相對其他節點的帶時間戳的位置信息

#include "ros/ros.h"
#include "tf/transform_listener.h"
#include "tf/message_filter.h"
#include "message_filters/subscriber.h"

class PoseDrawer
{
public:
  //這裏的構造函數很有意思，5個私有成員變量，tf_、target_frame_在初始化列表中構造，point_sub_、tf_filter_ 在構造
//函數的函數體中構造，n_沒有提及
  //冒號後面是初始化列表；tf_()沒有參數，是顯式調用默認構造函數（可能加快速度？）
  //變量名後加下劃線，一種命名風格，表示該變量爲類的成員
  //target_frame_目標座標系，此處爲turtle1。
 //重要參數1：目標座標系turtle1
  PoseDrawer() : tf_(),  target_frame_("turtle1")
  {
    /*使用 topic “turtle_point_stamped”初始化message_filters::Subscriber（point_sub_是私有成員變量，類型爲
message_filters::Subscriber）
    重要參數2：turtle3位置消息的topic名 "turtle_point_stamped",
   */
point_sub_.subscribe(n_, "turtle_point_stamped", 10);
/*使用該message_filters::Subscriber初始化tf::MessageFilter，tf::MessageFilter初始化需要的參數還有目標
座標系target_frame，target_frame是tf::MessageFilter會確認transform能夠成功執行的座標系
*/
tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
/*And the callback function is the function which will be called when the data is ready. 回調函數是當數據準備
好時會調用的函數。此處boost::bind(&PoseDrawer::msgCallback, this, _1)的作用是將本類中的成員函數
PoseDrawer::msgCallback綁定到tf::MessageFilter上，成爲其回調函數
*/
    tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
  } ;

private:
  //這是一個模板類，模板參數類型爲geometry_msgs::PointStamped
  message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
  tf::TransformListener tf_;
  tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
  ros::NodeHandle n_;
  std::string target_frame_;

  /*Callback to register with tf::MessageFilter to be called when transforms are available
  *數據準備好後執行如下函數，將turtle3的在全局座標系下的位置消息轉爲在turtle1下的位置消息
  *point_ptr爲turtle3的在全局座標系下的位置消息，target_frame_目標座標系，point_out爲轉換後turtle3在目標坐
*標系下的位置消息。此處再強調一次，前面的教程都是發佈的turtle相對於其他節點的變換transform消息，這次發佈的
*是geometry_msgs::PointStamped類型的msg消息
*point_out就是轉換後turtle3在目標座標系下的位置消息，如果有publisher就可以發佈了
*/
  void msgCallback(const boost::shared_ptr<const geometry_msgs::PointStamped>& point_ptr) 
  {
    geometry_msgs::PointStamped point_out;
    try 
    {
      tf_.transformPoint(target_frame_, *point_ptr, point_out);
      
      printf("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n", 
             point_out.point.x,
             point_out.point.y,
             point_out.point.z);
    }
    catch (tf::TransformException &ex) 
    {
      printf ("Failure %s\n", ex.what()); //Print exception which was caught
    }
  };

};


int main(int argc, char ** argv)
{
  ros::init(argc, argv, "pose_drawer"); //Init ROS
  PoseDrawer pd; //Construct class
  ros::spin(); // Run until interupted 
};