ROS中的message_filter中的时间同步

#ifndef MESSAGE_FILTERS_TIME_SYNCHRONIZER_H
#define MESSAGE_FILTERS_TIME_SYNCHRONIZER_H

#include <memory>

#include "message_filters/message_event.h"
#include "message_filters/synchronizer.h"
#include "message_filters/sync_policies/exact_time.h"

namespace message_filters
{

/**
 * \brief Synchronizes up to 9 messages by their timestamps.
 *
 * TimeSynchronizer synchronizes up to 9 incoming channels by the timestamps contained in their messages' headers.
 * TimeSynchronizer takes anywhere from 2 to 9 message types as template parameters, and passes them through to a
 * callback which takes a shared pointer of each.
 *
 * The required queue size parameter when constructing the TimeSynchronizer tells it how many sets of messages it should
 * store (by timestamp) while waiting for messages to arrive and complete their "set"
 *
 * \section connections CONNECTIONS
 *
 * The input connections for the TimeSynchronizer object is the same signature as for rclcpp subscription callbacks, ie.
\verbatim
void callback(const std::shared_ptr<M const>&);
\endverbatim
 * The output connection for the TimeSynchronizer object is dependent on the number of messages being synchronized.  For
 * a 3-message synchronizer for example, it would be:
\verbatim
void callback(const std::shared_ptr<M0 const>&, const std::shared_ptr<M1 const>&, const std::shared_ptr<M2 const>&);
\endverbatim
 * \section usage USAGE
 * Example usage would be:
\verbatim
TimeSynchronizer<sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image> sync_policies(caminfo_sub, limage_sub, rimage_sub, 3);
sync_policies.registerCallback(callback);
\endverbatim
 * The callback is then of the form:
\verbatim
void callback(const sensor_msgs::CameraInfo::ConstPtr&, const sensor_msgs::Image::ConstPtr&, const sensor_msgs::Image::ConstPtr&);
\endverbatim
 *
 */
template<class M0, class M1, class M2 = NullType, class M3 = NullType, class M4 = NullType,
         class M5 = NullType, class M6 = NullType, class M7 = NullType, class M8 = NullType>
class TimeSynchronizer : public Synchronizer<sync_policies::ExactTime<M0, M1, M2, M3, M4, M5, M6, M7, M8> >
{
public:
  typedef sync_policies::ExactTime<M0, M1, M2, M3, M4, M5, M6, M7, M8> Policy;
  typedef Synchronizer<Policy> Base;
  typedef std::shared_ptr<M0 const> M0ConstPtr;
  typedef std::shared_ptr<M1 const> M1ConstPtr;
  typedef std::shared_ptr<M2 const> M2ConstPtr;
  typedef std::shared_ptr<M3 const> M3ConstPtr;
  typedef std::shared_ptr<M4 const> M4ConstPtr;
  typedef std::shared_ptr<M5 const> M5ConstPtr;
  typedef std::shared_ptr<M6 const> M6ConstPtr;
  typedef std::shared_ptr<M7 const> M7ConstPtr;
  typedef std::shared_ptr<M8 const> M8ConstPtr;

  using Base::add;
  using Base::connectInput;
  using Base::registerCallback;
  using Base::setName;
  using Base::getName;
  using Policy::registerDropCallback;
  typedef typename Base::M0Event M0Event;
  typedef typename Base::M1Event M1Event;
  typedef typename Base::M2Event M2Event;
  typedef typename Base::M3Event M3Event;
  typedef typename Base::M4Event M4Event;
  typedef typename Base::M5Event M5Event;
  typedef typename Base::M6Event M6Event;
  typedef typename Base::M7Event M7Event;
  typedef typename Base::M8Event M8Event;

  template<class F0, class F1>
  TimeSynchronizer(F0& f0, F1& f1, uint32_t queue_size)
  : Base(Policy(queue_size))
  {
    connectInput(f0, f1);
  }

  template<class F0, class F1, class F2>
  TimeSynchronizer(F0& f0, F1& f1, F2& f2, uint32_t queue_size)
  : Base(Policy(queue_size))
  {
    connectInput(f0, f1, f2);
  }

  template<class F0, class F1, class F2, class F3>
  TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, uint32_t queue_size)
  : Base(Policy(queue_size))
  {
    connectInput(f0, f1, f2, f3);
  }

