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(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 point_sub_;
  tf::TransformListener tf_;
  tf::MessageFilter * 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& 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 
};