<ROS> message_filters 对齐多种传感器数据的时间戳

联合标定三维雷达和IMU，第一步要先对齐两种传感信息的时间戳。

ros官网提供了message_filters用于对齐多种传感信息的时间戳。

http://wiki.ros.org/message_filters#Time_Synchronizer

注意，对齐传感信息时间戳有两种方式，一种是时间戳完全对齐 ExactTime Policy ，另一种是时间戳相近 

ApproximateTime Policy ，前者更为严格。

本人选用时间戳相近的对齐方法，代码如下：

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <ros/ros.h>
#include <iostream>

using namespace std;
using namespace sensor_msgs;
using namespace message_filters;

  PointCloud2 syn_pointcloud;

  Imu syn_imu;

void callback(const PointCloud2ConstPtr& ori_pointcloud, const ImuConstPtr& ori_imu)
{
  // Solve all of perception here...
  syn_pointcloud = *ori_pointcloud;
  syn_imu = *ori_imu;
  cout << "syn velodyne points' timestamp : " << syn_pointcloud.header.stamp << endl;
  cout << "syn Imu's timestamp : " << syn_imu.header.stamp << endl;
  ROS_INFO("pointcloud stamp value is: %d", syn_pointcloud.header.stamp);
  ROS_INFO("imu stamp value is: %d", syn_imu.header.stamp);
}


int main(int argc, char** argv)
{
  ros::init(argc, argv, "msg_synchronizer");
  ros::NodeHandle nh;

  message_filters::Subscriber<Imu> imu_sub(nh, "/imu/data", 1);
  message_filters::Subscriber<PointCloud2> velodyne_sub(nh, "/velodyne_points", 1);
  typedef sync_policies::ApproximateTime<PointCloud2, Imu> MySyncPolicy;    
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), velodyne_sub, imu_sub); //queue size=10
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();
  return 0;
}

callback函数订阅的所有/topic必须有数据存在，否则调用失败。

 

把上述代码写成一个类。

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <ros/ros.h>
#include <iostream>

using namespace std;

class mySynchronizer{
public:
    ros::Publisher pubVelodyne;
    ros::Publisher pubImu;

    mySynchronizer();
    ~mySynchronizer(){};

    void callback(const sensor_msgs::PointCloud2::ConstPtr& ori_pointcloud, const sensor_msgs::Imu::ConstPtr& ori_imu);
    
};

void mySynchronizer::callback(const sensor_msgs::PointCloud2::ConstPtr& ori_pointcloud, const sensor_msgs::Imu::ConstPtr& ori_imu){
    cout << "*******************" << endl;
    sensor_msgs::PointCloud2 syn_pointcloud = *ori_pointcloud;
    sensor_msgs::Imu syn_imu = *ori_imu;

    ROS_INFO("pointcloud stamp value is: %f", syn_pointcloud.header.stamp.toSec());
    ROS_INFO("imu stamp value is: %f", syn_imu.header.stamp.toSec());

    pubVelodyne.publish(syn_pointcloud);
    pubImu.publish(syn_imu);
}

mySynchronizer::mySynchronizer()
{
    ros::NodeHandle nh;
    pubVelodyne = nh.advertise<sensor_msgs::PointCloud2>("/Syn/imu/data", 1);
    pubImu = nh.advertise<sensor_msgs::Imu>("/Syn/velodyne_points", 1);

    message_filters::Subscriber<sensor_msgs::Imu> imu_sub(nh, "/imu/data", 1);
    message_filters::Subscriber<sensor_msgs::PointCloud2> velodyne_sub(nh, "/velodyne_points", 1);

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Imu> MySyncPolicy;
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), velodyne_sub, imu_sub);
    sync.registerCallback(boost::bind(&mySynchronizer::callback, this, _1, _2));

    ros::spin();
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "msg_synchronizer");
    ROS_INFO("\033[1;32m---->\033[0m Sync msgs node Started.");

    mySynchronizer wode;

    // ros::spin();

    return 0;
}

其中，非常注意2点：

A

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

由于callback函数是类内函数，所以要使用类内成员函数的写法；

加上this关键字；

 参考：https://blog.csdn.net/Felaim/article/details/78212738#commentBox

 

B

 ros::spin();  要放在订阅函数之后。如果放在主函数中，由于作用域的原因，会出现无法订阅/topic的错误。

参考：https://www.cnblogs.com/liu-fa/p/5925381.html

转载于:https://www.cnblogs.com/gdut-gordon/p/10293446.html