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\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 
};

以上的程序都須要添加如下頭文件：

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

能夠看到示例1、2、三結構都是差很少：

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]一些傳感器數據讀取融合問題的思考

同時訂閱兩個話題？看到沒測試過