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

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

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

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

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

ApproximateTime Policy ，前者更为严格。

 

msg_syn.h

#ifndef CLIMBING_ROBOT_GT_MAP_
#define CLIMBING_ROBOT_GT_MAP_

#include <string>

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

namespace climbing_robot
{


class MsgSynchronizer{
public:
    MsgSynchronizer(ros::NodeHandle node);
    ~MsgSynchronizer(){};

    void callback(const sensor_msgs::PointCloud2::ConstPtr& ori_pointcloud, const geometry_msgs::PoseStamped::ConstPtr& ori_pose);
    
    inline void CloseBag()
    {
        msg_syn_bag.close();
    }

private:
    // ros::Publisher pubVelodyne;
    // ros::Publisher pubImu;
    std::string syn_bag_path;
    rosbag::Bag msg_syn_bag;
};



}//namespace climbing_robot


#endif// CLIMBING_ROBOT_GT_MAP_

 

msg_syn.cpp

#include "msg_syn.h"

#include <iostream>

namespace climbing_robot
{


MsgSynchronizer::MsgSynchronizer(ros::NodeHandle node)
{
    // pubVelodyne = nh.advertise<sensor_msgs::PointCloud2>("/Syn/imu/data", 1);

    message_filters::Subscriber<geometry_msgs::PoseStamped> pose_sub(node, "/vrpn_client_node/RigidBody/pose", 1);
    message_filters::Subscriber<sensor_msgs::PointCloud2> velodyne_sub(node, "/velodyne_points", 1);

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, geometry_msgs::PoseStamped> approximate_policy;
    message_filters::Synchronizer<approximate_policy> sync(approximate_policy(10), velodyne_sub, pose_sub);
    sync.registerCallback(boost::bind(&MsgSynchronizer::callback, this, _1, _2));


    node.getParam("msg_syn_bag_path", syn_bag_path);
    msg_syn_bag.open(syn_bag_path, rosbag::bagmode::Write);


    ros::spin();
}


void MsgSynchronizer::callback(const sensor_msgs::PointCloud2::ConstPtr& ori_pointcloud, const geometry_msgs::PoseStamped::ConstPtr& ori_pose)
{
    ROS_INFO("pointcloud stamp value is: %f", ori_pointcloud->header.stamp.toSec());
    ROS_INFO("pose stamp value is: %f", ori_pose->header.stamp.toSec());
   
    // ros::Time timestamp = ori_pointcloud.header.stamp;
    msg_syn_bag.write("/velodyne_points", ori_pointcloud->header.stamp, *ori_pointcloud);
    msg_syn_bag.write("/vrpn_client_node/RigidBody/pose", ori_pointcloud->header.stamp, *ori_pose);

    // pubVelodyne.publish(syn_pointcloud);
}

}//namespace climbing_robot

 

main.cpp

#include "msg_syn.h"


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

    climbing_robot::MsgSynchronizer synchronizer(node);

    // ros::spin();

    synchronizer.CloseBag();

    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