ROS subscription to multiple topics time synchronization problem

message_filters的介绍：

message_filters是一个用于roscpp和rospy的实用程序库. 它集合了许多的常用的消息“过滤”算法.消息过滤器message_filters类似一个消息缓存,当消息到达消息过滤器的时候,可能并不会立即输出,而是在稍后的时间点里满足一定条件下输出.
举个例子,比如时间同步器,它接收来自多个源的不同类型的消息,并且仅当它们在具有相同时间戳的每个源上接收到消息时才输出它们,也就是起到了一个消息同步输出的效果.

Time Synchronizer 时间同步器：

C++ message_filters::TimeSynchronizer API docs
Python message_filters.TimeSynchronizer
TimeSynchronizerfilter by including in itsThe timestamp in the header to synchronize the input channels,并以单个回调的形式输出它们需要相同数量的通道. C ++实现可以同步最多9个通道.

例子（C++）：

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
{
    
  // Solve all of perception here...
}

int main(int argc, char** argv)
{
    
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;

  message_filters::Subscriber<Image> image_sub(nh, "image", 1);
  message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);
  TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}

Policy-Based Synchronizer 基于策略的同步器 ：

Synchronizer filter同步filter by including in itsThe timestamp in the header to synchronize the input channels,并以单个回调的形式输出它们需要相同数量的通道. C ++实现可以同步最多9个通道.

Synchronizer过滤器在确定如何同步通道的策略上进行模板化. 目前有两个策略：ExactTime和ApproximateTime.

C++ Header: message_filters/synchronizer.h
例1（ExactTime）：

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)
{
    
  // Solve all of perception here...
}

int main(int argc, char** argv)
{
    
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;
  message_filters::Subscriber<Image> image_sub(nh, "image", 1);
  message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);

  typedef sync_policies::ExactTime<Image, CameraInfo> MySyncPolicy;
  // ExactTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, info_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}

例2（ApproximateTime）：

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

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image1, const ImageConstPtr& image2)
{
    
  // Solve all of perception here...
}

int main(int argc, char** argv)
{
    
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;
  message_filters::Subscriber<Image> image1_sub(nh, "image1", 1);
  message_filters::Subscriber<Image> image2_sub(nh, "image2", 1);

  typedef sync_policies::ApproximateTime<Image, Image> MySyncPolicy;
  // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image1_sub, image2_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}

注意：The premise of realizing the above three kinds of time synchronization is that there is a header containing the timestamp in the message body：

std_msg/Header

uint32 seq //存储原始数据类型uint32
time stamp //存储ROS中的时间戳信息
string frame_id //用于表示和此数据关联的帧,在坐标系变化中可以理解为数据所在的坐标系名称