message_filters::Subscriber & tf::MessageFilter理解

希望大家收藏：

本文更新地址：https://haoqchen.site/2018/05/07/understanding-of-message_filters/

左侧专栏还在更新其他ROS实用技巧哦，关注一波？

0. 写在最前面

    因为日常看代码经常能看到tf相关的一些函数，转来转去，绕得很晕，有不懂的就仔细查一下，将自己的理解整理出来，这篇是关于 tf::MessageFilter的。

    message_filters，顾名思义是消息过滤器；tf::MessageFilter，顾名思义是tf下的消息过滤器。消息过滤器为什么要用tf呢？tf::MessageFilter可以订阅任何的ROS消息，然后将其缓存，直到这些消息可以转换到目标坐标系，然后进行相应的处理（一般在回调函数中处理）。说白了就是消息订阅+坐标转换。实际上，后者继承于前者：

    下面给出三个在ROS包中看到的例子：

示例一（amcl中激光雷达的回调）：
tf_ = new TransformListenerWrapper();
message_filters::Subscriber<sensor_msgs::LaserScan>* laser_scan_sub_;
tf::MessageFilter<sensor_msgs::LaserScan>* laser_scan_filter_;

laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);
laser_scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_, 
                                                        *tf_, 
                                                        odom_frame_id_, 
                                                        100);

laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived, this, _1));

void AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan){
    this->tf_->transformPose(base_frame_id_, ident, laser_pose);//这个函数的意思是，ident在base_frame_id下的表示为laser_pose
}

示例二（leg_detector中激光雷达的回调）：
TransformListener tfl_;
message_filters::Subscriber<sensor_msgs::LaserScan> laser_sub_;
tf::MessageFilter<sensor_msgs::LaserScan> laser_notifier_;

laser_sub_(nh_, "scan", 10)
laser_notifier_(laser_sub_, tfl_, fixed_frame, 10)

laser_notifier_.registerCallback(boost::bind(&LegDetector::laserCallback, this, _1))
laser_notifier_.setTolerance(ros::Duration(0.01));

void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan){
    tfl_.transformPoint(fixed_frame, loc, loc);
}

示例三（参考一中的示例）：

class PoseDrawer
{
public:
  PoseDrawer() : tf_(),  target_frame_("turtle1")
  {
    point_sub_.subscribe(n_, "turtle_point_stamped", 10);
    tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
    tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
  } ;

private:
  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
  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);
    }
    catch (tf::TransformException &ex) 
    {
      printf ("Failure %s
", 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 
};

以上的程序都需要添加以下头文件：

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

可以看到示例一、二、三结构都是差不多：

1. 定义数据：TransformListener、message_filters::Subscriber、tf::MessageFilter
2. 用消息的名称来初始化message_filters::Subscriber
3. 用tf、message_filters::Subscriber、目标坐标系来初始化tf::MessageFilter
4. 给tf::MessageFilter注册callback。
5. 编写callback，并在回调中完成坐标转换。至此完成消息订阅+坐标转换

在看message_filters的主页过程中发现，它可以做的远比以上说的多，比如：

An example is the time synchronizer, which takes in messages of different types from multiple sources, and outputs them only if it has received a message on each of those sources with the same timestamp.

有兴趣的自己可以看看。

 

参考：

ROS官网tf::MessageFilter教程

ROS官方tf::MessageFilter文档

ROS官网message_filters主页

ROS官方message_filters::SimpleFilter文档

[ROS]一些传感器数据读取融合问题的思考

同时订阅两个话题？看到没测试过