[ROS]message filters时间同步

１．message_filters介绍

message_filters用于对齐多种传感信息的时间戳，对齐时间戳有两种方式，一种是时间戳完全对齐 ：ExactTime Policy ，另一种是时间戳相近：ApproximateTime Policy

２．ｍessage_filters作用

1. 同时订阅并发布话题
2. 时间同步

３．message_filters实例

• 头文件

//ros头文件
#include 
//时间同步
#include 
#include 
#include 
//传感器消息
#include 
#include 
#include 

• subandpub.h

#ifndef __SUBANDPUB_H__
#define __SUBANDPUB_H__
//ros头文件
#include 
//时间同步
#include 
#include 
#include 
//传感器消息
#include 
#include 
#include 

class subscriberANDpublisher{
public:
    subscriberANDpublisher();
    void callback(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::PointCloud2ConstPtr &pointcloud);

private:
    ros::NodeHandle nh;
    ros::Publisher camera_pub;
    ros::Publisher lidar_pub;
    message_filters::Subscriber lidar_sub;//雷达订阅
    message_filters::Subscriber camera_sub;//相机订阅

    typedef message_filters::sync_policies::ApproximateTime syncpolicy;//时间戳对齐规则
    typedef message_filters::Synchronizer Sync;
    boost::shared_ptr sync_;//时间同步器

};
#endif

• subandpub.cpp

#include "SubandPub.h"

subscriberANDpublisher::subscriberANDpublisher()
{
    //订阅话题
    lidar_sub.subscribe(nh, "/rslidar_points", 1);
    camera_sub.subscribe(nh, "/zed/zed_node/left/image_rect_color", 1);

    //发布者
    camera_pub=nh.advertise("sync/img",1000);
    lidar_pub = nh.advertise("sync/lidar", 1000);

    //回调
    sync_.reset(new Sync(syncpolicy(10), camera_sub, lidar_sub));
    sync_->registerCallback(boost::bind(&subscriberANDpublisher::callback, this, _1, _2));
}
void subscriberANDpublisher::callback(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::PointCloud2ConstPtr &pointcloud) {
    ROS_INFO("done! ");
    camera_pub.publish(image);
    lidar_pub.publish(pointcloud);
}

• main.cpp

#include 
#include "SubandPub.h"

int main(int argc, char **argv) {
    ros::init(argc, argv, "node");
    subscriberANDpublisher sp;
    ROS_INFO("main done! ");
    ros::spin();

}

• cmakelist

• package.xml

４．运行结果

５．参考

链接：https://wiki.ros.org/message_filters