  template<class F0, class F1, class F2, class F3, class F4>
  TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, uint32_t queue_size)
  : Base(Policy(queue_size))
  {
    connectInput(f0, f1, f2, f3, f4);
  }

  template<class F0, class F1, class F2, class F3, class F4, class F5>
  TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, uint32_t queue_size)
  : Base(Policy(queue_size))
  {
    connectInput(f0, f1, f2, f3, f4, f5);
  }

  template<class F0, class F1, class F2, class F3, class F4, class F5, class F6>
  TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, uint32_t queue_size)
  : Base(Policy(queue_size))
  {
    connectInput(f0, f1, f2, f3, f4, f5, f6);
  }

  template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7>
  TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, uint32_t queue_size)
  : Base(Policy(queue_size))
  {
    connectInput(f0, f1, f2, f3, f4, f5, f6, f7);
  }

  template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7, class F8>
  TimeSynchronizer(F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, F8& f8, uint32_t queue_size)
  : Base(Policy(queue_size))
  {
    connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8);
  }

  TimeSynchronizer(uint32_t queue_size)
  : Base(Policy(queue_size))
  {
  }

  
  // For backwards compatibility
  
  void add0(const M0ConstPtr& msg)
  {
    this->template add<0>(M0Event(msg));
  }

  void add1(const M1ConstPtr& msg)
  {
    this->template add<1>(M1Event(msg));
  }

  void add2(const M2ConstPtr& msg)
  {
    this->template add<2>(M2Event(msg));
  }

  void add3(const M3ConstPtr& msg)
  {
    this->template add<3>(M3Event(msg));
  }

  void add4(const M4ConstPtr& msg)
  {
    this->template add<4>(M4Event(msg));
  }

  void add5(const M5ConstPtr& msg)
  {
    this->template add<5>(M5Event(msg));
  }

  void add6(const M6ConstPtr& msg)
  {
    this->template add<6>(M6Event(msg));
  }

  void add7(const M7ConstPtr& msg)
  {
    this->template add<7>(M7Event(msg));
  }

  void add8(const M8ConstPtr& msg)
  {
    this->template add<8>(M8Event(msg));
  }
};

}  // namespace message_filters

#endif  // MESSAGE_FILTERS__TIME_SYNCHRONIZER_H_

message_filters是ROS的一个实用程序库，集合了许多的常用的消息“过滤”算法。消息过滤器message_filters类似一个消息缓存，当消息到达消息过滤器的时候，可能并不会立即输出，而是在稍后的时间点里满足一定条件下输出（也就是上面代码中定义那一些列事件）。
举个例子，比如时间同步器（上面的代码就是时间同步过滤器），它接收来自多个源的不同类型的消息，并且仅当它们在具有相同时间戳的每个源上接收到消息时才输出它们，也就是起到了一个消息同步输出的效果，时间同步过滤器本身并不会修改各个数据源传过来的数据（也就是缓冲区中的数据），而是将各个缓冲区建立时间线，然后根据时间线上的各个时间点来判断是否需要输出。当这些时间线上都存在某个时间点的数据，则说明该时间点，各个数据源都有数据，这些数据自然是对齐的，然后就可以输出这一组对齐的数据，供用户应用使用。当然，这里存在严格同步和近似同步的策略。

// msg_filter.cpp

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <iostream>

std::string img_topic = "/fuhong/_img";
std::string point_topic = "/fuhong/_points";

//rosbag::Bag bag_record;//文件直接记录

ros::Publisher img_pub;
ros::Publisher pointcloud_pub;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> testSyncPolicy; //近似同步策略

//回调函数
void callback(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::PointCloud2ConstPtr &point)  //回调中包含多个消息
{
    //以实时发布的方式发布
    img_pub.publish(*image);
    pointcloud_pub.publish(*point);
}

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

    //发布的话题
    img_pub = n.advertise<sensor_msgs::Image>(img_topic, 1000);
    pointcloud_pub = n.advertise<sensor_msgs::PointCloud2>(point_topic, 1000);
    //订阅的话题
    message_filters::Subscriber<sensor_msgs::Image> img_sub(n, "/zed/zed_node/left/image_rect_color", 1);// topic1 输入
    message_filters::Subscriber<sensor_msgs::PointCloud2> pointcloud_sub(n, "/rslidar_points", 1);// topic2 输入

    message_filters::Synchronizer<testSyncPolicy> sync(testSyncPolicy(10), img_sub, pointcloud_sub);// 同步
    sync.registerCallback(boost::bind(&callback, _1, _2));

    ros::spin();
    ros::shutdown();

    return 0;
}

同步前的效果:

同步后的效果:

上述两个时间刻度不同，同步前的频率高，同步后的频率低，也就是说对于那些没有对齐的数据，进行同步处理后，并没有输出出来。最终pub出来的只是原始数据的子集。

参考：https://blog.csdn.net/hongge_smile/article/details/106324562?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromBaidu-3.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromBaidu-3.control