激光、摄像头、IMU等传感器数据同步方法(message_filters)

这里介绍一种软同步方法，也就是算法同步方法，如果想要进行硬件同步，可以查阅论文：VersaVIS—An Open Versatile Multi-Camera Visual-Inertial Sensor Suite(好像还有源码)。

message_filters有两种同步策略，一种是时间戳完全对齐的策略(ExactTime Policy)，另一种是时间戳相近的策略(ApproximateTime Policy)。

下面是C++的同步策略框架代码(根据自己需求进行更改)，可以同步多个传感器数据，如果想要使用python代码，可以查阅ros官网。

注意：创建ros包的时候注意加上message_filters，类似于roscpp、sensor_msgs等

ExactTime Policy

message_filters :: sync_policies :: ExactTime策略要求消息具有完全相同的时间戳以便匹配。 只有在具有相同确切时间戳的所有指定通道上收到消息时，才会调用回调。

#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

message_filters :: sync_policies :: ApproximateTime策略使用自适应算法来匹配基于其时间戳的消息。

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

---

参考：

http://wiki.ros.org/message_filters
https://www.cnblogs.com/qiuheng/p/9254557.html