ROS多传感器数据时间戳同步方案——message_filters::TimeSynchronizer

综述

在ROS开发过程中，有一些时候需要处理来自多个传感器的数据，比如我自己做的一些东西，就需要把不同相机的点云数据进行融合，融合的过程中需要判断传感器数据的时间戳，保证融合的数据尽可能是两个传感器的同步数据，一般的实现是用队列来存储传感器的数据，在不断的出队过程中比较传感器队列的时间戳，如果时间戳之间相差的时间在容忍范围内，就可以认为是同步的。

std::queue buf1;
std::queue buf2;
std::thread process;
//typedef M 传感器数据类型，比如sensor_msg::PointCloud2、sensor_msg::Image等等
void callback1(M& msg){
	//数据入队
	buf1.push(msg);
	//其他代码。。。
}
void callback2(M& msg){
	//数据入队
	buf2.push(msg);
	//其他代码。。。
}
void process_comparation(){
	while(ros::ok()){
		M data1=buf1.front();
		M data2=buf2.front();
		if(data1.header.timestamp.toSec()>data1.header.timestamp.toSec()){
			buf2.pop();
		}
		else 
			buf1.pop();
		//其他操作data1和data2的代码
	}
}
int main(int argc,char** argv){
	//initialization
	
}

这个也是VINS里面处理双目相机和IMU数据的方法，这样的方法一个很大的缺点是需要用一些进程锁和条件变量来保证内存访问，因为有一些全局变量例如传感器数据的队列可能同时会被多个进程访问，这个缺点在处理图片和IMU这类小体量数据的时候体现不出来，并且VINS也确实需要这样的队列来完成算法，在处理点云数据的时候，这个缺点对于程序的性能就有很大的影响。幸好ROS提供了message_filters::TimeSynchronizer等同步类来完成多传感器数据同步融合的任务。

message_filters::TimeSynchronizer

参照官文链接: ROS wiki

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/PointCloud2.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const PointCloud2ConstPtr& image, const PointCloud2ConstPtr& 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<PointCloud2> cloud1(nh, "/camera1/depth/color/points", 1);
  message_filters::Subscriber<PointCloud2> cloud2(nh, "/camera2/depth/color/points", 1);
  TimeSynchronizer<PointCloud2, PointCloud2> sync(cloud1, cloud2, 10);
  sync.registerCallback(boost::bind(&callback, _1, _2));//boost::bind的用法可以参考https://www.cnblogs.com/benxintuzi/p/4862129.html

  ros::spin();

  return 0;
}

注意：回调函数里面的参数一定是const MConstPtr& msg，不能是M& msg。比如点云数据一定是const PointCloud2ConstPtr& msg，别的像const PointCloud2Ptr& msg或者const PointCloud2& msg都不行，这个也是困扰了我一个晚上的小小细节。