ROS多消息同步（如LiDAR点云与IMU数据）

ROS的message_filters库提供了一种机制来处理来自一个或多个主题的消息，尤其是当这些消息需要以某种方式同步或以某种特定顺序处理时。例如LiDAR点云与IMU数据之间的同步。

以下是一些主要的message_filters功能：

• Time Synchronizer: message_filters::TimeSynchronizer允许你同步两个或多个主题的消息，这些消息的时间戳必须完全相同。当所有主题都有一个新消息时，就会触发一个回调。

• Approximate Time Policy: message_filters::sync_policies::ApproximateTime是一个更灵活的同步策略，它允许消息的时间戳有小的误差。这对于处理那些不能精确同步的设备（例如，不同的传感器可能有微小的时间差）非常有用。

• Message Filters: message_filters还提供了一些过滤器，例如message_filters::Subscriber（订阅一个主题的消息）、message_filters::Cache（缓存一段时间内的消息）和message_filters::TimeSequencer（确保消息按时间顺序处理）。

• Chain: message_filters::Chain允许你将多个过滤器链接在一起，创建一个处理消息的管道。

使用message_filters，你可以创建复杂的消息处理逻辑，而无需手动处理所有的细节。例如，你可以创建一个系统，该系统等待从多个传感器接收数据，然后当所有数据都准备就绪时，同步处理这些数据。

接下来给出一个使用案例，进行同步LiDAR点云与IMU。

#include <iostream>
#include <fstream>
#include <vector>
#include <csignal>  

#include "global_relocation/global_relocation.h"

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>

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


void messageCallback(const sensor_msgs::PointCloud2ConstPtr& pointMsgIn, const sensor_msgs::ImuConstPtr& ImuMsgIn)
{
    std::cout << "hi!--Lidar: " << std::setprecision(16) << pointMsgIn->header.stamp.toSec() <<
    " IMU: " << ImuMsgIn->header.stamp.toSec() << std::endl;
}

int main(int argc, char ** argv)
{
    ros::init(argc, argv, "global_relocation");
	ros::NodeHandle nh;
    // sync the time by ros message filter
    message_filters::Subscriber<sensor_msgs::PointCloud2> subPointCloud(nh, "/os_cloud_node_1/points", 1);
    message_filters::Subscriber<sensor_msgs::Imu> subIMU(nh, "/os_cloud_node_1/imu", 1);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Imu> syncPolicy;
    typedef message_filters::Synchronizer<syncPolicy> Sync;
    message_filters::Synchronizer<syncPolicy> sync(syncPolicy(100), subPointCloud, subIMU);
    sync.registerCallback(boost::bind(&messageCallback, _1, _2));

    ROS_INFO("hello ros!");

    ros::spin();
    return 0;
}

注意：如果两个topic的频率相差很大时，需要修改syncPolicy(100)的值，比如说两个点云数据的topic都是10hz，那么我们可以设置为syncPolicy(10)，但是针对10Hz的点云与100Hz的IMU数据，我们需要设置为syncPolicy(100)。

如果队列大小设置得太小，那么可能会因为消息没有足够的机会找到匹配的对而被丢弃。如果设置得太大，那么可能会消耗过多的内存。所以，选择合适的队列大小需要根据应用和硬件进行权衡。