ros之message_filter

A message filter is defined as something which a message arrives into and may or may not be spit back out of at a later point in time. 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.

Time Synchronizer

The TimeSynchronizer filter synchronizes incoming channels by the timestamps contained in their headers, and outputs them in the form of a single callback that takes the same number of channels. The C++ implementation can synchronize up to 9 channels.

ExactTime Policy

The message_filters::sync_policies::ExactTime policy requires messages to have exactly the same timestamp in order to match. Your callback is only called if a message has been received on all specified channels with the same exact timestamp. The timestamp is read from the header field of all messages (which is required for this policy).

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
{
  // Solve all of perception here...
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;
  message_filters::Subscriber<Image> image_sub(nh, "image", 1);
  message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);

  typedef sync_policies::ExactTime<Image, CameraInfo> MySyncPolicy;
  // ExactTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, info_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();
  return 0;
}

ApproximateTime Policy

The message_filters::sync_policies::ApproximateTime policy uses an adaptive algorithm to match messages based on their timestamp.

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <sensor_msgs/Image.h>

void callback(const sensor_msgs::ImageConstPtr& image1, const sensor_msgs::ImageConstPtr& image2)
{
  // Solve all of perception here...
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;
  message_filters::Subscriber<sensor_msgs::Image> image1_sub(nh, "image1", 1);
  message_filters::Subscriber<sensor_msgs::Image> image2_sub(nh, "image2", 1);

  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> MySyncPolicy;
  // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image1_sub, image2_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();
  return 0;
}

注： void callback(const boost::shared_ptr &, ..., const boost::shared_ptr &) , 回調函數的輸入類型必須是ConstPtr類型的共享指針，當年找了這個錯誤找了一下午，謹記。

CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
             message_filters)

頭文件

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

boost::bind()

• 如果callback()函數是全局函數，如上例所示：

boost::bind(&callback, _1, _2)  

注： _1, _2為占位符， 代表接收的參數， 在函數真正調用的時候傳入真正的參數。占位符的名字知識表示它在調用式中的順序， 最多可以綁定9個占位符。

• 如果callback()函數是類成員函數：

boost::bind(&X::back, this, _1, _2)

注：第二個參數this指針指向當前對象， 從而可以調用當前對象的成員函數，因為this指針占用了一個占位符，所以最多綁定8個參數

message_filter作為成員變量

xx.h

class A
{
private:
typedef message_filters::sync_policies::ApproximateTime<type_a, type_b> MySyncPolicy;
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
boost::shared_ptr<Sync> sync_;
message_filters::Subscriber<type_a> a_sub_;
message_filters::Subscriber<type_b> b_sub_;
};

xx.cpp

A::A(ros::NodeHandle& nh, ros::NodeHandle& priv_nh)
{
  a_sub_.subscribe(nh, a_topic, 100);
  b_sub_.subscribe(nh, b_topic, 100);
  sync_.reset(new Sync(MySyncPolicy(1000), a_sub_, b_sub_));
  sync_->registerCallback(boost::bind(&A::callback, this, _1, _2));
}

這個是