ros_多消息同步回调（Synchronizer）

//=====================================================//
ros中 多消息同步回调可以通过ros中的 message_filters相关模块实现。
所谓多消息同步回调指：用同一个回调函数，处理时间一致或相近的多个订阅的消息。
参考了roswiki相关内容 message_filters后，总结如下：

1 Time Synchronizer

TimeSynchronizer根据多个传入消息的时间戳进行同步，从而实现调用同一个回调函数的目的。看一个roswiki官网的例子：roswiki_message_filters

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.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);
  TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}

关键步骤：
1）建立需要订阅的消息对应的订阅器

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

2）建立同步器

TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);

3）为同步器设置回调函数

sync.registerCallback(boost::bind(&callback, _1, _2));

4）构建多消息回调函数

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

2 Policy-Based Synchronizer [ROS 1.1+]

另外，ros还有一种Policy-Based的消息同步机制，本质与上述Time Synchronizer类似。它分为两种，ExactTime Policy 和ApproximateTime Policy。

2.1 ExactTime 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;
}

2.2 ApproximateTime Policy

这种方法根据输入消息的时间戳进行近似匹配，不要求消息时间完全相同。

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

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image1, const 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<Image> image1_sub(nh, "image1", 1);
  message_filters::Subscriber<Image> image2_sub(nh, "image2", 1);

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

  ros::spin();

  return 0;
}

3 多消息同步回调应用实例

参考 ros消息时间同步与回调
此例在类中定义消息订阅器、同步器、消息同步机制、回调函数等；然后在类的构造函数中为订阅器、同步器开辟空间和初始化，并为同步器设定回调函数。

//qrslam.h
class QrSlam
{
	//
	...
	//
 typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry,sensor_msgs::Image> slamSyncPolicy;

  message_filters::Subscriber<nav_msgs::Odometry>* odom_sub_ ; // topic1 输入
  message_filters::Subscriber<sensor_msgs::Image>* img_sub_;   // topic2 输入
  message_filters::Synchronizer<slamSyncPolicy>* sync_;
}

//qrslam.cpp
QrSlam::QrSlam(){

    odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(ar_handle, "/odom", 1);
    img_sub_  = new message_filters::Subscriber<sensor_msgs::Image>(ar_handle, "/usb_cam/image_raw", 1);
   
    sync_ = new  message_filters::Synchronizer<slamSyncPolicy>(slamSyncPolicy(10), *odom_sub_, *img_sub_);
    sync_->registerCallback(boost::bind(&QrSlam::combineCallback,this, _1, _2));
    }

完整代码：kintzhao/cv_ekfslam

4 相关参考

http://wiki.ros.org/message_filters
ros消息时间同步与回调
kintzhao/cv_ekfslam