ros中的message filters时间同步--＞ 用于image和imu的结合_message_filters::sync_policies::approximatetime

转载自： https://blog.csdn.net/He3he3he/article/details/109643391
１．message_filters介绍
message_filters用于对齐多种传感信息的时间戳，对齐时间戳有两种方式，一种是时间戳完全对齐 ：ExactTime Policy ，另一种是时间戳相近：ApproximateTime Policy

２．ｍessage_filters作用
同时订阅并发布话题
时间同步

３．message_filters实例
头文件

//ros头文件
#include <ros/ros.h>
//时间同步
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
//传感器消息
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud2.h>
subandpub.h

#ifndef __SUBANDPUB_H__
#define __SUBANDPUB_H__
//ros头文件
#include <ros/ros.h>
//时间同步
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
//传感器消息
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud2.h>
 
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<sensor_msgs::PointCloud2> lidar_sub;//雷达订阅
    message_filters::Subscriber<sensor_msgs::Image> camera_sub;//相机订阅
 
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> syncpolicy;//时间戳对齐规则
    typedef message_filters::Synchronizer<syncpolicy> Sync;
    boost::shared_ptr<Sync> sync_;//时间同步器
 
};
#endif
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43

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<sensor_msgs::Image>("sync/img",1000);
    lidar_pub = nh.advertise<sensor_msgs::PointCloud2>("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);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

main.cpp

#include <ros/ros.h>
#include "SubandPub.h"
 
int main(int argc, char **argv) {
    ros::init(argc, argv, "node");
    subscriberANDpublisher sp;
    ROS_INFO("main done! ");
    ros::spin();
 
}
1
2
3
4
5
6
7
8
9
10

cmakelist

package.xml

５．参考
链接：https://wiki.ros.org/message_filters